#if XNA
using Microsoft.Xna.Framework;
#endif

namespace XELF.Framework {

	/// <summary>
	/// デュアルクォータニオン
	/// </summary>
	public partial struct DualQuaternion {
		/// <summary>
		/// 実部
		/// </summary>
		public Quaternion Real;
		/// <summary>
		/// 非実部
		/// </summary>
		public Quaternion Dual;
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// コンストラクタ
		/// </summary>
		/// <param name="rx"></param>
		/// <param name="ry"></param>
		/// <param name="rz"></param>
		/// <param name="rw"></param>
		/// <param name="dx"></param>
		/// <param name="dy"></param>
		/// <param name="dz"></param>
		/// <param name="dw"></param>
		public DualQuaternion(float rx, float ry, float rz, float rw, float dx, float dy, float dz, float dw) {
			Real = new Quaternion(rx, ry, rz, rw);
			Dual = new Quaternion(dx, dy, dz, dw);
		}
		/// <summary>
		/// コンストラクタ
		/// </summary>
		/// <param name="real"></param>
		/// <param name="dual"></param>
		public DualQuaternion(Quaternion real, Quaternion dual) {
			Real = real;
			Dual = dual;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 逆デュアルクォータニオン
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Invert(ref DualQuaternion a, out DualQuaternion result) {
			var rr = 1 / a.Real.LengthSquared();
			var rd = -2 * (a.Real.X * a.Dual.X + a.Real.Y * a.Dual.Y + a.Real.Z * a.Dual.Z + a.Real.W * a.Dual.W) * rr * rr;
			result = new DualQuaternion(
				-a.Real.X * rr,
				-a.Real.Y * rr,
				-a.Real.Z * rr,
				+a.Real.W * rr,
				-a.Dual.X * rr - a.Real.X * rd,
				-a.Dual.Y * rr - a.Real.Y * rd,
				-a.Dual.Z * rr - a.Real.Z * rd,
				+a.Dual.W * rr + a.Real.W * rd);
		}
		/// <summary>
		/// 逆デュアルクォータニオン
		/// </summary>
		/// <param name="a"></param>
		/// <returns></returns>
		public static DualQuaternion Invert(DualQuaternion a) {
			DualQuaternion result;
			Invert(ref a, out result);
			return result;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 文字列表現を得る。
		/// </summary>
		/// <returns></returns>
		public override string ToString() {
			return string.Format(System.Globalization.CultureInfo.InvariantCulture,
				"[{0}]+[{1}]", Real, Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 回転と位置からデュアルクォータニオン
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="position"></param>
		/// <returns></returns>
		public static DualQuaternion CreateFromRotationPosition(Quaternion rotation, Vector3 position) {
			DualQuaternion result;
			CreateFromRotationPosition(ref rotation, ref position, out result);
			return result;
		}
		/// <summary>
		/// 回転と位置からデュアルクォータニオン
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="position"></param>
		/// <param name="result"></param>
		public static void CreateFromRotationPosition(ref Quaternion rotation, ref Vector3 position,
			out DualQuaternion result) {
			result = new DualQuaternion(rotation, new Quaternion(
				0.5f * (position.X * rotation.W + position.Y * rotation.Z - position.Z * rotation.Y),
				0.5f * (-position.X * rotation.Z + position.Y * rotation.W + position.Z * rotation.X),
				0.5f * (position.X * rotation.Y - position.Y * rotation.X + position.Z * rotation.W),
				-0.5f * (position.X * rotation.X + position.Y * rotation.Y + position.Z * rotation.Z)));
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 和
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Add(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			Quaternion.Add(ref a.Real, ref b.Real, out result.Real);
			Quaternion.Add(ref a.Dual, ref b.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 差
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Subtract(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			Quaternion.Subtract(ref a.Real, ref b.Real, out result.Real);
			Quaternion.Subtract(ref a.Dual, ref b.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a">元のデュアルクォータニオン</param>
		/// <param name="scale">スカラー</param>
		/// <param name="result">結果</param>
		public static void Multiply(ref DualQuaternion a, float scale, out DualQuaternion result) {
			Quaternion.Multiply(ref a.Real, scale, out result.Real);
			Quaternion.Multiply(ref a.Dual, scale, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Multiply(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			result = new DualQuaternion(
				a.Real.W * b.Real.X + a.Real.X * b.Real.W + a.Real.Y * b.Real.Z - a.Real.Z * b.Real.Y,
				a.Real.W * b.Real.Y - a.Real.X * b.Real.Z + a.Real.Y * b.Real.W + a.Real.Z * b.Real.X,
				a.Real.W * b.Real.Z + a.Real.X * b.Real.Y - a.Real.Y * b.Real.X + a.Real.Z * b.Real.W,
				a.Real.W * b.Real.W - a.Real.X * b.Real.X - a.Real.Y * b.Real.Y - a.Real.Z * b.Real.Z,
				a.Real.W * b.Dual.X + a.Real.X * b.Dual.W + a.Real.Y * b.Dual.Z - a.Real.Z * b.Dual.Y
				+ a.Dual.W * b.Real.X + a.Dual.X * b.Real.W + a.Dual.Y * b.Real.Z - a.Dual.Z * b.Real.Y,
				a.Real.W * b.Dual.Y - a.Real.X * b.Dual.Z + a.Real.Y * b.Dual.W + a.Real.Z * b.Dual.X
				+ a.Dual.W * b.Real.Y - a.Dual.X * b.Real.Z + a.Dual.Y * b.Real.W + a.Dual.Z * b.Real.X,
				a.Real.W * b.Dual.Z + a.Real.X * b.Dual.Y - a.Real.Y * b.Dual.X + a.Real.Z * b.Dual.W
				+ a.Dual.W * b.Real.Z + a.Dual.X * b.Real.Y - a.Dual.Y * b.Real.X + a.Dual.Z * b.Real.W,
				a.Real.W * b.Dual.W - a.Real.X * b.Dual.X - a.Real.Y * b.Dual.Y - a.Real.Z * b.Dual.Z
				+ a.Dual.W * b.Real.W - a.Dual.X * b.Real.X - a.Dual.Y * b.Real.Y - a.Dual.Z * b.Real.Z);
		}
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns></returns>
		public static DualQuaternion Multiply(DualQuaternion a, DualQuaternion b) {
			DualQuaternion result;
			Multiply(ref a, ref b, out result);
			return result;
		}
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns></returns>
		public static DualQuaternion operator *(DualQuaternion a, DualQuaternion b) {
			DualQuaternion result;
			Multiply(ref a, ref b, out result);
			return result;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 長さの2乗
		/// </summary>
		/// <param name="real">実部</param>
		/// <param name="dual">非実部</param>
		public void LengthSquared(ref float real, ref float dual) {
			real = Real.LengthSquared();
			Quaternion.Dot(ref Real, ref Dual, out dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 符号反転
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Negate(ref DualQuaternion a, out DualQuaternion result) {
			Quaternion.Negate(ref a.Real, out result.Real);
			Quaternion.Negate(ref a.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 共役（乗算入れ替え用）
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void SwapConjugate(ref DualQuaternion a, out DualQuaternion result) {
			Quaternion.Conjugate(ref a.Real, out result.Real);
			Quaternion.Conjugate(ref a.Dual, out result.Dual);
		}

		/// <summary>
		/// 共役（座標変換用）
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void TransformConjugate(ref DualQuaternion a, out DualQuaternion result) {
			Quaternion.Conjugate(ref a.Real, out result.Real);
			result.Dual = new Quaternion(a.Dual.X, a.Dual.Y, a.Dual.Z, -a.Dual.W);
		}
	}

	public partial struct DualQuaternion {
		static readonly DualQuaternion identity = new DualQuaternion(0, 0, 0, 1, 0, 0, 0, 0);

		/// <summary>
		///	単位デュアルクォータニオン（乗法の単位元）
		/// </summary>
		public static DualQuaternion Identity { get { return identity; } }
	}

}