package org.cocos2d.kazmath;

import static org.cocos2d.kazmath.Utility.kmEpsilon;
import static org.cocos2d.kazmath.Utility.kmMax;
import static org.cocos2d.kazmath.Utility.kmMin;

public class kmRay2 {

	public kmVec2 start = new kmVec2();
	public kmVec2 dir = new kmVec2();

	public static void kmRay2Fill(kmRay2 ray, float px, float py, float vx, float vy) {
		ray.start.x = px;
		ray.start.y = py;
		ray.dir.x = vx;
		ray.dir.y = vy;
	}

	public static boolean kmRay2IntersectLineSegment(final kmRay2 ray, final kmVec2 p1, final kmVec2 p2, kmVec2 intersection) {
		float x1 = ray.start.x;
		float y1 = ray.start.y;
		float x2 = ray.start.x + ray.dir.x;
		float y2 = ray.start.y + ray.dir.y;
		float x3 = p1.x;
		float y3 = p1.y;
		float x4 = p2.x;
		float y4 = p2.y;

		float denom = (y4 -y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
		float ua, x, y;
		//If denom is zero, the lines are parallel
		if(denom > -kmEpsilon && denom < kmEpsilon) {
			return false;
		}

		ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denom;
//		float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denom;

		x = x1 + ua * (x2 - x1);
		y = y1 + ua * (y2 - y1);

		if(	x < kmMin(p1.x, p2.x) - kmEpsilon ||
			x > kmMax(p1.x, p2.x) + kmEpsilon ||
			y < kmMin(p1.y, p2.y) - kmEpsilon ||
			y > kmMax(p1.y, p2.y) + kmEpsilon) {
			//Outside of line
			//printf("Outside of line, %f %f (%f %f)(%f, %f)\n", x, y, p1->x, p1->y, p2->x, p2->y);
			return false;
		}

		if(	x < kmMin(x1, x2) - kmEpsilon ||
			x > kmMax(x1, x2) + kmEpsilon ||
			y < kmMin(y1, y2) - kmEpsilon ||
			y > kmMax(y1, y2) + kmEpsilon) {
			//printf("Outside of ray, %f %f (%f %f)(%f, %f)\n", x, y, x1, y1, x2, y2);
			return false;
		}

		intersection.x = x;
		intersection.y = y;

		return true;
	}

	private static void calculate_line_normal(final kmVec2 p1, final kmVec2 p2, kmVec2 normal_out) {
		kmVec2 tmp = new kmVec2();
		kmVec2.kmVec2Subtract(tmp, p2, p1); //Get direction vector

		normal_out.x = -tmp.y;
		normal_out.y = tmp.x;
		kmVec2.kmVec2Normalize(normal_out, normal_out);

		//TODO: should check that the normal is pointing out of the triangle
	}

	public static boolean kmRay2IntersectTriangle(final kmRay2 ray, final kmVec2 p1, final kmVec2 p2, final kmVec2 p3, kmVec2 intersection, kmVec2 normal_out) {
		kmVec2 intersect = new kmVec2();
		kmVec2 final_intersect = new kmVec2();
		kmVec2 normal = new kmVec2();
		float distance = 10000.0f;
		boolean intersected = false;

		if(kmRay2IntersectLineSegment(ray, p1, p2, intersect)) {
			kmVec2 tmp = new kmVec2();
			float this_distance;

			intersected = true;
			this_distance = kmVec2.kmVec2Length(kmVec2.kmVec2Subtract(tmp, intersect, ray.start));
			if(this_distance < distance) {
				final_intersect.x = intersect.x;
				final_intersect.y = intersect.y;
				distance = this_distance;

				calculate_line_normal(p1, p2, normal);
			}
		}

		if(kmRay2IntersectLineSegment(ray, p2, p3, intersect)) {
			kmVec2 tmp = new kmVec2();
			float this_distance;
			intersected = true;

			this_distance = kmVec2.kmVec2Length(kmVec2.kmVec2Subtract(tmp, intersect, ray.start));
			if(this_distance < distance) {
				final_intersect.x = intersect.x;
				final_intersect.y = intersect.y;
				distance = this_distance;

				calculate_line_normal(p2, p3, normal);
			}
		}

		if(kmRay2IntersectLineSegment(ray, p3, p1, intersect)) {
			kmVec2 tmp = new kmVec2();
			float this_distance;
			intersected = true;

			this_distance = kmVec2.kmVec2Length(kmVec2.kmVec2Subtract(tmp, intersect, ray.start));
			if(this_distance < distance) {
				final_intersect.x = intersect.x;
				final_intersect.y = intersect.y;
				//distance = this_distance;

				calculate_line_normal(p3, p1, normal);
			}
		}

		if(intersected) {
			intersection.x = final_intersect.x;
			intersection.y = final_intersect.y;
			if(normal_out != null) {
				normal_out.x = normal.x;
				normal_out.y = normal.y;
			}
		}

		return intersected;
	}

	public static boolean kmRay2IntersectCircle(final kmRay2 ray, final kmVec2 centre, final float radius, kmVec2 intersection) {
		assert false : "Not implemented";
		return false;
	}
}
