using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;

namespace KinematicsBase
{
    #region BaseClass

    [Serializable]
    public enum UnitType
    {
        mm,
        cm,
        m
    }
    [Serializable]
    public enum LimitType
    {
        Constant,

        NoLimits
        //,Variable
    }

    public enum JointType
    {
        /// <summary>
        /// 旋转
        /// </summary>
        Revolute,
        /// <summary>
        /// 滑移
        /// </summary>
        Prismatic
    }
    [Serializable]
    public class JointMaxValues
    {
        public JointMaxValues()
        {
        }

        public JointMaxValues(double speed, double acceleration)
        {
            Speed = speed;
            Acceleration = acceleration;
        }
        public double SpeedFromJointLimit(JointLimit jointLimit)
        {
            if (jointLimit.Interval <= 0.000000001)
            {
                Speed = 0;
            }
            else
            {
                Speed = (jointLimit.HighLimit - jointLimit.LowLimit) / (jointLimit.Interval / 1000f);
            }
            return Speed;
        }

        public double Speed { get; set; }

        public double Acceleration { get; set; }
    }
    [Serializable]
    public class JointLimit
    {
        public JointLimit()
        {
        }

        /// <summary>
        /// 创建JointLimit类构造方法 
        /// </summary>
        /// <param name="limitType">LimitType. </param>
        /// <param name="lowLimit">最小极限 double </param>
        /// <param name="highLimit">最大极限 double</param>
        public JointLimit(LimitType limitType, double lowLimit, double highLimit)
        {
            this.LimitType = limitType;
            this.LowLimit = lowLimit;
            this.HighLimit = highLimit;
        }
        public JointLimit(LimitType limitType, double lowLimit, double highLimit, int interval)
        {
            this.LimitType = limitType;
            this.LowLimit = lowLimit;
            this.HighLimit = highLimit;
            this.Interval = interval;

        }

        public LimitType LimitType { get; set; }

        public double LowLimit { get; set; }

        public double HighLimit { get; set; }
        /// <summary>
        /// 时间优先时候的时间，单位毫秒
        /// </summary>
        public int Interval;
    }

    [Serializable]
    public class Joint
    {
        /// <summary>
        /// ModeId JointId value
        /// </summary>
        public static event Action<string, string, object> OnDataChange = null;
        public static event Action<string, string, bool> OnDataChangeJointIsRun = null;
        //public static event Action<Joint> UpdateJoint;

        public static void DataChange(string modeId, string id, object val)
        {
            if (OnDataChange != null)
            {
                OnDataChange.Invoke(modeId, id, val);
            }
        }

        public static void DataChangeJointIsRun(string modeId, string id, bool val)
        {
            if (OnDataChangeJointIsRun != null)
            {
                OnDataChangeJointIsRun.Invoke(modeId, id, val);
            }
        }
        public Joint()
        {
            GraphPosition = new System.Drawing.Point[2];
        }


        public Joint(string id, string name, bool isDriving = true)
        {
            this.Id = id;
            this.Name = name;
            this.IsDriving = isDriving;
            //this.Valuefactor = 1;
        }
        public System.Drawing.Point[] GraphPosition { set; get; }

        public string Id { get; set; }

        public string Name { get; set; }
        public string ModelId { get; set; }
        /// <summary>
        /// 驱动轴
        /// 联动时的主运动
        /// </summary>
        public bool IsDriving { get; set; }

        public string ParentLinkId { get; set; }

        public string ChildLinkId { get; set; }
        /// <summary>
        /// 父模型代码
        /// </summary>
        public string PId { get; set; }
        /// <summary>
        /// 子模型代码
        /// </summary>
        public string CId { get; set; }


        public Axis Axis { get; set; }

        public JointType JointType { get; set; }

        public bool LockJoint { get; set; }

        public JointLimit JointLimit { get; set; }

        public JointMaxValues JointMaxValue { get; set; }
        /// <summary>
        /// 模型初始值
        /// </summary>
        public double Value { get; set; }

        public bool FindJointById(Joint joint)
        {
            return Id == joint.Id;
        }

        /// <summary>
        /// 输入参数
        /// </summary>
        public double ValueInput { get; set; }
        /// <summary>
        /// 倍率  1
        /// </summary>
        public double ValueFactor { get; set; } = 1;
        /// <summary>
        /// 模型当前值
        /// </summary>
        public double ValueCurrent { get; set; }
        public string Script;
        public bool IsScript;

        public bool IsOnlyHomePose;


        public static double GetJointPosition(Joint joint)
        {
            double val = -1;
            switch (joint.JointType)
            {
                case JointType.Prismatic:
                    switch (joint.Axis.AxisFlag)
                    {
                        case AxisFlag.X:
                            val = Convert.ToSingle(joint.Axis.Loaction.Position.X);
                            break;
                        case AxisFlag.Y:
                            val = Convert.ToSingle(joint.Axis.Loaction.Position.Y);
                            break;
                        case AxisFlag.Z:
                            val = Convert.ToSingle(joint.Axis.Loaction.Position.Z);
                            break;
                    }
                    break;
                //一、绕定轴X-Y-Z旋转（RPY角）
                //二、绕动轴Z-Y-X旋转（Euler角）
                case JointType.Revolute:
                    switch (joint.Axis.AxisFlag)
                    {
                        case AxisFlag.X:
                            val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.R);
                            break;
                        case AxisFlag.Y:
                            val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.P);
                            break;
                        case AxisFlag.Z:
                            val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.Y);
                            break;
                    }
                    break;
            }
            return val;
        }
        /// <summary>
        /// 计算位姿变换
        /// </summary>
        /// <param name="initCoordinate">初始坐标doule[7]</param>
        /// <param name="along">沿着某个轴转，X/Y/Z/RX/RY/RZ</param>
        /// <param name="step">移动步长，转动单位弧度，移动单位与输入一致</param>
        /// <returns>目标坐标doule[7]</returns>
        public PositionQuaternion TransformCoordinates(Joint joint, double step)
        {
            string along = GetJointAlong(joint);
            double[] initCoordinate = new double[7];
            initCoordinate[0] = joint.Axis.LoactionQuaternion.Position.X;
            initCoordinate[1] = joint.Axis.LoactionQuaternion.Position.Y;
            initCoordinate[2] = joint.Axis.LoactionQuaternion.Position.Z;
            initCoordinate[3] = joint.Axis.LoactionQuaternion.Quaternion.X;
            initCoordinate[4] = joint.Axis.LoactionQuaternion.Quaternion.Y;
            initCoordinate[5] = joint.Axis.LoactionQuaternion.Quaternion.Z;
            initCoordinate[6] = joint.Axis.LoactionQuaternion.Quaternion.W;


            return TransformCoordinates(initCoordinate, along, step, joint.JointType);


        }

        public static string GetJointAlong(Joint joint)
        {
            string along = "X";
            switch (joint.JointType)
            {
                case JointType.Prismatic:
                    switch (joint.Axis.AxisFlag)
                    {
                        case AxisFlag.X:
                            along = "X";
                            break;
                        case AxisFlag.Y:
                            along = "Y";
                            break;
                        case AxisFlag.Z:
                            along = "Z";
                            break;
                    }
                    break;
                //一、绕定轴X-Y-Z旋转（RPY角）
                //二、绕动轴Z-Y-X旋转（Euler角）
                case JointType.Revolute:
                    switch (joint.Axis.AxisFlag)
                    {
                        case AxisFlag.X:
                            along = "RX";
                            break;
                        case AxisFlag.Y:
                            along = "RY";
                            break;
                        case AxisFlag.Z:
                            along = "RZ";
                            break;
                    }
                    break;
            }
            return along;
        }

        /// <summary>
        /// 计算位姿变换
        /// </summary>
        /// <param name="initCoordinate">初始坐标doule[7]</param>
        /// <param name="along">沿着某个轴转，X/Y/Z/RX/RY/RZ</param>
        /// <param name="step">移动步长，转动单位弧度，移动单位与输入一致</param>
        /// <returns>目标坐标doule[7]</returns>
        static public PositionQuaternion TransformCoordinates(double[] initCoordinate, string along, double step, JointType jointType)
        {
            PositionQuaternion PoseLoactionQuaternion = new PositionQuaternion();
            // 存储转换结果
            //double[] res = new double[7];
            if (jointType == JointType.Revolute)
            {
                step = step * Math.PI / 180;
            }
            var stepFloat = Convert.ToSingle(step);

            // 初始姿态
            var initPos = new System.Numerics.Vector3(Convert.ToSingle(initCoordinate[0]), Convert.ToSingle(initCoordinate[1]), Convert.ToSingle(initCoordinate[2]));
            var initQua = new System.Numerics.Quaternion(Convert.ToSingle(initCoordinate[3]), Convert.ToSingle(initCoordinate[4]), Convert.ToSingle(initCoordinate[5]), Convert.ToSingle(initCoordinate[6]));
            var resPos = new System.Numerics.Vector3();
            var resQua = new System.Numerics.Quaternion();
            resPos = initPos;

            // 转动或移动
            switch (along.ToUpper())
            {
                case "X":
                    {
                        resPos.X += stepFloat;
                        resQua = initQua;
                        break;
                    }
                case "Y":
                    {
                        resPos.Y += stepFloat;
                        resQua = initQua;
                        break;
                    }
                case "Z":
                    {
                        resPos.Z += stepFloat;
                        resQua = initQua;
                        break;
                    }
                case "RX":
                    {
                        resPos = initPos;
                        var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), stepFloat);
                        resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
                        break;
                    }
                case "RY":
                    {
                        resPos = initPos;
                        var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), stepFloat);
                        resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
                        break;
                    }
                case "RZ":
                    {
                        resPos = initPos;
                        var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), stepFloat);
                        resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
                        break;
                    }
            }

            //res[0] = resPos.X/1000;
            //res[1] = resPos.Y/1000;
            //res[2] = resPos.Z/1000;
            //res[3] = resQua.X;
            //res[4] = resQua.Y;
            //res[5] = resQua.Z;
            //res[6] = resQua.W;
            PoseLoactionQuaternion.Position.X = resPos.X / 1000;
            PoseLoactionQuaternion.Position.Y = resPos.Y / 1000;
            PoseLoactionQuaternion.Position.Z = resPos.Z / 1000;
            PoseLoactionQuaternion.Quaternion.X = resQua.X;
            PoseLoactionQuaternion.Quaternion.Y = resQua.Y;
            PoseLoactionQuaternion.Quaternion.Z = resQua.Z;
            PoseLoactionQuaternion.Quaternion.W = resQua.W;
            return PoseLoactionQuaternion;
        }

        #region Method
        //public static void JointListRemove(List<Joint> JointList,List<Joint>JointListOld)
        //{
        //    if (JointListOld == null) return;
        //    foreach(var item in JointListOld)
        //    {
        //        int index = JointList.FindIndex(t => t.Id == item.Id);
        //        if (index >= 0)
        //        {
        //            JointList.RemoveAt(index);
        //        }
        //    }
        //}
        ///// <summary>
        ///// 将设备JointList加入系统列表
        ///// </summary>
        ///// <param name="JointList"></param>
        ///// <param name="jointList"></param>
        //public static void JointListViewAddAndUpdate(List<Joint> JointList, List<Joint> jointList)
        //{
        //    int index;
        //    foreach (var item in jointList)
        //    {
        //        index = JointList.FindIndex(t => t.Id == item.Id);
        //        if (index > -1)
        //        {
        //            JointList.RemoveAt(index);
        //            item.ValueCurrent = item.Value;
        //            JointList.Add(item);
        //            Joint.DataChange(item.Id, item.Value);
        //        }
        //        else
        //        {
        //            item.ValueCurrent = item.Value;
        //            JointList.Add(item);
        //            Joint.DataChange(item.Id, item.Value);
        //        }
        //    }
        //}


        #endregion
        ///// <summary>
        ///// 当前实际位置
        ///// </summary>
        //public PositionFixedAngle PositionCurrent { get; set; }
        ///// <summary>
        ///// 根据输入值和倍率计算模型当前位置
        ///// </summary>
        ///// <param name="ValueInput"></param>
        ///// <returns></returns>
        //public PositionFixedAngle GetPositionCurrent(double ValueInput)
        //{

        //    Value = Value * ValueFactor;
        //    this.ValueInput = ValueInput;
        //    PositionCurrent.Position.X = Axis.Loaction.Position.X + positionNormalMatrix.x * Value;
        //    PositionCurrent.Position.Y = Axis.Loaction.Position.Y + positionNormalMatrix.y * Value;
        //    PositionCurrent.Position.Z = Axis.Loaction.Position.Z + positionNormalMatrix.z * Value;
        //    PositionCurrent.FixedAngle.R = Axis.Loaction.FixedAngle.R + positionNormalMatrix.rx * Value;
        //    PositionCurrent.FixedAngle.P = Axis.Loaction.FixedAngle.P + positionNormalMatrix.ry * Value;
        //    PositionCurrent.FixedAngle.Y = Axis.Loaction.FixedAngle.Y + positionNormalMatrix.rz * Value;

        //    return PositionCurrent;
        //}
        ///// <summary>
        ///// 根据界面数据，给位置变换标准矩阵赋值
        ///// </summary>
        ///// <returns></returns>
        //public PositionNormalMatrix SetPositionNormalMatrix()
        //{
        //    positionNormalMatrix.x =0;
        //    positionNormalMatrix.y =0;
        //    positionNormalMatrix.z = 0;
        //    positionNormalMatrix.rx = 0;
        //    positionNormalMatrix.ry = 0;
        //    positionNormalMatrix.rz = 0;

        //    switch (JointType)
        //    {
        //        case JointType.Revolute:
        //            switch (Axis.AxisFlag)
        //            {
        //                case AxisFlag.X:
        //                    positionNormalMatrix.rx = 1;
        //                    break;
        //                case AxisFlag.Y:
        //                    positionNormalMatrix.ry = 1;
        //                    break;
        //                case AxisFlag.Z:
        //                    positionNormalMatrix.rz = 1;
        //                    break;
        //            }
        //            break;
        //        case JointType.Prismatic:
        //            switch (Axis.AxisFlag)
        //            {
        //                case AxisFlag.X:
        //                    positionNormalMatrix.x = 1;
        //                    break;
        //                case AxisFlag.Y:
        //                    positionNormalMatrix.y = 1;
        //                    break;
        //                case AxisFlag.Z:
        //                    positionNormalMatrix.z = 1;
        //                    break;
        //            }
        //            break;

        //    }
        //    switch (Axis.Flip)
        //    {
        //        case 1:
        //            positionNormalMatrix.x = -positionNormalMatrix.x;
        //            positionNormalMatrix.y = -positionNormalMatrix.y;
        //            positionNormalMatrix.z = -positionNormalMatrix.z;
        //            positionNormalMatrix.rx = -positionNormalMatrix.rx;
        //            positionNormalMatrix.ry = -positionNormalMatrix.rx;
        //            positionNormalMatrix.rz = -positionNormalMatrix.rx;
        //            break;
        //    }
        //    return positionNormalMatrix;
        //}
    }



    #endregion
}