using Microsoft.Xna.Framework;

namespace RigidBody {

	public enum CollisionMethod {
		/// <summary>
		/// ペナルティ法（バネ・ダンパ）
		/// </summary>
		Penalty,
		/// <summary>
		/// 衝撃ベース法
		/// </summary>
		Impulse,
	}

	/// <summary>
	/// 形状
	/// </summary>
	public abstract class Shape {
		public abstract Vector3 Scale { get; }
		public abstract void Collides(Body body, ref Plane plane, CollisionMethod method, float elapsed);

		/// <summary>
		/// バネ係数
		/// </summary>
		float SpringCoefficient = 0.2f;
		/// <summary>
		/// ダンパ係数
		/// </summary>
		float DamperCoefficient = 0.6f * 0.05f;
		/// <summary>
		/// 反発係数
		/// </summary>
		float RestitutionCofficient = 0.0f;

		/// <summary>
		/// 平面と剛体の点との衝突
		/// </summary>
		/// <param name="body"></param>
		/// <param name="point"></param>
		/// <param name="plane1"></param>
		public virtual void Collides(Body body, ref Vector3 point, ref Plane plane,
			CollisionMethod method, float elapsed) {
			var distance = plane.DotCoordinate(point);
			if (distance < 0) {
				var relativeVelocity = Vector3.Dot(-body.GetVelocityAt(point), plane.Normal);
				switch (method) {
				case CollisionMethod.Penalty: {
						var kd = SpringCoefficient * -distance;
						var damper = DamperCoefficient * relativeVelocity;
						var f = kd + damper;
						body.AddForce(point, (f * 300000) * plane.Normal);
					}
					break;
				case CollisionMethod.Impulse: {
						var ra = point - body.Position;
						float t = 0;
						t += body.MassProperty.InverseMass;
						t += Vector3.Dot(plane.Normal, Vector3.Cross(Vector3.TransformNormal(
							Vector3.Cross(ra, plane.Normal), body.InverseInertiaTensor), ra));
						var f = -(1 + RestitutionCofficient)
							* -relativeVelocity / (t * elapsed);
						body.AddForce(point, f * plane.Normal);
					}
					break;
				}
			}
		}
	}

	/// <summary>
	/// 球
	/// </summary>
	public class Sphere : Shape {
		public override Vector3 Scale { get { return new Vector3(Radius); } }
		/// <summary>
		/// 半径
		/// </summary>
		public float Radius;

		public override void Collides(Body body, ref Plane plane, CollisionMethod method, float elapsed) {
			var point = body.Position - plane.Normal * Radius;
			Collides(body, ref point, ref plane, method, elapsed);
		}
	}

	/// <summary>
	/// 直方体
	/// </summary>
	public class Box : Shape {
		Vector3 scale;
		Vector3[] corners = new Vector3[8];
		BoundingBox bounds = new BoundingBox(-Vector3.One, Vector3.One);

		public Box(Vector3 scale) {
			this.scale = scale;
		}
		public override Vector3 Scale { get { return scale; } }
		public override void Collides(Body body, ref Plane plane, CollisionMethod method, float elapsed) {
			bounds.GetCorners(corners);
			var transform = body.Transform;
			Vector3.Transform(corners, ref transform, corners);
			for (int i = 0; i < corners.Length; i++) {
				var f = body.Force;
				Collides(body, ref corners[i], ref plane, method, elapsed);
			}
		}
	}

}
