#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;

}