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

	double *dx, double *dy,

	double *vx, double *vy,

	double bounciness )

{

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

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

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

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

		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?

	*dx = 0; *dy = 0;

	if ( !data.found

	  || data.lmin >= -HELI_PRECISION

	  || data.lmax <= HELI_PRECISION )

		return FALSE;

	// calc displacement and velocity

	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;

	}

	double kd = (nx*data.dirx + ny*data.diry)*l;

	*dx = nx*kd;

	*dy = ny*kd;

	double kn = *vx*nx + *vy*ny;

	if (kn < 0) {

		*vx -= 2*kn*fabs(bounciness)*nx;

		*vy -= 2*kn*fabs(bounciness)*ny;

	}

	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;

}