using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

using Jitter.LinearMath;
using Jitter.Collision.Shapes;
using Jitter.Dynamics;
using Jitter.Collision;

namespace XNAEngine.Entities
{
	class PhysicsEntity : RenderableEntity
	{
		RigidBody PhysicsBody;

		public PhysicsEntity(Model Model, Jitter.World World)
			: base(Model)
		{

			/*List<TriangleVertexIndices> indices = new List<TriangleVertexIndices>();
			List<Vector3> vertices = new List<Vector3>();

			ExtractData(vertices, indices, Model);

			List<JVector> jvertices = new List<JVector>(vertices.Count);
			foreach (Vector3 vertex in vertices) jvertices.Add(new JVector(vertex.X, vertex.Y, vertex.Z));

			ConvexHullShape PhysMesh = new ConvexHullShape(jvertices);*/

			List<CompoundShape.TransformedShape> transformedShapes = new List<CompoundShape.TransformedShape>();

			Matrix[] bones_ = new Matrix[Model.Bones.Count];
			Model.CopyAbsoluteBoneTransformsTo(bones_);

			foreach (ModelMesh mm in Model.Meshes)
			{
				transformedShapes.Add( ExtractData(mm, bones_) );
			}

			CompoundShape PhysMesh = new CompoundShape(transformedShapes);

			PhysicsBody = new RigidBody(PhysMesh);
			PhysicsBody.EnableDebugDraw = true;
			World.AddBody(PhysicsBody);
		}

		public void Debug(Jitter.IDebugDrawer debugDraw)
		{
			PhysicsBody.DebugDraw(debugDraw);
		}

		public override Vector3 GetPosition()
		{
			return new Vector3(PhysicsBody.Position.X, PhysicsBody.Position.Y, PhysicsBody.Position.Z);
		}

		public override Matrix GetRotation()
		{
			return ToXNAMatrix(PhysicsBody.Orientation);
		}

		public Vector3 GetVelocity()
		{
			return new Vector3(PhysicsBody.LinearVelocity.X, PhysicsBody.LinearVelocity.Y, PhysicsBody.LinearVelocity.Z);
		}

		public Vector3 GetAngVelocity()
		{
			return new Vector3(PhysicsBody.AngularVelocity.X, PhysicsBody.AngularVelocity.Y, PhysicsBody.AngularVelocity.Z);
		}

		public bool GetIsStatic()
		{
			return PhysicsBody.IsStatic;
		}

		public override void SetPosition(Vector3 Position)
		{
			PhysicsBody.Position = new JVector(Position.X, Position.Y, Position.Z);
		}

		public override void SetRotation(Matrix Rotation)
		{
			PhysicsBody.Orientation = ToJitterMatrix(Rotation);
		}

		public void SetVelocity(Vector3 Velocity)
		{
			PhysicsBody.LinearVelocity = new JVector(Velocity.X, Velocity.Y, Velocity.Z);
		}

		public void SetAngVelocity(Vector3 AngVelocity)
		{
			PhysicsBody.AngularVelocity = new JVector(AngVelocity.X, AngVelocity.Y, AngVelocity.Z);
		}

		public void SetIsStatic(bool Static)
		{
			PhysicsBody.IsStatic = Static;
		}

		protected Matrix ToXNAMatrix(JMatrix Matrix)
		{
			return new Matrix ( Matrix.M11 ,
				Matrix.M12 ,
				Matrix.M13 ,
				0.0f ,
				Matrix.M21 ,
				Matrix.M22 ,
				Matrix.M23 ,
				0.0f ,
				Matrix.M31 ,
				Matrix.M32 ,
				Matrix.M33 ,
				0.0f , 0.0f , 0.0f , 0.0f , 1.0f);
		}

		protected JMatrix ToJitterMatrix(Matrix matrix)
		{
			JMatrix result;
			result.M11 = matrix.M11;
			result.M12 = matrix.M12;
			result.M13 = matrix.M13;
			result.M21 = matrix.M21;
			result.M22 = matrix.M22;
			result.M23 = matrix.M23;
			result.M31 = matrix.M31;
			result.M32 = matrix.M32;
			result.M33 = matrix.M33;
			return result;

		}

		protected void ExtractData(List<Vector3> vertices, List<TriangleVertexIndices> indices, Model model)
		{
			Matrix[] bones_ = new Matrix[model.Bones.Count];
			model.CopyAbsoluteBoneTransformsTo(bones_);
			foreach (ModelMesh mm in model.Meshes)
			{
				Matrix xform = bones_[mm.ParentBone.Index];
				foreach (ModelMeshPart mmp in mm.MeshParts)
				{
					int offset = vertices.Count;
					Vector3[] a = new Vector3[mmp.NumVertices];
					mmp.VertexBuffer.GetData<Vector3>(mmp.VertexOffset * mmp.VertexBuffer.VertexDeclaration.VertexStride,
						a, 0, mmp.NumVertices, mmp.VertexBuffer.VertexDeclaration.VertexStride);
					for (int i = 0; i != a.Length; ++i)
						Vector3.Transform(ref a[i], ref xform, out a[i]);
					vertices.AddRange(a);

					if (mmp.IndexBuffer.IndexElementSize != IndexElementSize.SixteenBits)
						throw new Exception(
							String.Format("Model uses 32-bit indices, which are not supported."));
					short[] s = new short[mmp.PrimitiveCount * 3];
					mmp.IndexBuffer.GetData<short>(mmp.StartIndex * 2, s, 0, mmp.PrimitiveCount * 3);
					TriangleVertexIndices[] tvi = new TriangleVertexIndices[mmp.PrimitiveCount];
					for (int i = 0; i != tvi.Length; ++i)
					{
						tvi[i].I0 = s[i * 3 + 0] + offset;
						tvi[i].I1 = s[i * 3 + 1] + offset;
						tvi[i].I2 = s[i * 3 + 2] + offset;
					}
					indices.AddRange(tvi);
				}
			}
		}

		protected CompoundShape.TransformedShape ExtractData(ModelMesh mm, Matrix[] bones_)
		{
			List<TriangleVertexIndices> indices = new List<TriangleVertexIndices>();
			List<Vector3> vertices = new List<Vector3>();

			Matrix xform = bones_[mm.ParentBone.Index];
			foreach (ModelMeshPart mmp in mm.MeshParts)
			{
				int offset = vertices.Count;
				Vector3[] a = new Vector3[mmp.NumVertices];
				mmp.VertexBuffer.GetData<Vector3>(mmp.VertexOffset * mmp.VertexBuffer.VertexDeclaration.VertexStride,
					a, 0, mmp.NumVertices, mmp.VertexBuffer.VertexDeclaration.VertexStride);
				//for (int i = 0; i != a.Length; ++i)
				//	Vector3.Transform(ref a[i], ref xform, out a[i]);
				vertices.AddRange(a);

				if (mmp.IndexBuffer.IndexElementSize != IndexElementSize.SixteenBits)
					throw new Exception(
						String.Format("Model uses 32-bit indices, which are not supported."));
				short[] s = new short[mmp.PrimitiveCount * 3];
				mmp.IndexBuffer.GetData<short>(mmp.StartIndex * 2, s, 0, mmp.PrimitiveCount * 3);
				TriangleVertexIndices[] tvi = new TriangleVertexIndices[mmp.PrimitiveCount];
				for (int i = 0; i != tvi.Length; ++i)
				{
					tvi[i].I0 = s[i * 3 + 0] + offset;
					tvi[i].I1 = s[i * 3 + 1] + offset;
					tvi[i].I2 = s[i * 3 + 2] + offset;
				}
				indices.AddRange(tvi);
			}

			List<JVector> jvertices = new List<JVector>(vertices.Count);
			foreach (Vector3 vertex in vertices) jvertices.Add(new JVector(vertex.X, vertex.Y, vertex.Z));

			Shape convexHull = new ConvexHullShape(jvertices);

			Vector3 translation, scale;
			Quaternion rotation;

			xform.Decompose(out scale, out rotation, out translation);
			
			return new CompoundShape.TransformedShape(convexHull, ToJitterMatrix(Matrix.CreateFromQuaternion(rotation)),  new JVector(translation.X, translation.Y, translation.Z));
		}
	}
}