using System.Collections.Generic;
using MikuMikuDance.XNA.Model;
using Microsoft.Xna.Framework;
using MikuMikuDance.XNA.Model.ModelData;

namespace MikuMikuDance.XNA.Motion
{

    /// <summary>
    /// 回転制限クラス
    /// </summary>
    internal class RotationLimit
    {
        /// <summary>
        /// 最大回転
        /// </summary>
        /// <remarks>X回転、Y回転、Z回転制限</remarks>
        public float[] MaxRot { get; protected set; }
        /// <summary>
        /// 最小回転
        /// </summary>
        /// <remarks>X回転、Y回転、Z回転制限</remarks>
        public float[] MinRot { get; protected set; }

        public bool Mirror { get; set; }

        const float restitution = 0.5f;
        /// <summary>
        /// 既定のコンストラクタ
        /// </summary>
        public RotationLimit()
        {
            MaxRot = new float[3];
            MinRot = new float[3];
            for (int i = 0; i < 3; i++)
            {
                MaxRot[i] = MathHelper.Pi;
                MinRot[i] = -MathHelper.Pi;
            }
            Mirror = false;
        }
        /// <summary>
        /// 指定した角度をアジャストする
        /// </summary>
        /// <param name="value">回転角</param>
        /// <param name="index">回転軸</param>
        /// <returns>アジャスト済み角度</returns>
        public float Adjust(float value, int index)
        {
            if (MinRot[index] > MaxRot[index])
            {
                if (MinRot[index] > value && MaxRot[index] < value)
                {
                    if (value > (MinRot[index] + MaxRot[index]) / 2)
                    {
                        if (Mirror)
                            return MinRot[index] * (1 + restitution) - value * restitution;
                        else
                            return MinRot[index];
                    }
                    else
                    {
                        if (Mirror)
                            return MaxRot[index] * (1 + restitution) - value * restitution;
                        else
                            return MaxRot[index];
                    }
                }
                else
                    return value;
            }
            else
            {
                if (MaxRot[index] < value)
                {
                    if (Mirror)
                        return MaxRot[index] * (1 + restitution) - value * restitution;
                    else
                        return MaxRot[index];
                }
                else if (MinRot[index] > value)
                {
                    if (Mirror)
                        return MinRot[index] * (1 + restitution) - value * restitution;
                    else
                        return MinRot[index];
                }
                else
                    return value;
            }
        }
    }
    /// <summary>
    /// IKの稼働制限を指定するクラス
    /// </summary>
    internal class IKLimitation
    {

        /// <summary>
        /// 総合稼働軸制限一覧
        /// </summary>
        /// <remarks>ボーン名マッチング用の正規表現オブジェクトと許可回転軸(親ボーン基準)</remarks>
        public Dictionary<string, RotationLimit> TotalRotationLimits { get; protected set; }



        /// <summary>
        /// 既定のコンストラクタ
        /// </summary>
        public IKLimitation()
        {

            //総合稼働制限
            TotalRotationLimits = new Dictionary<string, RotationLimit>();
            RotationLimit limit;
            limit = new RotationLimit();
            limit.MaxRot[0] = MathHelper.PiOver2;
            limit.MinRot[0] = 0;
            limit.MinRot[1] = 0;
            limit.MaxRot[1] = 0;
            limit.MinRot[2] = 0;
            limit.MaxRot[2] = 0;
            TotalRotationLimits.Add("左ひざ", limit);
            limit = new RotationLimit();
            limit.MaxRot[0] = MathHelper.PiOver2;
            limit.MinRot[0] = 0;
            limit.MinRot[1] = 0;
            limit.MaxRot[1] = 0;
            limit.MinRot[2] = 0;
            limit.MaxRot[2] = 0;
            TotalRotationLimits.Add("右ひざ", limit);


        }

        bool CheckNaN(Vector3 input)
        {
            if (float.IsNaN(input.X))
                return true;
            if (float.IsNaN(input.Y))
                return true;
            if (float.IsNaN(input.Z))
                return true;
            return false;
        }
        /// <summary>
        /// ボーン全体の動きに可動制限をかけてボーンの姿勢を修正
        /// </summary>
        /// <param name="boneIndex">ボーン番号</param>
        /// <param name="manager">ボーンマネージャ</param>
        /// <returns>修正済みボーントランスフォーム</returns>
        public virtual QuatTransform AdjustTotalBoneMove(int boneIndex, MMDBoneManager manager)
        {
            bool flag = false;
            foreach (var i in TotalRotationLimits)
            {
                if (i.Key == manager[boneIndex].BoneData.Name)
                    flag = true;
            }
            if (!flag)
                return manager[boneIndex].BoneTransform;
            //まずは軸回転のマトリクスを生成する
            Matrix bonetrans = manager[boneIndex].BoneTransform.CreateMatrix();
            Matrix bindpose = manager[boneIndex].BoneData.BindPose.CreateMatrix();
            Matrix moveMat = bonetrans * Matrix.Invert(bindpose);

            //回転を取得
            Vector3 temp, trans;
            Quaternion rot;

            moveMat.Decompose(out temp, out rot, out trans);


            float YRot, XRot, ZRot;
            MMDMath.DecompositeQuaternion(rot, out YRot, out XRot, out ZRot);

            RotationLimit lim = TotalRotationLimits[manager[boneIndex].BoneData.Name];
            XRot = lim.Adjust(XRot, 0);
            YRot = lim.Adjust(YRot, 1);
            ZRot = lim.Adjust(ZRot, 2);

            return new QuatTransform(Quaternion.CreateFromYawPitchRoll(YRot, XRot, ZRot), trans) * manager[boneIndex].BoneData.BindPose;

        }
    }
}
