//********************************************************************
// This source file is part of Ninfa3D Engine
// 
// Copyright (C) 2008-2009 Viz_fuerte.
// All rights reserved.
// 
// LIMITED WARRANTY
// This software is provided "as-is", without any express or implied warranty.
// In no event shall the author be held liable for any damages arising from the use of this software.
// This is a contract. By accepting the Ninfa3D Engine (the SOFTWARE), 
// you (either an individual or a single entity, the LICENSEE) accept the terms and conditions of this licensing agreement.
// 
// If the LICENSEE does not accept the terms and conditions, then LICENSEE must not accept the SOFTWARE.
// If at a later time the LICENSEE decides not to continue licensing the SOFTWARE under these terms and 
// conditions, then the LICENSEE must delete permanently all files constituting the SOFTWARE.
//
// File: CBodySceneNode.cpp
//********************************************************************

/*--------------------------------------------------------------------
Includos*/

	#include "CMousePick.h"

/*--------------------------------------------------------------------
Variables externas*/

	extern ISceneManager* SceneManager;
	extern IVideoDriver* driver;
	extern NewtonWorld* nWorld;

/*--------------------------------------------------------------------
Variables globales*/

static dFloat pickedParam;
static dVector pickedForce;
static dVector rayLocalNormal;
static dVector rayWorldNormal;
static dVector rayWorldOrigin;
static dVector attachmentPoint;

static bool isPickedBodyDynamics;
static NewtonBody* pickedBody;
static NewtonApplyForceAndTorque chainForceCallBack; 

extern float GravityY;

#define MOUSE_PICK_DAMP			 10.0f
#define MOUSE_PICK_STIFFNESS	 100.0f
#define MAX_PICK_ACCEL			(64.0f * 10.0f)

static NewtonUserJoint* bodyPickController;

/*--------------------------------------------------------------------
Declaracion global de funciones*/

	unsigned RayCastPrefilter(const NewtonBody* body,const NewtonCollision* collision,void* userData)
	{
		return NewtonCollisionIsTriggerVolume(collision) ? 0 : 1;
	}

	float RayCastFilter(const NewtonBody* body,const float* normal,int collisionID,void* userData,float intersetParam)
	{
		float mass,Ixx,Iyy,Izz;

		NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
		if (intersetParam <  pickedParam)
		{
			isPickedBodyDynamics = (mass > 0.0f);
			pickedParam = intersetParam;
			pickedBody = (NewtonBody*)body;
			rayLocalNormal = dVector (normal[0], normal[1], normal[2]);
		}
		return intersetParam;
	}

	void PhysicsApplyPickForce(const NewtonBody* body,float timestep,int threadIndex)
	{
		float mass,Ixx,Iyy,Izz;
		dVector com,veloc,omega;
		dMatrix matrix;

		if (chainForceCallBack)
			chainForceCallBack(body,timestep,threadIndex);

		NewtonBodyGetVelocity(body,&veloc[0]);

		NewtonBodyGetOmega(body,&omega[0]);
		NewtonBodyGetVelocity(body,&veloc[0]);
		NewtonBodyGetMassMatrix(body,&mass,&Ixx,&Iyy,&Izz);

		dVector force (pickedForce.Scale(mass * MOUSE_PICK_STIFFNESS));
		dVector dampForce (veloc.Scale(MOUSE_PICK_DAMP * mass));
		force -= dampForce;

		NewtonBodyGetMatrix(body, &matrix[0][0]);
		NewtonBodyGetCentreOfMass (body, &com[0]);

		dVector point(matrix.RotateVector(attachmentPoint - com));
		dVector torque(point * force);

		dVector torqueDamp(omega.Scale(mass * 0.1f));

		NewtonBodyAddForce(body,&force.m_x);
		NewtonBodyAddTorque(body,&torque.m_x);

		NewtonBodySetFreezeState(body,0);
	}

	NewtonBody* MousePick(int mouse1x,int mouse1y,int mouseLeftKey1)
	{
		static int mouseLeftKey0;
		static int mouse0x,mouse0y;
		static bool mousePickMode = false;
		dMatrix matrix;
		line3d<f32> line;

		if (mouseLeftKey1)
		{
			if (!mouseLeftKey0)
			{
				line = SceneManager->getSceneCollisionManager()->getRayFromScreenCoordinates(position2d<s32>(mouse1x,mouse1y),SceneManager->getActiveCamera());

				dVector p0(dVector(line.start.X,line.start.Y,line.start.Z));
				dVector p1(dVector(line.end.X,line.end.Y,line.end.Z));

				pickedBody = NULL;
				pickedParam = 1.1;
				isPickedBodyDynamics = false;
				NewtonWorldRayCast(nWorld,&p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
				if(pickedBody)
				{
					float Ixx,Iyy,Izz,mass;
					mousePickMode = true;

					NewtonBodyGetMatrix(pickedBody,&matrix[0][0]);
					dVector p (p0 + (p1 - p0).Scale (pickedParam));

					attachmentPoint = matrix.UntransformVector (p);
					rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);

					NewtonBodyGetMassMatrix (pickedBody, &mass, &Ixx, &Iyy, &Izz);
					if (mass)
					{
					bodyPickController = CreateCustomKinematicController (pickedBody, &p[0]);
					CustomKinematicControllerSetMaxLinearFriction (bodyPickController, MAX_PICK_ACCEL);
					CustomKinematicControllerSetMaxAngularFriction (bodyPickController, MAX_PICK_ACCEL * 5.0f);
						//bodyPickController = CreateCustomPickBody(pickedBody, &p[0]);
						//CustomPickBodySetMaxLinearFriction(bodyPickController,(GravityY * 10.0f));
						//CustomPickBodySetMaxAngularFriction(bodyPickController,(GravityY * 10.0f) * 5.0f);
						//CustomPickBodySetMaxLinearFriction(bodyPickController,MAX_PICK_ACCEL);
						//CustomPickBodySetMaxAngularFriction(bodyPickController,MAX_PICK_ACCEL * 5.0f);
					}
				}
			}

			if (mousePickMode)
			{
				line = SceneManager->getSceneCollisionManager()->getRayFromScreenCoordinates(position2d<s32>(mouse1x,mouse1y),SceneManager->getActiveCamera());

				dVector p0(dVector(line.start.X,line.start.Y,line.start.Z));
				dVector p1(dVector(line.end.X,line.end.Y,line.end.Z));

				dVector p(p0 + (p1 - p0).Scale(pickedParam));

				if (bodyPickController)
					CustomKinematicControllerSetTargetPosit (bodyPickController, &p[0]);

				dMatrix matrix;
				NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
				rayWorldNormal = matrix.RotateVector(rayLocalNormal);

				dVector p2(matrix.TransformVector(attachmentPoint));

				matrix4 mat;
				SMaterial material;
				material.setTexture(1,0);
				material.setFlag(EMF_LIGHTING, false);
				driver->setTransform(ETS_WORLD, mat);
				driver->setMaterial(material);

				position2d<s32> pos1 = SceneManager->getSceneCollisionManager()->getScreenCoordinatesFrom3DPosition(vector3df(p.m_x,p.m_y,p.m_z),SceneManager->getActiveCamera());
				position2d<s32> pos2 = SceneManager->getSceneCollisionManager()->getScreenCoordinatesFrom3DPosition(vector3df(p2.m_x,p2.m_y,p2.m_z),SceneManager->getActiveCamera());
				
				driver->draw2DRectangle(SColor(255,0,0,255),rect<s32>(pos1.X-2.5,pos1.Y-2.5,pos1.X+5,pos1.Y+5));
				driver->draw2DRectangle(SColor(255,0,0,255),rect<s32>(pos2.X-2.5,pos2.Y-2.5,pos2.X+5,pos2.Y+5));
				
				driver->draw3DLine(vector3df(p.m_x,p.m_y,p.m_z),vector3df(p2.m_x,p2.m_y,p2.m_z),SColor(255,255,255,255));
			}
		}
		else
		{
			mousePickMode = false;
			if(pickedBody)
			{
				if(bodyPickController)
				{
					CustomDestroyJoint(bodyPickController);
				}
				bodyPickController = NULL;
			}
		}

		mouse0x = mouse1x;
		mouse0y = mouse1y;
		mouseLeftKey0 = mouseLeftKey1;

		return pickedBody;
	}