using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.Serialization;
using System.Runtime.Serialization.Formatters.Binary;
using System.Xml.Linq;

namespace KinematicsBase
{
    [Serializable]
    public class Kinematics
    {
        #region Fields

        public string Id { get; set; }
        public string ModelId { get; set; }

        public string ModelName { get; set; }

        public string OpName { get; set; }

        public DeviceType DeviceType { get; set; }

        public PositionQuaternionWithID Base { get; set; }

        public PositionQuaternionWithID Flange { get; set; }

        public PositionQuaternionWithID Tool { get; set; }

        public List<Link> LinkList { get; set; } = new List<Link>();

        public List<Joint> JointList { get; set; } = new List<Joint>();
        /// <summary>
        /// 按父子级排序
        /// </summary>
        public List<Joint> JointListOrderBy { get; set; } = new List<Joint>();


        /// <summary>
        /// 创建Joint时,自动创建两个姿态。Joint的最小值作为HOME姿态，最大值作为姿态POSE
        /// </summary>
        public List<Pose> PoseList = new List<Pose>();
        public List<Sensor> SensorList = new List<Sensor>();


        #endregion

        #region Constructor
        public Kinematics()
        {


        }



        #endregion

        #region Link

        /// <summary>
        /// 根据Id删除Link
        /// 
        /// </summary>
        /// <param name="id"></param>
        public void RemoveLinkById(string id)
        {
            Link currentLink = null;
            foreach (var link in LinkList)
            {
                if (link.Id == id)
                {
                    currentLink = link;
                    break;
                }
            }

            LinkList.Remove(currentLink);
        }



        /// <summary>
        /// 根据Id（内部ID，非模型Id）获取Link name，和模型Id
        /// </summary>
        /// <param name="id"></param>
        /// <returns></returns>
        public string[] GetLinkNameModelIdById(string id)
        {
            string[] str = new[] { "Nan", "Nan" };
            foreach (var link in LinkList)
            {
                if (link.Id == id)
                {
                    str[0] = link.Name;
                    str[1] = link.LinkElements[0].Id;
                    return str;
                }
            }

            return str;
        }


        /// <summary>
        /// 根据ID获取Link
        /// </summary>
        /// <param name="linkList"></param>
        /// <param name="id"></param>
        /// <returns></returns>
        public Link GetLinkById(string id)
        {
            foreach (var link in LinkList)
            {
                if (link.Id == id)
                {
                    return link;
                }
            }

            return null;
        }

        public List<string> GetLastLinkId()
        {
            var res = new List<string>();

            //var lastLinkId = "";
            //var lastLinkName = "";

            //foreach (var link in LinkList)
            //{
            //    foreach (var joint in JointList)
            //    {
            //        if (link.LinkElements[0].Id == joint.ParentLinkId)
            //        {
            //            continue;
            //        }

            //        lastLinkId = link.LinkElements[0].Id;
            //        lastLinkName = link.LinkElements[0].Name;
            //    }
            //}


            var link = LinkList.Find(o=>o.Name.Contains("_6"));
            var lastLinkId = link.LinkElements[0].Id;
            var lastLinkName = link.LinkElements[0].Name;

            res.Add(lastLinkId);
            res.Add(lastLinkName);
            return res;
        }






        #endregion
        #region Clone
        public static List<T> Clone<T>(object List)
        {
            using (Stream objectStream = new MemoryStream())
            {
                IFormatter formatter = new BinaryFormatter();
                formatter.Serialize(objectStream, List);
                objectStream.Seek(0, SeekOrigin.Begin);
                return formatter.Deserialize(objectStream) as List<T>;
            }
        }
        #endregion

        #region SensorSetup
        public static void SensorSetup()
        {
            foreach (var k in PP.ReturnKinematicsList())
            {
                foreach (var sensor in k.SensorList)
                {
                    sensor.Setup();
                }
            }
        }
        #endregion
        #region  Joint
        public static void SetJointValueCurrent(string modelId, string id, double val)
        {
            // Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
            k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val;
            Joint.DataChange(k.ModelId, id, val);
        }

        //public static void SetJointValueCurrentPosition(string modelId, string id, double val,double valePosition)
        //{
        //    Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
        //    k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val+ valePosition;
        //    Joint.DataChange(k.ModelId, id, val);
        //}
        //public void SetJointValueCurrent(string id, double val)
        //{
        //    //Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
        //    this.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val;
        //}
        public static void SetJointListValueCurrent(string modelId, Pose pose)
        {
            // Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
            foreach (var item in k.JointList)
            {
                item.ValueCurrent = pose.PoseInfo[item.Id];

                Joint.DataChange(k.ModelId, item.Id, Convert.ToSingle(item.ValueCurrent));
            }
        }
        //public  void SetJointListValueCurrent(Pose pose)
        //{
        //    foreach (var item in this.JointList)
        //    {
        //        item.Value = pose.PoseInfo[item.Id];
        //    }
        //}

        public static string GoeModleIdByPoseId(string poseId)
        {
            foreach (var k in PP.ReturnKinematicsList())
            {
                var pose = k.PoseList.FirstOrDefault(t => t.Id == poseId);
                if (pose != null)
                {
                    return k.ModelId;
                }
            }
            return null;
        }

        public static double GetJointValueCurrent(string modelId, string id)
        {
            //Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
            return k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent;
        }
        public static Pose GetPoseByPoseId(string poseId)
        {
            List<Pose> PoseListView = new List<Pose>();
            foreach (var item in PP.ReturnKinematicsList())
            {
                PoseListView.AddRange(item.PoseList);
            }
            return PoseListView.FirstOrDefault(t => t.Id == poseId);
        }


        public static List<Joint> GetJointListView()
        {
            List<Joint> JointListView = new List<Joint>();
            foreach (var item in PP.ReturnKinematicsList())
            {
                JointListView.AddRange(item.JointList);
            }
            return JointListView;
        }
        public static List<Sensor> GetSensorListView()
        {
            List<Sensor> SensorListView = new List<Sensor>();
            foreach (var item in PP.ReturnKinematicsList())
            {
                SensorListView.AddRange(item.SensorList);
            }
            return SensorListView;
        }
        //SensorListView

        //JointListView
        public static Pose GetPoseByModeIdAndPoseId(string modelId, string poseId)
        {
            // return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).PoseList.FirstOrDefault(t => t.Id == poseId);
            return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.PoseList.FirstOrDefault(t => t.Id == poseId);
        }
        public static string GetModelNameByModelId(string modelId)
        {
            // Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
            return k.ModelName;
        }
        public static List<Joint> GetJointListByModelId(string modelId)
        {
            // return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).JointList;
            return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.JointList;
        }
        public static Joint GetJointByModelIdAndJointId(string modelId, string jointId)
        {
            // return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).JointList.FirstOrDefault(t => t.Id == jointId);
            return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.JointList.FirstOrDefault(t => t.Id == jointId);
        }
        public static Kinematics GetKinematicsByModelId(string modelId)
        {
            // return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            return PP.ModelCodeFindModelAttribute(modelId).KinematicsData;

        }
        //Kinematics.SetJointValueCurrent(modelId, jId, valuCurrent);
        /// <summary>
        /// 根据Id删除joint
        /// </summary>
        /// <param name="id"></param>
        public void RemeveJointById(string id)
        {
            Joint currentJoint = null;
            foreach (var joint in JointList)
            {
                if (joint.Id == id)
                {
                    currentJoint = joint;
                    break;
                }
            }

            JointList.Remove(currentJoint);

        }

        ///// <summary>
        ///// 根据ID获取Joint
        ///// </summary>
        ///// <param name="jointList"></param>
        ///// <param name="id"></param>
        ///// <returns></returns>
        //public Joint GetJointById(string id)
        //{
        //    foreach (var joint in JointList)
        //    {
        //        if (joint.Id == id)
        //        {
        //            return joint;
        //        }
        //    }

        //    return null;
        //}



        //topId = 0;
        //    var list = GetSonNode(topId, nodeList);

        //    foreach (var item in list)
        //    {
        //        Console.WriteLine("id = "+item.Id+ "/nParentId="+item.ParentId+ "/nMame = " +item.Name);
        //    }
        /// <summary>
        /// Joint 按父节点到子节点递归排序
        /// </summary>
        /// <param name="id"></param>
        /// <param name="nodeList"></param>
        /// <returns></returns>
        private List<Joint> GetSonNode(string ChildLinkId, List<Joint> nodeList)
        {


            var query = nodeList.Where(c => c.ParentLinkId == ChildLinkId);

            return query.ToList().Concat(query.ToList().SelectMany(t => GetSonNode(t.ChildLinkId, nodeList))).ToList();
        }
        private string GetParents(string ChildLinkId, List<Joint> list)
        {
            List<Joint> result = list.Where(x => x.ChildLinkId == ChildLinkId).ToList();
            if (result.Count > 0 && result[0].ParentLinkId != "" && result[0].ParentLinkId.ToString() != "")
                return GetParents(result[0].ParentLinkId, list);
            else
                return ChildLinkId;
        }

        /// <summary>
        /// Joint 按父节点到子节点递归排序
        /// </summary>
        /// <returns></returns>
        public List<Joint> GetSonNode()
        {
            List<Joint> nodeList = JointList;
            if (nodeList.Count < 1) return null;
            string ChildLinkId_init = nodeList[0].ChildLinkId;
            string ChildLinkId = GetParents(ChildLinkId_init, nodeList);
            List<Joint> result;
            result = GetSonNode(ChildLinkId, nodeList);

            return result;
        }
        /// <summary>
        /// 关节联动时
        /// 计算每个关节的值
        /// </summary>
        /// <param name="nodeList"></param>
        /// <param name="val"></param>
        public string JointListValueCurrent(double val)
        {
            JointListOrderBy = GetSonNode();

            List<Joint> nodeList;
            nodeList = JointListOrderBy;
            JArray jarrayObj = new JArray();

            Dictionary<string, double> dicJointListValueCurrent = new Dictionary<string, double>();

            dicJointListValueCurrent.Add(nodeList[0].ParentLinkId, val);
            for (int i = 0; i < nodeList.Count; i++)
            {
                nodeList[i].ValueCurrent = dicJointListValueCurrent[nodeList[i].ParentLinkId] * nodeList[i].ValueFactor;
                if (!dicJointListValueCurrent.ContainsKey(nodeList[i].ChildLinkId))
                {
                    dicJointListValueCurrent.Add(nodeList[i].ChildLinkId, nodeList[i].ValueCurrent);
                }
            }
            foreach (var joint in nodeList)
            {
                var jObj = new JObject();
                jObj["jId"] = joint.Id;
                jObj["jValue"] = joint.ValueCurrent + Joint.GetJointPosition(joint); ;
                jarrayObj.Add(jObj);

                SetJointValueCurrent(this.ModelId, joint.Id, joint.ValueCurrent);
                //Joint.DataChange(this.ModelId, joint.Id, joint.ValueCurrent);

            }
            var jsonData = new JObject(
                                            new JProperty("ModelId", this.ModelId),
                                            new JProperty("MotionType", "3"),
                                            new JProperty("MotionInfo", jarrayObj)
                                        );
            var jsonStr = jsonData.ToString().Replace("\n", "").Replace("\r", "").Replace(" ", "");
            string message = "|SendMotionInfo|" + jsonStr;
            return message;
        }
        public List<C_ObjStates> JointListValueCurrentNew(double val)
        {
            List<C_ObjStates> ObjStatesList = new List<C_ObjStates>();
            List<C_ObjStates> objStates = new List<C_ObjStates>();
            JointListOrderBy = GetSonNode();

            List<Joint> nodeList;
            nodeList = JointListOrderBy;
            JArray jarrayObj = new JArray();

            Dictionary<string, double> dicJointListValueCurrent = new Dictionary<string, double>();

            dicJointListValueCurrent.Add(nodeList[0].ParentLinkId, val);
            for (int i = 0; i < nodeList.Count; i++)
            {
                nodeList[i].ValueCurrent = dicJointListValueCurrent[nodeList[i].ParentLinkId] * nodeList[i].ValueFactor;
                if (!dicJointListValueCurrent.ContainsKey(nodeList[i].ChildLinkId))
                {
                    dicJointListValueCurrent.Add(nodeList[i].ChildLinkId, nodeList[i].ValueCurrent);
                }
            }
            foreach (var joint in nodeList)
            {
                var jObj = new JObject();
                jObj["jId"] = joint.Id;
                jObj["jValue"] = joint.ValueCurrent + Joint.GetJointPosition(joint); ;
                jarrayObj.Add(jObj);

                SetJointValueCurrent(this.ModelId, joint.Id, joint.ValueCurrent);

                objStates = new OPERATION_FramesList().GetObjStates(this.ModelId, joint.Id, joint.ValueCurrent);
                foreach (var item in objStates)
                {
                    ObjStatesList.Add(item);
                }
                //Joint.DataChange(this.ModelId, joint.Id, joint.ValueCurrent);

            }

            return ObjStatesList;
        }


        #endregion

        #region KinematicsList

        /// <summary>
        /// 根据设备代码判断设备是否存在
        /// </summary>
        /// <param name="modelId"></param>
        /// <returns></returns>
        public static Kinematics KinematicsListFindByModelId(string modelId)
        {
            //  var k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
            var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
            return k;
        }


        /// <summary>
        /// 设备存在，更新；不存在增加
        /// </summary>
        /// <param name="k"></param>
        /// <returns></returns>
        public static void KinematicsListAddAndUpDate(Kinematics k)
        {
            // var objK = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == k.ModelId);
            var objK = PP.ModelCodeFindModelAttribute(k.ModelId).KinematicsData;
            if (objK == null)
            {
                //  DataBaseJson.dataBaseJson.KinematicsList.Add(k);
                foreach (var item in k.SensorList)
                {

                    switch (item.SensorType)
                    {
                        case SensorType.IsRun:
                            Joint.OnDataChangeJointIsRun -= item.Joint_OnDataChangeJointIsRun;
                            Joint.OnDataChangeJointIsRun += item.Joint_OnDataChangeJointIsRun;
                            break;
                        default:
                            Joint.OnDataChange -= item.Joint_OnDataChange;
                            Joint.OnDataChange += item.Joint_OnDataChange;
                            break;
                    }

                }
            }
            else
            {
                foreach (var item in k.SensorList)
                {

                    switch (item.SensorType)
                    {
                        case SensorType.IsRun:
                            Joint.OnDataChangeJointIsRun -= item.Joint_OnDataChangeJointIsRun;
                            Joint.OnDataChangeJointIsRun += item.Joint_OnDataChangeJointIsRun;
                            break;
                        default:
                            Joint.OnDataChange -= item.Joint_OnDataChange;
                            Joint.OnDataChange += item.Joint_OnDataChange;
                            break;
                    }

                }

                objK = k;
            }

            // Event.KinematicsDataChange(k);
        }



        private double GetJointPosition(Kinematics k, string jointId)
        {
            double val = -1;

            var item = k.JointList.FirstOrDefault(t => t.Id == jointId);
            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;
            }
            return val;
        }

        ///// <summary>
        ///// 根据设备代码删除设备信息
        ///// </summary>
        ///// <param name="modelId"></param>
        ///// <returns></returns>
        //public static bool KinematicsListRemoveByModelId(string modelId)
        //{
        //    var k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
        //    if (k != null)
        //    {
        //        DataBaseJson.dataBaseJson.KinematicsList.Remove(k);
        //        return true;
        //    }
        //    return false;
        //}


        #endregion



    }
}