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

namespace RigidBody {

	/// <summary>
	/// 剛体
	/// </summary>
	public partial class Body {
		/// <summary>
		/// 質量特性
		/// </summary>
		public MassProperty MassProperty;

		/// <summary>
		/// 描画モデル
		/// </summary>
		public Model Visual;

		/// <summary>
		/// 変換行列
		/// </summary>
		public Matrix Transform {
			get {
				var scale = Shape.Scale;
				Matrix result;
				Matrix.CreateScale(ref scale, out result);
				Matrix.Transform(ref result, ref Rotation, out result);
				result.Translation = Position;
				return result;
			}
		}

		/// <summary>
		/// 形状
		/// </summary>
		public Shape Shape;

		/// <summary>
		/// 位置
		/// </summary>
		public Vector3 Position;
		/// <summary>
		/// 運動量
		/// </summary>
		public Vector3 Momentum;

		/// <summary>
		/// 力
		/// </summary>
		public Vector3 Force;
		/// <summary>
		/// トルク
		/// </summary>
		public Vector3 Torque;

		/// <summary>
		/// 角運動量
		/// </summary>
		Vector3 AngularMomentum;
		/// <summary>
		/// 姿勢
		/// </summary>
		public Quaternion Rotation = Quaternion.Identity;

		Vector3 angularVelocity;
		/// <summary>
		/// 角速度
		/// </summary>
		public Vector3 AngularVelocity {
			get { return angularVelocity; }
			protected set { angularVelocity = value; }
		}

		Vector3 velocity;

		/// <summary>
		/// 速度
		/// </summary>
		public Vector3 Velocity {
			get { return velocity; }
			protected set { velocity = value; }
		}

		Matrix inverseInertiaTensor;
		/// <summary>
		/// 現在の逆慣性テンソル
		/// </summary>
		public Matrix InverseInertiaTensor {
			get { return inverseInertiaTensor; }
		}
	}

	public partial class Body {
		/// <summary>
		/// 力を初期化する。
		/// </summary>
		public void ClearForce() {
			Force = Vector3.Zero;
			Torque = Vector3.Zero;
		}
	}

	public partial class Body {
		/// <summary>
		/// 指定点に力を加える。
		/// </summary>
		/// <param name="worldPoint"></param>
		/// <param name="worldForce"></param>
		public void AddForce(Vector3 worldPoint, Vector3 worldForce) {
			Force += worldForce;
			Torque += Vector3.Cross(worldPoint - Position, worldForce);
		}
	}

	public partial class Body {
		/// <summary>
		/// 指定点での物体の速度
		/// </summary>
		/// <param name="worldPoint"></param>
		/// <returns></returns>
		public Vector3 GetVelocityAt(Vector3 worldPoint) {
			return Velocity + Vector3.Cross(angularVelocity, worldPoint - Position);
		}
	}

	public partial class Body {
		/// <summary>
		/// result = bias + value * scale
		/// </summary>
		/// <param name="bias"></param>
		/// <param name="value"></param>
		/// <param name="scale"></param>
		/// <param name="result"></param>
		static void MAdd(ref Vector3 bias, ref Vector3 value, float scale, out Vector3 result) {
			Vector3 a;
			Vector3.Multiply(ref value, scale, out a);
			Vector3.Add(ref bias, ref a, out result);
		}
		/// <summary>
		/// result = bias + value * scale
		/// </summary>
		/// <param name="bias"></param>
		/// <param name="value"></param>
		/// <param name="scale"></param>
		/// <param name="result"></param>
		static void MAdd(ref Quaternion bias, ref Quaternion value, float scale, out Quaternion result) {
			Quaternion a;
			Quaternion.Multiply(ref value, scale, out a);
			Quaternion.Add(ref bias, ref a, out result);
		}
	}

	public partial class Body {

		/// <summary>
		/// 時間を進める。
		/// </summary>
		/// <param name="elasped"></param>
		public void Update(float elasped, bool fbEuler) {
			if (fbEuler) {
				UpdateMomentum(elasped);
				UpdatePose(elasped);
			} else {
				UpdatePose(elasped);
				UpdateMomentum(elasped);
			}
		}

		/// <summary>
		/// 運動量を更新する。
		/// </summary>
		/// <param name="elasped"></param>
		public void UpdateMomentum(float elasped) {
			// 運動量の更新 : Momentum += Force * elapsed;
			MAdd(ref Momentum, ref Force, elasped, out Momentum);

			// 速度の更新
			Velocity = Momentum * MassProperty.InverseMass;

			// 角運動量の更新 : AngularMomentum += Torque * elapsed;
			MAdd(ref AngularMomentum, ref Torque, elasped, out AngularMomentum);

			// 逆慣性テンソルの更新
			Matrix TransposedRotationMatrix;
			Matrix RotationMatrix;
			Matrix.CreateFromQuaternion(ref Rotation, out RotationMatrix);
			Matrix.Transpose(ref RotationMatrix, out TransposedRotationMatrix);
			inverseInertiaTensor = RotationMatrix * MassProperty.InverseInertiaTensor * TransposedRotationMatrix;

			// 角速度の更新 : angularVelocity = AngularMomentum * inverseInertiaTensor;
			Vector3.TransformNormal(ref AngularMomentum, ref inverseInertiaTensor, out angularVelocity);
		}

		/// <summary>
		/// 位置と姿勢を更新する。
		/// </summary>
		/// <param name="elasped"></param>
		public void UpdatePose(float elasped) {
			// 位置の更新 : Position += Velocity * elapsed;
			MAdd(ref Position, ref velocity, elasped, out Position);

			// 姿勢の更新
			float qt = elasped * 0.5f;
			var q = new Quaternion(qt * angularVelocity.X, qt * angularVelocity.Y, qt * angularVelocity.Z, 0);
			Quaternion spin;
			Quaternion.Multiply(ref q, ref Rotation, out spin);
			Quaternion.Add(ref Rotation, ref spin, out Rotation);
			Rotation.Normalize();
		}

	}

}
