#include <iostream>
#include <math.h>       /* for cos(), sin(), and sqrt() */
#include <stdlib.h>	/* for drand48() */
#include <string.h>
#include <time.h>
#include <unistd.h>

static unsigned int Dims  = 32;
static unsigned int Gnats = 7;
static double MaxVelocity     = 30.0;
static double MaxAcceleration =  10.0;
static double AccelFactor = 1.0;
static double PointDuration = 1.5;

static double* zero;

static double* GnatMaxVelocity;
static double* GnatMaxAcceleration;

static double* wp;
static double* wv;
static double* wa;
static double* wc;
static double** gp;
static double** gv;
static double** ga;

static double
randomCoord( void )
{
    return drand48() * 15.0 - 7.5;
}

static double
randomVelocity( void )
{
    return drand48() * 6.0 - 3.0;
}

static double
randomAcceleration( void )
{
    return drand48() * 3.0 - 1.5;
}

static void
resetCoefs( double* buf )
{
    for ( unsigned int jj=0; jj < Dims; ++jj ) {
	buf[ jj ] = 0.0;
    }
}

static void
addCoefs( const double* coef, const double* rel, double* buf )
{
    for ( unsigned int jj=0; jj < Dims; ++jj ) {
	buf[ jj ] += ( coef[jj] - rel[jj] );
    }
}


static void
emitCoefs( double* buf, double gain, bool doAbs )
{
    for ( unsigned int jj=0; jj < Dims; ++jj ) {
	int index = (int)( gain * buf[ jj ]  );

	if ( doAbs ) {
	    if ( index < 0 ) {
		index = -index;
	    }
	} else if ( index < 0 ) {
	    index = 0;
	}

	if ( index > 255 ) {
	    index = 255;
	}

	cout << (unsigned char)index;
    }
}

/* Advance time varying state when idle callback registered. */
static void
idle( double elapsed )
{
    static double nextNewPoint = PointDuration;
    static double curTime = 0.0;
    double mag = 0.0;

    curTime += elapsed;

    for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	mag = 0.0;

	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    gv[ kk ][ ii ] += ga[ kk ][ ii ] * elapsed;
	    mag += gv[ kk ][ ii ] * gv[ kk ][ ii ];
	}

	if ( mag > GnatMaxVelocity[ kk ] * GnatMaxVelocity[ kk ] ) {
	    double factor = GnatMaxVelocity[ kk ] / sqrt( mag );
	    for ( unsigned int ii=0; ii < Dims; ++ii ) {
		gv[ kk ][ ii ] *= factor;
	    }
	}

	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    gp[ kk ][ ii ] += gv[ kk ][ ii ] * elapsed;
	}
    }

    mag = 0.0;

    for ( unsigned int ii=0; ii < Dims; ++ii ) {
	wv[ ii ] += wa[ ii ] * elapsed;
	mag += wv[ ii ] * wv[ ii ];
    }

    if ( mag > MaxVelocity * MaxVelocity ) {
	double factor = MaxVelocity / sqrt( mag );
	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    wv[ ii ] *= factor;
	}
    }

    mag = 0.0;

    for ( unsigned int ii=0; ii < Dims; ++ii ) {
	wp[ ii ] += wv[ ii ] * elapsed;

	double diff = ( wp[ ii ] - wc[ ii ] );
	mag += diff * diff;
    }


    if ( mag < 100.0 || curTime >= nextNewPoint ) {
	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    wc[ ii ] = randomCoord() / 2.0;
	}
	nextNewPoint = curTime + PointDuration;
    }

    mag = 0.0;

    for ( unsigned int ii=0; ii < Dims; ++ii ) {
	wa[ ii ] = AccelFactor * ( wc[ ii ] - wp[ ii ] );
	mag += wa[ ii ] * wa[ ii ];
    }

    if ( mag > MaxAcceleration * MaxAcceleration ) {
	double factor = MaxAcceleration / sqrt( mag );
	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    wa[ ii ] *= factor;
	}
    }

    for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	const double* tp = &wp[ 0 ];

	if ( kk > 0 && (lrand48()&3) == 0 ) {
	    tp = &ga[ kk-1 ][ 0 ];
	}

	mag = 0.0;

	for ( unsigned int ii=0; ii < Dims; ++ii ) {
	    ga[ kk ][ ii ] = AccelFactor * 1.5 * drand48()
		* ( tp[ ii ] - gp[ kk ][ ii ] );
	    mag += ga[ kk ][ ii ] * ga[ kk ][ ii ];
	}

	if ( mag > GnatMaxAcceleration[ kk ] * GnatMaxAcceleration[ kk ] ) {
	    double factor = GnatMaxAcceleration[ kk ] / sqrt( mag );
	    for ( unsigned int ii=0; ii < Dims; ++ii ) {
		ga[ kk ][ ii ] *= factor;
	    }
	}
    }
}

int
main(int argc, char **argv)
{
    double linesPerSecond = 32.0;
    double gain = 10.0;
    double* buf;
    unsigned int lines = 0;
    time_t timeSeed = time( 0 );
    pid_t pidSeed = getpid();
    unsigned short seed[3];
    bool doAbs = false;
    bool help = false;

    seed[ 0 ] = (unsigned short)( pidSeed & 0x0000FFFF );
    seed[ 1 ] = (unsigned short)( ( timeSeed >>  0 ) & 0x0000FFFF );
    seed[ 2 ] = (unsigned short)( ( timeSeed >> 16 ) & 0x0000FFFF );

    for ( unsigned int ii=1; ii < argc; ++ii ) {
	if ( !strcmp( argv[ ii ], "-lines" ) ) {
	    lines = strtol( argv[ ++ii ], 0, 0 );
	} else if ( !strcmp( argv[ ii ], "-dims" ) ) {
	    Dims = strtol( argv[ ++ii ], 0, 0 );
	} else if ( !strcmp( argv[ ii ], "-gnats" ) ) {
	    Gnats = strtol( argv[ ++ii ], 0, 0 );
	} else if ( !strcmp( argv[ ii ], "-mv" ) ) {
	    MaxVelocity = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-ma" ) ) {
	    MaxAcceleration = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-af" ) ) {
	    AccelFactor = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-pd" ) ) {
	    PointDuration = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-lps" ) ) {
	    linesPerSecond = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-gain" ) ) {
	    gain = strtod( argv[ ++ii ], 0 );
	} else if ( !strcmp( argv[ ii ], "-seed" ) ) {
	    seed[0] = strtol( argv[ ++ii ], 0, 16 );
	    seed[1] = strtol( argv[ ++ii ], 0, 16 );
	    seed[2] = strtol( argv[ ++ii ], 0, 16 );
	} else if ( !strcmp( argv[ ii ], "-abs" ) ) {
	    doAbs = true;
	} else if ( !strcmp( argv[ ii ], "-help" ) ) {
	    help = true;
	} else {
	    help = true;
	}
    }

    if ( help ) {
	cerr << "Usage: " << argv[0] << endl;
	cerr << "\t[-lines lines]" << endl;
	cerr << "\t[-dims dimensions]" << endl;
	cerr << "\t[-gnats gnats]" << endl;
	cerr << "\t[-mv maxVelocity]" << endl;
	cerr << "\t[-ma maxAcceleration]" << endl;
	cerr << "\t[-af accelerationFactor]" << endl;
	cerr << "\t[-pd pointDuration]" << endl;
	cerr << "\t[-lps linesPerSecond]" << endl;
	cerr << "\t[-gain gain]" << endl;
	cerr << "\t[-seed 0x#### 0x#### 0x####]" << endl;
	cerr << "\t[-abs]" << endl;
	cerr << "\t[-help]" << endl;
	return __LINE__;
    }

    seed48( seed );

    gain /= (double)( Gnats + 1 );


    GnatMaxVelocity = new double[ Gnats ];
    GnatMaxAcceleration = new double[ Gnats ];

    buf = new double[ Dims ];

    wp = new double[ Dims ];
    wv = new double[ Dims ];
    wa = new double[ Dims ];
    wc = new double[ Dims ];

    zero = new double[ Dims ];

    gp = new double*[ Gnats ];
    gv = new double*[ Gnats ];
    ga = new double*[ Gnats ];

    for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	gp[ kk ] = new double[ Dims ];
	gv[ kk ] = new double[ Dims ];
	ga[ kk ] = new double[ Dims ];
    }

    for ( unsigned int ii=0; ii < Dims; ++ii ) {
	wc[ ii ] = randomCoord() / 2.0;
	wp[ ii ] = randomCoord();
	wv[ ii ] = randomVelocity();
	wa[ ii ] = randomAcceleration();
	zero[ ii ] = 0.0;
	for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	    gp[ kk ][ ii ] = randomCoord();
	    gv[ kk ][ ii ] = randomVelocity();
	    ga[ kk ][ ii ] = randomAcceleration();
	}
    }

    for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	GnatMaxVelocity[ kk ] = 29.0 + randomVelocity();
	GnatMaxAcceleration[ kk ] = 30.0 + randomAcceleration();
    }

    cout << ( Dims ) << endl;

    for ( unsigned int ii=0; ( lines == 0 ) || ii < lines; ++ii ) {
	resetCoefs( buf );

	addCoefs( &wp[ 0 ], &zero[ 0 ], buf );

	for ( unsigned int kk=0; kk < Gnats; ++kk ) {
	    addCoefs( &gp[ kk ][ 0 ], &zero[ 0 ], buf );
	}

	emitCoefs( buf, gain, doAbs );

	idle( 1.0 / linesPerSecond );
    }

    return 0;
}