using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
namespace KinematicsBase
{
    [Serializable]
    public class HomeOpen
    {
        public static string HOME = "HOME";
        public static string OPEN = "OPEN";
        public static string Position = "Position";
        public static string Current = "Current";
    }

    [Serializable]
    public class Pose
    {
        public static event Action<string, Pose> OnDataChange = null;

        public Pose()
        {
            this.Id = Guid.NewGuid().ToString("D").ToUpper();
        }
        public Pose(Kinematics k)
        {
            this.Id = Guid.NewGuid().ToString("D").ToUpper();
            JointPoseListCreate(k);
        }
        public Pose(string id, string name)
        {
            this.Id = id;
            this.Name = name;
        }

        public string Id { get; set; }

        public string Name { get; set; }
        public string ModelId { get; set; }
        /// <summary>
        /// ture 独立运行
        /// </summary>
        public bool IsJointByself { get; set; }
        public List<JointPose> JointPoseList { get; set; }
        /// <summary>
        /// 关节是否联动
        /// </summary>
        public bool JointGearing { get; set; }
        /// <summary>
        /// 时间优先时候的时间，单位毫秒
        /// </summary>
        public int Interval { get; set; }
        /// <summary>
        /// 时间优先或速度优先
        /// </summary>
        public MoveToPosePriorType m_MoveToPosePriorType { get; set; }
        /// <summary>
        /// string JointId double value
        /// </summary>
        public Dictionary<string, double> PoseInfo { get; set; } = new Dictionary<string, double>();
        #region Method
        public static void DataChange(string ModelId, Pose pose)
        {
            if (OnDataChange != null)
            {
                OnDataChange.Invoke(ModelId, pose);
            }
        }
        private void JointPoseListCreate(Kinematics k)
        {
            this.JointPoseList = new List<JointPose>();
            var JointListOrderByName = k.JointList.OrderBy(o => o.Name).ToList();

            foreach (var item in JointListOrderByName)
            {
                this.JointPoseList.Add(new JointPose(item.Id, item.Name));
            }
        }
        //public static void PoseListViewCreate(List<Pose> PoseViewList, List<Joint> joints)
        //{
        //    Pose pose;
        //    double val=-1;
        //    pose = new Pose();
        //    pose.Name = "JointPosition";
        //    pose.Id = Guid.NewGuid().ToString("D").ToUpper();
        //    pose.Interval = 3000;
        //    //删除相同的JointList
        //    PoseListViewRemove(PoseViewList, joints);
        //    foreach (var item in joints)
        //    {
        //        switch (item.JointType)
        //        {
        //            case JointType.Prismatic:
        //                switch (item.Axis.AxisFlag)
        //                {
        //                    case AxisFlag.X:
        //                        val = Convert.ToSingle( item.Axis.Loaction.Position.X);
        //                        break;
        //                    case AxisFlag.Y:
        //                        val = Convert.ToSingle( item.Axis.Loaction.Position.Y);
        //                        break;
        //                    case AxisFlag.Z:
        //                        val = Convert.ToSingle( item.Axis.Loaction.Position.Z);
        //                        break;
        //                }
        //                break;
        //            //一、绕定轴X-Y-Z旋转（RPY角）
        //            //二、绕动轴Z-Y-X旋转（Euler角）
        //            case JointType.Revolute:
        //                switch (item.Axis.AxisFlag)
        //                {
        //                    case AxisFlag.X:
        //                        val = Convert.ToSingle( item.Axis.Loaction.FixedAngle.R);
        //                        break;
        //                    case AxisFlag.Y:
        //                        val = Convert.ToSingle( item.Axis.Loaction.FixedAngle.P);
        //                        break;
        //                    case AxisFlag.Z:
        //                        val = Convert.ToSingle( item.Axis.Loaction.FixedAngle.Y);
        //                        break;
        //                }
        //                break;
        //        }
        //        pose.PoseInfo.Add(item.Id, val);
        //    }
        //    PoseViewList.Add(pose);

        //}
        //public static void PoseListViewRemove(List<Pose> PoseViewList, List<Joint> joints)
        //{
        //    Pose pose=null;
        //    foreach(var joint in joints)
        //    {
        //        foreach (var item in PoseViewList)
        //        {
        //            if (item.PoseInfo.ContainsKey(joint.Id))
        //            {
        //                pose = item;
        //                break;
        //            }
        //        }
        //    }
        //    if(pose != null)
        //    {
        //        PoseViewList.Remove(pose);
        //    }
        //}
        //public static void PoseCurrentViewListSetValue(List<Pose> PoseList, string id,double val)
        //{
        //    foreach(var poseItem in PoseList)
        //    {
        //        if (poseItem.PoseInfo.ContainsKey(id))
        //        {
        //            poseItem.PoseInfo[id] = val;
        //            break;
        //        }
        //    }
        //}

        public static void HomeAndOpenPositionCreate(Kinematics k)
        {
            Pose pose;
            //HOME
            pose = Pose.CreatePoseHome(k);
            Pose.PoseListAddAndUpdateByName(k.PoseList, pose);
            //OPEN
            pose = Pose.CreatePoseOpen(k);
            Pose.PoseListAddAndUpdateByName(k.PoseList, pose);
            ////Position
            //pose = Pose.PosePositionCreator(k);
            //Pose.PoseListAddAndUpdateByName( k.PoseList, pose);

        }
        public static Pose PosePositionCreator(Kinematics k)
        {
            Pose pose;
            double val = -1;

            pose = new Pose(k);
            pose.Name = "Position";
            pose.ModelId = k.ModelId;
            pose.Interval = 3000;
            pose.PoseInfo = new Dictionary<string, double>();
            foreach (var item in k.JointList)
            {
                var joint = k.JointList.FirstOrDefault(t => t.Id == item.Id);
                val = Joint.GetJointPosition(joint);
                pose.PoseInfo.Add(item.Id, val);
            }
            return pose;
        }

        //public static Pose PoseCurrentCreator(List<Joint> joints)
        //{
        //    Pose pose;
        //    double val = -1;

        //    pose = new Pose(k);
        //    pose.Name = "Current";
        //    pose.Id = Guid.NewGuid().ToString("D").ToUpper();
        //    pose.Interval = 3000;
        //    foreach (var item in joints)
        //    {
        //        switch (item.JointType)
        //        {
        //            case JointType.Prismatic:
        //                switch (item.Axis.AxisFlag)
        //                {
        //                    case AxisFlag.X:
        //                        val = Convert.ToSingle(item.Axis.Loaction.Position.X);
        //                        break;
        //                    case AxisFlag.Y:
        //                        val = Convert.ToSingle(item.Axis.Loaction.Position.Y);
        //                        break;
        //                    case AxisFlag.Z:
        //                        val = Convert.ToSingle(item.Axis.Loaction.Position.Z);
        //                        break;
        //                }
        //                break;
        //            //一、绕定轴X-Y-Z旋转（RPY角）
        //            //二、绕动轴Z-Y-X旋转（Euler角）
        //            case JointType.Revolute:
        //                switch (item.Axis.AxisFlag)
        //                {
        //                    case AxisFlag.X:
        //                        val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.R);
        //                        break;
        //                    case AxisFlag.Y:
        //                        val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.P);
        //                        break;
        //                    case AxisFlag.Z:
        //                        val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.Y);
        //                        break;
        //                }
        //                break;
        //        }
        //        pose.PoseInfo.Add(item.Id, val);
        //    }
        //    return pose;
        //}
        public static void PoseListAddAndUpdateByName(List<Pose> PoseList, Pose val)
        {
            var poseIndex = PoseList.FindIndex(t => t.Name == val.Name);
            if (poseIndex > -1)
            {
                PoseList[poseIndex] = val;
            }
            else
            {
                PoseList.Add(val);
            }
        }

        //public static void PoseListAddAndUpdate(List<Pose> PoseList, Pose val)
        //{
        //    var obj = PoseList.FirstOrDefault(t => t.Id == val.Id);
        //    if (obj != null)
        //    {
        //        val.Id = obj.Id;
        //        PoseList.Remove(obj);
        //        PoseList.Add(val);
        //    }
        //    else
        //    {
        //        PoseList.Add(val);
        //    }
        //}

        //public static  void PoseListRemoveById(List<Pose> PoseList, string id)
        //{
        //    var obj = PoseList.FirstOrDefault(t => t.Id == id);
        //    if (obj != null)
        //    {
        //        PoseList.Remove(obj);
        //    }
        //}
        public static Pose PoseListFindById(List<Pose> PoseList, string id)
        {
            return PoseList.FirstOrDefault(t => t.Id == id);
        }
        //public static void PoseListAddAndUpdate(List<Pose> ObjList, List<Pose> objList)
        //{
        //    List<Pose> objListBak = new List<Pose>(objList);
        //    foreach (var item in objList)
        //    {
        //        var pose = ObjList.FirstOrDefault(t => t.Id == item.Id);
        //        if (pose!=null)
        //        {
        //            Pose pValue = objListBak.FirstOrDefault(t => t.Id == item.Id);
        //            pValue.Id = pose.Id;
        //            ObjList.Remove(pose);
        //            ObjList.Add(pValue);
        //        }
        //        else
        //        {
        //            ObjList.Add(item);
        //        }
        //    }
        //}

        //public static void PoseListAddAndUpdateHomeAndPose(List<Joint> JointList, List<Pose> PostList)
        //{
        //    Pose pose;
        //    pose = CreatePoseHome(JointList);
        //    PoseListAddAndUpdateByName(PostList, pose);
        //    pose = CreatePoseOpen(JointList);
        //    PoseListAddAndUpdateByName(PostList, pose);
        //}
        /// <summary>
        /// 用 Joint最小值
        /// </summary>
        /// <param name="JointList"></param>
        /// <returns></returns>
        public static Pose CreatePoseHome(Kinematics k)
        {
            Pose pose;
            pose = new Pose(k);
            pose.Name = HomeOpen.HOME;
            pose.ModelId = k.ModelId;
            pose.Interval = 3000;
            pose.PoseInfo.Clear();
            foreach (Joint joint in k.JointList)
            {
                var list_Limit = new List<double>();
                list_Limit.Add(Math.Abs(joint.JointLimit.LowLimit));
                list_Limit.Add(Math.Abs(joint.JointLimit.HighLimit));

                pose.PoseInfo.Add(joint.Id, list_Limit.Min());
            }
            return pose;
        }
        /// <summary>
        /// 用 Joint最大值
        /// </summary>
        /// <param name="JointList"></param>
        /// <returns></returns>
        public static Pose CreatePoseOpen(Kinematics k)
        {
            Pose pose;
            pose = new Pose(k);
            pose.Name = HomeOpen.OPEN;
            pose.ModelId = k.ModelId;
            pose.Interval = 3000;
            pose.PoseInfo.Clear();
            foreach (Joint joint in k.JointList)
            {
                var list_Limit = new List<double>();
                list_Limit.Add(Math.Abs(joint.JointLimit.LowLimit));
                list_Limit.Add(Math.Abs(joint.JointLimit.HighLimit));

                pose.PoseInfo.Add(joint.Id, list_Limit.Max());
            }
            return pose;
        }

        #endregion
    }

}