/***************************************************************************
 *            physics.c
 *
 *  Thu Jan 27 15:45:22 2005
 *  Copyright  2005  Peter Baldwin
 *  pgbaldwi@uwaterloo.ca
 ****************************************************************************/

#include <stdlib.h>
 #include <math.h>
#include "physics.h"

void pBounce(pObject *a, pObject *b, vector normal);


bool pCollisionCircCirc(pCircle *a, pCircle *b) {
	const double ra = a->radius;
	const double rb = b->radius;
	const double mindist = ra + rb;
	const vector sa = a->o->pos;
	const vector sb = b->o->pos;
	const vector d = vSubtract(sb, sa);
    const double d_squared = vDot(d,d);

	if (d_squared < mindist * mindist) {
		double dist = vMagnitude(d);
		vector normal = vUnit(d);

		//separate objects
		double collision_depth = mindist - dist;
		a->o->pos = vSubtract(sa, vScale(normal,collision_depth));
		b->o->pos = vAdd(sb, vScale(normal,collision_depth));
		
		pBounce(a->o,b->o,normal);
		return TRUE;
	} else {
		return FALSE;
	}
}

bool pCollisionCircRect(pCircle *a, pRectangle *b) {
	
	const double ma = a->o->mass;
	const double mb = b->o->mass;
	const double w = b->w;
	const double h = b->h;
	const double top = b->o->pos.y - h / 2;
	const double bottom = b->o->pos.y + h / 2;
	const double left = b->o->pos.x - w / 2;
	const double right = b->o->pos.x + w / 2;

	const double x = a->o->pos.x;
	const double y = a->o->pos.y;
	const double r = a->radius;
	vector normal = VZERO;
	bool collision = FALSE;

	if (x > left && x < right && y > top && y < bottom) {
		//case: centre of circle is inside rectangle
		//ideally, this case shouldn't happen if the physics are
		//being calculated incrementally on small intervals.
		//however, if the object is travelling faster than anticipated
		//this case may occur.
		//note that this does not resolve the case of the objects passing through
		//eachother, which may occur under unusual circumstances
		//TODO
		//current implementation: always bounce vertically
	}
	
	if (x > left && x < right) {
		//top and bottom walls
		if ((y < top && top - y < r) || (y > top && y < top + h / 2)) {
			//circle above rectangle
			if (ma > mb) { //circle is heavier
				b->o->pos.y+=r-(top-y);
			} else { //rect is heavier
				a->o->pos.y-=r-(top-y);
			}
			normal  =  vNew(0, -1);
			a->o->pos = vNew(x, top - r);
			collision = TRUE;

		} else if ((y > bottom && y - bottom < r) || (y < bottom
				&& y > bottom - h / 2)) {
			//circle below rectangle
			if (ma > mb) { //circle is heavier
				b->o->pos.y-=r-(y-bottom);
			} else { //rect is heavier
				a->o->pos.y+=r-(y-bottom);
			}
			normal = vNew(0, 1);
			a->o->pos = vNew(x, bottom + r);
			collision = TRUE;
		}
	} else if (y > top && y < bottom) {
		//left and right walls
		if ((x < left && left - x < r) || (x > left && x < left + w / 2)) {
			//circle left of rectangle
			if (ma > mb) { //circle is heavier
				b->o->pos.x+=r-(left-x);
			} else { //rect is heavier
				a->o->pos.x-=r-(left-x);
			}
			normal = vNew(-1, 0);
			a->o->pos = vNew(left - r, y);
			collision = TRUE;
		} else if ((x > right && x - right < r) || (x < right
				&& x > right - w / 2)) {
			//circle right of rectangle
			if (ma > mb) { //circle is heavier
				b->o->pos.x-=r-(x - right);
			} else { //rect is heavier
				a->o->pos.x+=r-(x - right);
			}
			normal = vNew(1, 0);
			a->o->pos = vNew(right + r, y);
			collision = TRUE;
		}
	} else {
		int i;
		//corners
		vector corners[] = { vNew(left, top),
				vNew(right, top), vNew(left, bottom),
				vNew(right, bottom) };
		for (i = 0; i < 4; i++) {
			vector d = vSubtract(a->o->pos, corners[i]);
			if (vMagnitude(d) < r) {
				normal = vUnit(d);
				if (ma > mb) { //circle is heavier
					b->o->pos = vAdd(a->o->pos, (vScale(normal,r)));
				} else { //rect is heavier
					a->o->pos = vSubtract(corners[i], (vScale(normal,r)));
				}
				a->o->pos = vAdd(corners[i],vScale(normal,r));
				collision = TRUE;
				break;
			}
		}
	}
	
	if (collision) {
		pBounce(a->o,b->o,normal); 
	}
	
	return collision;
}

//cause the objects a and b to bounce away from eachother

void pBounce(pObject *a, pObject *b, vector normal) {
	//get some basic information about the objects for quick reference
	const vector va = a->vel;
	const vector vb = b->vel;
	const double ma = a->mass;
	const double mb = b->mass;
	
	//find components of velocities
	const vector va_normal = vScale(normal, vDot(va,normal));
	const vector va_perpendicular = vSubtract(va,va_normal);
	const vector vb_normal = vScale(normal,vDot(vb,normal));
	const vector vb_perpendicular = vSubtract(vb,vb_normal);
	
	if (mb == HUGE_VAL) { //infinite mass
		a->vel = vScaleNormal(vAdd(vScale(va_normal,-1),va_perpendicular),normal,1.0-a->uimpact);
	} else if (ma == HUGE_VAL) { //infinite mass
		b->vel = vScaleNormal(vAdd(vScale(vb_normal,-1),vb_perpendicular),normal,1.0-a->uimpact);
	} else { //normal collision
		//find the relative velocity along the normal
		const vector vr_normal = vSubtract(va_normal,vb_normal);
		
		const vector var_final = vAdd(vScale(vr_normal, (ma - mb) / (ma + mb)),
				va_perpendicular);
		const vector vbr_final = vAdd(vScale(vr_normal,2 * ma / (ma + mb)),
				vb_perpendicular);
		
		//make relative velocities absolute and subtract lost energy
		a->vel = vScaleNormal(vAdd(var_final,vb_normal),normal,1.0-a->uimpact);
		b->vel = vScaleNormal(vAdd(vbr_final,vb_normal),normal,1.0-b->uimpact);
	}
}
bool pCollisionRectRect(pRectangle *a, pRectangle *b) {
	const double ax = a->o->pos.x;
	const double ay = a->o->pos.y;
	const double bx = b->o->pos.x;
	const double by = b->o->pos.y;
	const double dx = ax - bx;
	const double dy = ay - by;
	const double mindx = a->w/2 + b->w/2;
	const double mindy = a->h/2 + b->h/2;
	const double adx = fabs(dx);
	const double ady = fabs(dy);
	if (adx < mindx && ady < mindy) {
		const double xdepth = mindx - adx;
		const double ydepth = mindy - ady;
		vector normal;
		if (xdepth < ydepth) { //most likely a horizontal collision
			if (ax > bx) { // a to right of b
				//separate
				if (a->o->mass > b->o->mass) { //move b
					b->o->pos.x -= xdepth;
				} else { //move a
					a->o->pos.x += xdepth;
				}
				normal = vNew(-1,0);
			} else { //a to left of b
				//separate
				if (a->o->mass > b->o->mass) { //move b
					b->o->pos.x += xdepth;
				} else { //move a
					a->o->pos.x -= xdepth;
				}
				normal = vNew(+1,0);
			}
		} else { //most likely a vertical collision
			if (ay < by) { // a above b
				//separate
				//if (a->o->mass > b->o->mass) { //move b
				//	b->o->pos.y += ydepth;
				//} else { //move a
					a->o->pos.y -= ydepth;
				//}
				normal = vNew(0,+1);
			} else { //a below b
				//separate
				//if (a->o->mass > b->o->mass) { //move b
					b->o->pos.y -= ydepth;
				//} else { //move a
				//	a->o->pos.y += ydepth;
				//}
				normal = vNew(0,-1);
			}
		}
		pBounce(a->o,b->o,normal);
		return TRUE;
	} else {
		return FALSE;
	}
}

void pMove(pObject *obj, vector fnet, double t) {
	vector a = vScale(fnet, 1/obj->mass); //get acceleration
	obj->vel = vAdd(obj->vel, vScale(a,t)); //calc new velocity
	obj->pos = vAdd(obj->pos, vScale(obj->vel,t)); //calc new position
}
