using System;
using Microsoft.Xna.Framework;
using Jitter.Collision;
using Jitter.Dynamics.Constraints.SingleBody;
using Jitter.LinearMath;

namespace KinPortal {
	public class GrabStrategy : TargetingStrategy {
		private PointOnPoint m_leftGrabConstraint = null;
		private PointOnPoint m_rightGrabConstraint = null;
		private float m_leftHitDistance;
		private float m_rightHitDistance;
		private float m_leftHandHitDepth;
		private float m_rightHandHitDepth;

		public void Update(DynamicObject obj, GameTime gameTime) {
			KinPortal.Game game = (KinPortal.Game) obj.Game;

			if(game.HandsTracker.LeftHand.IsClosed) {
				Vector3 ray = obj.Scene.Camera.Unproject(game.HandsTracker.LeftHand.Pos.X, game.HandsTracker.LeftHand.Pos.Y);
				ray.Normalize();
				if(m_leftGrabConstraint != null) {
					float hitDistance = m_leftHitDistance * (1.0f + (float) game.HandsTracker.LeftHand.Depth - m_leftHandHitDepth);
					m_leftGrabConstraint.Anchor = Geometry.GetJitterVector(obj.Scene.Camera.Pos + ray * hitDistance);
					obj.PhysicModel.IsActive = true;
					obj.PhysicModel.LinearVelocity *= 0.98f;
					obj.PhysicModel.AngularVelocity *= 0.98f;
				}
			} else {
				if(m_leftGrabConstraint != null) {
					obj.Scene.PhysicWorld.RemoveConstraint(m_leftGrabConstraint);
					m_leftGrabConstraint = null;
				}
			}

			if(game.HandsTracker.RightHand.IsClosed) {
				Vector3 ray = obj.Scene.Camera.Unproject(game.HandsTracker.RightHand.Pos.X, game.HandsTracker.RightHand.Pos.Y);
				ray.Normalize();
				if(m_rightGrabConstraint != null) {
					float hitDistance = m_rightHitDistance * (1.0f + (float) game.HandsTracker.RightHand.Depth - m_rightHandHitDepth);
					m_rightGrabConstraint.Anchor = Geometry.GetJitterVector(obj.Scene.Camera.Pos + ray * hitDistance);
					obj.PhysicModel.IsActive = true;
					obj.PhysicModel.LinearVelocity *= 0.98f;
					obj.PhysicModel.AngularVelocity *= 0.98f;
				}
			} else {
				if(m_rightGrabConstraint != null) {
					obj.Scene.PhysicWorld.RemoveConstraint(m_rightGrabConstraint);
					m_rightGrabConstraint = null;
				}
			}
		}

		public void Target(DynamicObject obj, HandsProjection leftHand, HandsProjection rightHand) {
			KinPortal.Game game = (KinPortal.Game) obj.Game;

			if(leftHand.IsProjected && game.HandsTracker.LeftHand.IsClosed && !game.HandsTracker.LeftHand.PrevIsClosed) {
				if(m_leftGrabConstraint != null) {
					obj.Scene.PhysicWorld.RemoveConstraint(m_leftGrabConstraint);
				}

				if(m_rightGrabConstraint != null) {
					return;
				}

				JVector lanchor = Geometry.GetJitterVector(leftHand.HitPoint - obj.Pos);
				lanchor = JVector.Transform(lanchor, JMatrix.Transpose(obj.PhysicModel.Orientation));

				m_leftGrabConstraint = new PointOnPoint(obj.PhysicModel, lanchor);
				m_leftGrabConstraint.Softness = 0.004f;
				m_leftGrabConstraint.BiasFactor = 0.1f;

				obj.Scene.PhysicWorld.AddConstraint(m_leftGrabConstraint);
				m_leftHitDistance = (leftHand.HitPoint - obj.Scene.Camera.Pos).Length();
				m_leftGrabConstraint.Anchor = Geometry.GetJitterVector(leftHand.HitPoint);
				m_leftHandHitDepth = (float) game.HandsTracker.LeftHand.Depth;
			}

			if(rightHand.IsProjected && game.HandsTracker.RightHand.IsClosed && !game.HandsTracker.RightHand.PrevIsClosed) {
				if(m_rightGrabConstraint != null) {
					obj.Scene.PhysicWorld.RemoveConstraint(m_rightGrabConstraint);
				}

				if(m_leftGrabConstraint != null) {
					return;
				}

				JVector lanchor = Geometry.GetJitterVector(rightHand.HitPoint - obj.Pos);
				lanchor = JVector.Transform(lanchor, JMatrix.Transpose(obj.PhysicModel.Orientation));

				m_rightGrabConstraint = new PointOnPoint(obj.PhysicModel, lanchor);
				m_rightGrabConstraint.Softness = 0.004f;
				m_rightGrabConstraint.BiasFactor = 0.1f;

				obj.Scene.PhysicWorld.AddConstraint(m_rightGrabConstraint);
				m_rightHitDistance = (rightHand.HitPoint - obj.Scene.Camera.Pos).Length();
				m_rightGrabConstraint.Anchor = Geometry.GetJitterVector(rightHand.HitPoint);
				m_rightHandHitDepth = (float) game.HandsTracker.RightHand.Depth;
			}
		}
	}
}