#include <math.h>
#include "timer.h"

#define NUMBALLS 18
#define WHITEBALL 0
#define BALLRADIUS 5
#define TABLEWIDTH 100.0f
#define TABLELENGTH 200.0f
#define TABLEBOUNCE 0.8f
#define TABLEDRAG 0.995f
#define MINSPEED 0.05f

typedef struct {float x, y, z, w;} Vector_t;
short NumberOfBalls = 0;
short NumberOfMovingBalls = 0;
Vector_t BallPositions [NUMBALLS];
Vector_t BallVelocities [NUMBALLS];
short MoveballsReps = 16;
int framecounter = 0;

void SetupBalls (void)
{
    int i, x, y, w;

    BallPositions[WHITEBALL].x = 0;
    BallPositions[WHITEBALL].y = 0;
    BallPositions[WHITEBALL].z = -BALLRADIUS*10;

    x = 0; y = 0; w = 1;
    for (i=1; i<NUMBALLS; i++) {
        BallPositions[i].x = (x * 2 - w + 1) * BALLRADIUS;
        BallPositions[i].y = 0;
        BallPositions[i].z = (y * 2) * BALLRADIUS;
        x++;
        if (x >= w) {
            x = 0; w++; y++;
        }
    }

    for (i=0; i<NUMBALLS; i++) {
        BallVelocities[i].x = 0;
        BallVelocities[i].y = 0;
        BallVelocities[i].z = 0;
        BallVelocities[i].w = 0;
    }
    BallVelocities[WHITEBALL].z = 5.0f;
}


void MoveBall ()
{

#define NumberOfBalls j
#define Position BallPositions[j]
#define Velocity BallVelocities[j]

    short i, j, k, NumberOfCollisions = 0;
    Vector_t CollisionVector, v, PushDir;
    float d, Distance, CollisionSpeed;

	for (k=0; k < MoveballsReps; k++)

    for (j=0; j<NUMBALLS; j++) {

		CollisionVector.x = 0;
		CollisionVector.y = 0;
		CollisionVector.z = 0;
		CollisionVector.w = 0;

		Position.x += Velocity.x;
		Position.y += Velocity.y;
		Position.z += Velocity.z;

		if (Position.x >= TABLEWIDTH/2-BALLRADIUS) {
			Velocity.x = -Velocity.x * TABLEBOUNCE;
			Position.x = TABLEWIDTH/2-BALLRADIUS;
		}
		if (Position.x <= -TABLEWIDTH/2+BALLRADIUS) {
			Velocity.x = -Velocity.x * TABLEBOUNCE;
			Position.x = -TABLEWIDTH/2+BALLRADIUS;
		}
		if (Position.z >= TABLELENGTH/2-BALLRADIUS) {
			Velocity.z = -Velocity.z * TABLEBOUNCE;
			Position.z = TABLELENGTH/2-BALLRADIUS;
		}
		if (Position.z <= -TABLELENGTH/2+BALLRADIUS) {
			Velocity.z = -Velocity.z * TABLEBOUNCE;
			Position.z = -TABLELENGTH/2+BALLRADIUS;
		}

		for (i=0; i<NumberOfBalls; i++) {
			v.x = Position.x - BallPositions [i].x;
			v.y = Position.y - BallPositions [i].y;
			v.z = Position.z - BallPositions [i].z;
			d = v.x * v.x + v.y * v.y + v.z * v.z;
			if (d < BALLRADIUS * BALLRADIUS * 4) {
				/* collision */
				Distance = sqrt (d);
				v.x = v.x / Distance;
				v.y = v.y / Distance;
				v.z = v.z / Distance;
				Distance = BALLRADIUS * 2 - Distance;
				PushDir = v;
				NumberOfCollisions++;

				CollisionSpeed = (Velocity.x - BallVelocities [i].x) * v.x
							 + (Velocity.y - BallVelocities [i].y) * v.y
							 + (Velocity.z - BallVelocities [i].z) * v.z;
				if (CollisionSpeed < 0) {
					BallVelocities [i].x += v.x * CollisionSpeed;
					BallVelocities [i].y += v.y * CollisionSpeed;
					BallVelocities [i].z += v.z * CollisionSpeed;
					Velocity.x -= v.x * CollisionSpeed;
					Velocity.y -= v.y * CollisionSpeed;
					Velocity.z -= v.z * CollisionSpeed;
				}
			}
		}

		if (NumberOfCollisions) {
			Position.x += PushDir.x * Distance;
			Position.y += PushDir.y * Distance;
			Position.z += PushDir.z * Distance;
		}

		Velocity.x = Velocity.x * TABLEDRAG;
		Velocity.y = Velocity.y * TABLEDRAG;
		Velocity.z = Velocity.z * TABLEDRAG;

		if (Velocity.x < -MINSPEED || Velocity.x > MINSPEED
		 || Velocity.y < -MINSPEED || Velocity.y > MINSPEED
		 || Velocity.z < -MINSPEED || Velocity.z > MINSPEED) {
			NumberOfMovingBalls++;
		} else {
			/* stopped */
			Velocity.x = 0;
			Velocity.y = 0;
			Velocity.z = 0;
		}

	}

#undef Position
#undef Velocity
#undef NumberOfBalls

}


void MoveBalls ( ) {
	
	framecounter++;

    NumberOfBalls = 0;
    NumberOfMovingBalls = 0;


	MoveBall ( );

    if ( NumberOfMovingBalls == 0 ) {
        /* If no balls are still moving, then hit the cue-ball again */
        BallVelocities[WHITEBALL].x = 10.0f * sin (framecounter);
        BallVelocities[WHITEBALL].z = 10.0f * cos (framecounter);
    }
}

int main () {
	unsigned a;

    SetupBalls ( );

	START_TIMER();

	for ( a = 0; a < 2000; a++ )
		MoveBalls ( );

	STOP_TIMER ( );	
	return 0;
}