/*************************************************************************/
/*  test_misc.cpp                                                        */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                      https://godotengine.org                          */
/*************************************************************************/
/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur.                 */
/*                                                                       */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the       */
/* "Software"), to deal in the Software without restriction, including   */
/* without limitation the rights to use, copy, modify, merge, publish,   */
/* distribute, sublicense, and/or sell copies of the Software, and to    */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions:                                             */
/*                                                                       */
/* The above copyright notice and this permission notice shall be        */
/* included in all copies or substantial portions of the Software.       */
/*                                                                       */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
/*************************************************************************/
#include "test_misc.h"
#include "math_funcs.h"
#include "os/main_loop.h"
#include "print_string.h"
#include "servers/visual_server.h"

namespace TestMisc {

struct ConvexTestResult {

	Vector3 edgeA[2];
	Vector3 edgeB[2];
	bool valid;
	Vector3 contactA;
	Vector3 contactB;
	Vector3 contactNormal;
	float depth;

	/*
	Vector3 contactA;
	Vector3 contactB;
	Vector3 contactNormal;
	Vector3 contactX;
	Vector3 contactY;
	Vector3 edgeA[2];
	Vector3 edgeB[2];
	float depth;
	bool valid;
	bool isEdgeEdge;
	bool needTransform;
	neBool ComputerEdgeContactPoint(ConvexTestResult & res);
	neBool ComputerEdgeContactPoint2(float & au, float & bu);
	void Reverse()
	{
		neSwap(contactA, contactB);
		contactNormal *= -1.0f;
	}*/
	bool ComputerEdgeContactPoint2(float &au, float &bu);
};

bool ConvexTestResult::ComputerEdgeContactPoint2(float &au, float &bu) {
	float d1343, d4321, d1321, d4343, d2121;
	float numer, denom;

	Vector3 p13;
	Vector3 p43;
	Vector3 p21;
	Vector3 diff;

	p13 = (edgeA[0]) - (edgeB[0]);
	p43 = (edgeB[1]) - (edgeB[0]);

	if (p43.length_squared() < CMP_EPSILON2) {
		valid = false;
		goto ComputerEdgeContactPoint2_Exit;
	}

	p21 = (edgeA[1]) - (edgeA[0]);

	if (p21.length_squared() < CMP_EPSILON2) {
		valid = false;
		goto ComputerEdgeContactPoint2_Exit;
	}

	d1343 = p13.dot(p43);
	d4321 = p43.dot(p21);
	d1321 = p13.dot(p21);
	d4343 = p43.dot(p43);
	d2121 = p21.dot(p21);

	denom = d2121 * d4343 - d4321 * d4321;

	if (ABS(denom) < CMP_EPSILON) {
		valid = false;

		goto ComputerEdgeContactPoint2_Exit;
	}

	numer = d1343 * d4321 - d1321 * d4343;
	au = numer / denom;
	bu = (d1343 + d4321 * (au)) / d4343;

	if (au < 0.0f || au >= 1.0f) {
		valid = false;
	} else if (bu < 0.0f || bu >= 1.0f) {
		valid = false;
	} else {
		valid = true;
	}
	{
		Vector3 tmpv;

		tmpv = p21 * au;
		contactA = (edgeA[0]) + tmpv;

		tmpv = p43 * bu;
		contactB = (edgeB[0]) + tmpv;
	}

	diff = contactA - contactB;

	depth = Math::sqrt(diff.dot(diff));

	return true;

ComputerEdgeContactPoint2_Exit:

	return false;
}

struct neCollisionResult {

	float depth;
	bool penetrate;
	Matrix3 collisionFrame;
	Vector3 contactA;
	Vector3 contactB;
};

struct TConvex {

	float radius;
	float half_height;
	float CylinderRadius() const { return radius; }
	float CylinderHalfHeight() const { return half_height; }
};

float GetDistanceFromLine2(Vector3 v, Vector3 &project, const Vector3 &pointA, const Vector3 &pointB) {
	Vector3 ba = pointB - pointA;

	float len = ba.length();

	if (len < CMP_EPSILON)
		ba = Vector3();
	else
		ba *= 1.0f / len;

	Vector3 pa = v - pointA;

	float k = pa.dot(ba);

	project = pointA + ba * k;

	Vector3 diff = v - project;

	return diff.length();
}

void TestCylinderVertEdge(neCollisionResult &result, Vector3 &edgeA1, Vector3 &edgeA2, Vector3 &vertB,
		TConvex &cA, TConvex &cB, Transform &transA, Transform &transB, bool flip) {
	Vector3 project;

	float dist = GetDistanceFromLine2(vertB, project, edgeA1, edgeA2);

	float depth = cA.CylinderRadius() + cB.CylinderRadius() - dist;

	if (depth <= 0.0f)
		return;

	if (depth <= result.depth)
		return;

	result.penetrate = true;

	result.depth = depth;

	if (!flip) {
		result.collisionFrame.set_axis(2, (project - vertB).normalized());

		result.contactA = project - result.collisionFrame.get_axis(2) * cA.CylinderRadius();

		result.contactB = vertB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();
	} else {

		result.collisionFrame.set_axis(2, (vertB - project).normalized());

		result.contactA = vertB - result.collisionFrame.get_axis(2) * cB.CylinderRadius();

		result.contactB = project + result.collisionFrame.get_axis(2) * cA.CylinderRadius();
	}
}

void TestCylinderVertVert(neCollisionResult &result, Vector3 &vertA, Vector3 &vertB,
		TConvex &cA, TConvex &cB, Transform &transA, Transform &transB) {
	Vector3 diff = vertA - vertB;

	float dist = diff.length();

	float depth = cA.CylinderRadius() + cB.CylinderRadius() - dist;

	if (depth <= 0.0f)
		return;

	if (depth <= result.depth)
		return;

	result.penetrate = true;

	result.depth = depth;

	result.collisionFrame.set_axis(2, diff * (1.0f / dist));

	result.contactA = vertA - result.collisionFrame.get_axis(2) * cA.CylinderRadius();

	result.contactB = vertB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();
}

void Cylinder2CylinderTest(neCollisionResult &result, TConvex &cA, Transform &transA, TConvex &cB, Transform &transB) {
	result.penetrate = false;

	Vector3 dir = transA.basis.get_axis(1).cross(transB.basis.get_axis(1));

	float len = dir.length();

	//	bool isParallel = len<CMP_EPSILON;

	//	int doVertCheck = 0;

	ConvexTestResult cr;

	cr.edgeA[0] = transA.origin + transA.basis.get_axis(1) * cA.CylinderHalfHeight();
	cr.edgeA[1] = transA.origin - transA.basis.get_axis(1) * cA.CylinderHalfHeight();
	cr.edgeB[0] = transB.origin + transB.basis.get_axis(1) * cB.CylinderHalfHeight();
	cr.edgeB[1] = transB.origin - transB.basis.get_axis(1) * cB.CylinderHalfHeight();

	//	float dot = transA.basis.get_axis(1).dot(transB.basis.get_axis(1));

	if (len > CMP_EPSILON) {
		float au, bu;

		cr.ComputerEdgeContactPoint2(au, bu);

		if (cr.valid) {
			float depth = cA.CylinderRadius() + cB.CylinderRadius() - cr.depth;

			if (depth <= 0.0f)
				return;

			result.depth = depth;

			result.penetrate = true;

			result.collisionFrame.set_axis(2, (cr.contactA - cr.contactB) * (1.0f / cr.depth));

			result.contactA = cr.contactA - result.collisionFrame.get_axis(2) * cA.CylinderRadius();

			result.contactB = cr.contactB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();

			return;
		}
	}
	result.depth = -1.0e6f;

	int i;

	for (i = 0; i < 2; i++) {
		//project onto edge b

		Vector3 diff = cr.edgeA[i] - cr.edgeB[1];

		float dot = diff.dot(transB.basis.get_axis(1));

		if (dot < 0.0f) {
			TestCylinderVertVert(result, cr.edgeA[i], cr.edgeB[1], cA, cB, transA, transB);
		} else if (dot > (2.0f * cB.CylinderHalfHeight())) {
			TestCylinderVertVert(result, cr.edgeA[i], cr.edgeB[0], cA, cB, transA, transB);
		} else {
			TestCylinderVertEdge(result, cr.edgeB[0], cr.edgeB[1], cr.edgeA[i], cB, cA, transB, transA, true);
		}
	}
	for (i = 0; i < 2; i++) {
		//project onto edge b

		Vector3 diff = cr.edgeB[i] - cr.edgeA[1];

		float dot = diff.dot(transA.basis.get_axis(1));

		if (dot < 0.0f) {
			TestCylinderVertVert(result, cr.edgeB[i], cr.edgeA[1], cA, cB, transA, transB);
		} else if (dot > (2.0f * cB.CylinderHalfHeight())) {
			TestCylinderVertVert(result, cr.edgeB[i], cr.edgeA[0], cA, cB, transA, transB);
		} else {
			TestCylinderVertEdge(result, cr.edgeA[0], cr.edgeA[1], cr.edgeB[i], cA, cB, transA, transB, false);
		}
	}
}

class TestMainLoop : public MainLoop {

	RID meshA;
	RID meshB;
	RID poly;
	RID instance;
	RID camera;
	RID viewport;
	RID boxA;
	RID boxB;
	RID scenario;

	Transform rot_a;
	Transform rot_b;

	bool quit;

public:
	virtual void input_event(const InputEvent &p_event) {

		if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_LEFT) {

			rot_b.origin.y += -p_event.mouse_motion.relative_y / 100.0;
			rot_b.origin.x += p_event.mouse_motion.relative_x / 100.0;
		}
		if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_MIDDLE) {

			//rot_b.origin.x+=-p_event.mouse_motion.relative_y/100.0;
			rot_b.origin.z += p_event.mouse_motion.relative_x / 100.0;
		}
		if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_RIGHT) {

			float rot_x = -p_event.mouse_motion.relative_y / 100.0;
			float rot_y = p_event.mouse_motion.relative_x / 100.0;
			rot_b.basis = rot_b.basis * Matrix3(Vector3(1, 0, 0), rot_x) * Matrix3(Vector3(0, 1, 0), rot_y);
		}
	}
	virtual void request_quit() {

		quit = true;
	}
	virtual void init() {

		VisualServer *vs = VisualServer::get_singleton();

		camera = vs->camera_create();

		viewport = vs->viewport_create();
		vs->viewport_attach_to_screen(viewport);
		vs->viewport_attach_camera(viewport, camera);
		vs->camera_set_transform(camera, Transform(Matrix3(), Vector3(0, 0, 3)));

		/* CONVEX SHAPE */

		DVector<Plane> cylinder_planes = Geometry::build_cylinder_planes(0.5, 2, 9, Vector3::AXIS_Y);
		RID cylinder_material = vs->fixed_material_create();
		vs->fixed_material_set_param(cylinder_material, VisualServer::FIXED_MATERIAL_PARAM_DIFFUSE, Color(0.8, 0.2, 0.9));
		vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_ONTOP, true);
		//vs->material_set_flag( cylinder_material, VisualServer::MATERIAL_FLAG_WIREFRAME,true);
		vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_DOUBLE_SIDED, true);
		vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_UNSHADED, true);

		RID cylinder_mesh = vs->mesh_create();
		Geometry::MeshData cylinder_data = Geometry::build_convex_mesh(cylinder_planes);
		vs->mesh_add_surface_from_mesh_data(cylinder_mesh, cylinder_data);
		vs->mesh_surface_set_material(cylinder_mesh, 0, cylinder_material);

		meshA = vs->instance_create2(cylinder_mesh, scenario);
		meshB = vs->instance_create2(cylinder_mesh, scenario);
		boxA = vs->instance_create2(vs->get_test_cube(), scenario);
		boxB = vs->instance_create2(vs->get_test_cube(), scenario);

		/*
		RID lightaux = vs->light_create( VisualServer::LIGHT_OMNI );
		vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_RADIUS, 80 );
		vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ATTENUATION, 1 );
		vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ENERGY, 1.5 );
		light = vs->instance_create2( lightaux );
		*/
		RID lightaux = vs->light_create(VisualServer::LIGHT_DIRECTIONAL);
		//vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_AMBIENT, Color(0.0,0.0,0.0) );
		//vs->light_set_shadow( lightaux, true );
		RID light = vs->instance_create2(lightaux, scenario);

		//rot_a=Transform(Matrix3(Vector3(1,0,0),Math_PI/2.0),Vector3());
		rot_b = Transform(Matrix3(), Vector3(2, 0, 0));

		//rot_x=0;
		//rot_y=0;
		quit = false;
	}
	virtual bool idle(float p_time) {

		VisualServer *vs = VisualServer::get_singleton();

		vs->instance_set_transform(meshA, rot_a);
		vs->instance_set_transform(meshB, rot_b);

		neCollisionResult res;
		TConvex a;
		a.radius = 0.5;
		a.half_height = 1;
		Cylinder2CylinderTest(res, a, rot_a, a, rot_b);
		if (res.penetrate) {

			Matrix3 scale;
			scale.scale(Vector3(0.1, 0.1, 0.1));
			vs->instance_set_transform(boxA, Transform(scale, res.contactA));
			vs->instance_set_transform(boxB, Transform(scale, res.contactB));
			print_line("depth: " + rtos(res.depth));
		} else {

			Matrix3 scale;
			scale.scale(Vector3());
			vs->instance_set_transform(boxA, Transform(scale, res.contactA));
			vs->instance_set_transform(boxB, Transform(scale, res.contactB));
		}
		print_line("collided: " + itos(res.penetrate));

		return false;
	}

	virtual bool iteration(float p_time) {

		return quit;
	}
	virtual void finish() {
	}
};

MainLoop *test() {

	return memnew(TestMainLoop);
}
}