#include "private.h"


typedef struct _HeliCollisionData {
	int flip;
	double dirx, diry;
	double radius;
	double x1, y1, x2, y2;
	double lmin, lmax;
	double nxMin, nyMin;
	double nxMax, nyMax;
	int found;
} HeliCollisionData;

static void applyPoint(HeliCollisionData *data, double l, double nx, double ny) {
	if (data->flip) {
		l = -l;
		nx = -nx;
		ny = -ny;
	}
	if (!data->found || l < data->lmin) {
		data->lmin = l;
		data->nxMin = nx;
		data->nyMin = ny;
	}
	if (!data->found || l > data->lmax) {
		data->lmax = l;
		data->nxMax = nx;
		data->nyMax = ny;
	}
	data->found = TRUE;
}


static void collision(HeliCollisionData *data) {
	double A = data->x2 - data->x1;
	double B = data->y2 - data->y1;
	double AB2 = A*A + B*B;
	if (AB2 <= HELI_PRECISION_SQR) {
		// circle with circle
		if (fabs(data->radius) > HELI_PRECISION) {
			A = data->dirx*data->dirx + data->diry*data->diry;
			if (A > HELI_PRECISION_SQR) {
				B = -2*(data->dirx * data->x1 + data->diry * data->y1);
				double C = data->x1*data->x1 + data->y1*data->y1 - data->radius*data->radius;
				double D = B*B - 4*A*C;

				double l[2];
				int count = 0;
				if (fabs(D) <= HELI_PRECISION_SQR*HELI_PRECISION_SQR) {
					count = 1;
					l[0] = -0.5*B/A;
				} else
				if (D > 0) {
					count = 2;
					double d = sqrt(D);
					double ka = 0.5/A;
					l[0] = (-B - d)*ka;
					l[1] = (-B + d)*ka;
				}
				
				double kr = 1/data->radius;
				for(int i = 0; i < count; ++i) {
					double nx = (l[i]*data->dirx - data->x1)*kr;
					double ny = (l[i]*data->diry - data->y1)*kr;
					applyPoint(data, l[i], nx, ny);
				}
			}
		}
	} else {
		// circle with line
		double div = B*data->dirx - A*data->diry;
		if (fabs(div) > HELI_PRECISION_SQR) {
			double mult = 1/div;
			
			double kn = 1/sqrt(AB2);
			double nx = -B*kn;
			double ny =  A*kn;
			double kx = -nx*fabs(data->radius) - data->x1;
			double ky = -ny*fabs(data->radius) - data->y1;
			
			double ll = (ky*data->dirx - kx*data->diry)*mult;
			if (ll >= -HELI_PRECISION && ll <= 1 + HELI_PRECISION) {
				double l = (ky*A - kx*B)*mult;
				applyPoint(data, l, nx, ny);
			}
		}
	}
};

static void calcCorners(HeliCollider *c, double *corners) {
	double a = c->rotation*(PI/180);
	double cn = cos(a);
	double sn = sin(a);
	
	double wx =  0.5*cn*fabs(c->width);
	double wy =  0.5*sn*fabs(c->width);
	double hx = -0.5*sn*fabs(c->height);
	double hy =  0.5*cn*fabs(c->height);
	
	corners[0] =  wx + hx + c->x;
	corners[1] =  wy + hy + c->y;

	corners[2] =  wx - hx + c->x;
	corners[3] =  wy - hy + c->y;

	corners[4] = -wx - hx + c->x;
	corners[5] = -wy - hy + c->y;

	corners[6] = -wx + hx + c->x;
	corners[7] = -wy + hy + c->y;
}

static void collisionCorners(HeliCollisionData *data, double *corners, double x, double y) {
	for(int i = 0; i < 4; ++i) {
		int j = (i + 1)%4;
		data->x1 = corners[2*i+0] - x;
		data->y1 = corners[2*i+1] - y;
		data->x2 = corners[2*j+0] - x;
		data->y2 = corners[2*j+1] - y;
		collision(data);
		data->x2 = data->x1;
		data->y2 = data->y1;
		collision(data);
	}
}


int heliCheckCollision(
	HeliCollider *a,
	HeliCollider *b,
	HeliCollisionInfo *info,
	double distance )
{
	distance = fabs(distance);
	HeliCollisionData data = {};
	
	// calc "moving" direction
	data.dirx = b->x - a->x;
	data.diry = b->y - a->y;
	double k = data.dirx*data.dirx + data.diry*data.diry;
	if (k <= HELI_PRECISION_SQR) {
		data.dirx = 1;
		data.diry = 0;
	} else {
		k = 1/sqrt(k);
		data.dirx *= k;
		data.diry *= k;
	}
	
	// check collision
	if (a->type == COLLIDER_CIRCLE && b->type == COLLIDER_CIRCLE) {
		data.radius = fabs(a->radius) + fabs(b->radius) + distance;
		data.x1 = data.x2 = b->x - a->x;
		data.y1 = data.y2 = b->y - a->y;
		collision(&data);
	} else
	if (a->type == COLLIDER_CIRCLE && b->type == COLLIDER_RECTANGLE) {
		double corners[8];
		calcCorners(b, corners);
		data.radius = fabs(a->radius) + fabs(b->radius) + distance;
		if (data.radius > HELI_PRECISION) collisionCorners(&data, corners, a->x, a->y);
	} else
	if (a->type == COLLIDER_RECTANGLE && b->type == COLLIDER_CIRCLE) {
		double corners[8];
		calcCorners(a, corners);
		data.flip = TRUE;
		data.radius = fabs(a->radius) + fabs(b->radius) + distance;
		if (data.radius > HELI_PRECISION) collisionCorners(&data, corners, b->x, b->y);
	} else
	if (a->type == COLLIDER_RECTANGLE && b->type == COLLIDER_RECTANGLE) {
		double cornersA[8], cornersB[8];
		calcCorners(a, cornersA);
		calcCorners(b, cornersB);
		data.radius = fabs(a->radius) + fabs(b->radius) + distance;
		for(int i = 0; i < 4; ++i)
			collisionCorners(&data, cornersB, cornersA[2*i+0], cornersA[2*i+1]);
		data.flip = TRUE;
		for(int i = 0; i < 4; ++i)
			collisionCorners(&data, cornersA, cornersB[2*i+0], cornersB[2*i+1]);
	}
	
	// is collision found?
	info->actualCollision = FALSE;
	info->distance = 0;
	info->dx = info->dy = 0;
	if ( !data.found
	  || data.lmin >= -HELI_PRECISION
	  || data.lmax <= HELI_PRECISION )
	{
		info->ax = info->ay = 0;
		return FALSE;
	}
	
	// calc normal
	double l, nx, ny;
	if (-data.lmin < data.lmax) {
		l = data.lmin;
		nx = data.nxMin;
		ny = data.nyMin;
	} else {
		l = data.lmax;
		nx = data.nxMax;
		ny = data.nyMax;
	}
	
	// calc displacement
	info->actualCollision = TRUE;
	double kd = (nx*data.dirx + ny*data.diry)*l;
	info->distance = kd;
	if (kd >  distance + HELI_PRECISION) kd -= distance - HELI_PRECISION; else
	if (kd < -distance - HELI_PRECISION) kd += distance - HELI_PRECISION; else
		{ kd = 0; info->actualCollision = FALSE; }
	
	if (info->actualCollision) {
		info->dx = nx*kd;
		info->dy = ny*kd;
	}
	
	// calc velocity
	double bounciness = fabs(a->bounciness * b->bounciness);
	double bouncinessThreshold = fabs(a->bouncinessThreshold) + fabs(b->bouncinessThreshold);
	if (bounciness < HELI_PRECISION) bounciness = 0;
	
	double vb = -info->vx*nx - info->vy*ny;
	double vs =  info->vy*nx - info->vx*ny;
	
	if (vb > -0.5*HELI_PRECISION) {
		vb -= bouncinessThreshold;
		if (vb < HELI_PRECISION) vb = 0;
		vb *= bounciness;
	
		if (info->actualCollision || vb > HELI_PRECISION) {
			info->vx = nx*vb - ny*vs;
			info->vy = ny*vb + nx*vs;
		}
	}
	
	// calc weight
	double w = -info->ax*nx - info->ay*ny;
	if (w < HELI_PRECISION) w = 0;
	if (fabs(vb) > 0.4*HELI_PRECISION) w = 0;
	info->ax = nx*w;
	info->ay = ny*w;
	
	return TRUE;
}


int heliPointCollision(HeliCollider *c, double x, double y) {
	x -= c->x;
	y -= c->y;
	if (c->type == COLLIDER_CIRCLE) {
		return x*x + y*y <= c->radius*c->radius;
	} else
	if (c->type == COLLIDER_RECTANGLE) {
		double a = c->rotation*(PI/180);
		double sn = sin(a);
		double cn = cos(a);
		return fabs(x*cn - y*sn) <= c->width*0.5
			&& fabs(x*sn + y*cn) <= c->height*0.5;
	}
	return FALSE;
}