using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using URSoldering.Common;

namespace URSoldering.DeviceLibrary
{
    public class URRobotClient
    {
        public static bool IsSpitPackage = true  ;
        public delegate void SafetModePro(string mode);
        public static event SafetModePro SafetModeFun;
        public delegate void RobotModePro(string mode);
        public static event RobotModePro RobotModeFun;
        private static URTcpClient listenClient = null;  
        private static int Port = 30003;
        public static string  LastMoveCMD = "";
        public static void StartListen(string ip)
        {
            LastReviceDataTime = DateTime.Now;
            listenClient = new URTcpClient();
            listenClient.DefaultDataLength = 1108;
            listenClient.ReviceSleepMS = 5;
            listenClient.connect(ip, Port, HandleMessage);
        }
        public static void StopListen()
        {
            if (listenClient != null)
            {
                listenClient.close();
            }
        }
        public static bool IsConnected()
        {
            if (listenClient != null)
            {
                return listenClient.IsConnected();
            }return false;
        }
        public static string LogName
        {
            get { return "【UR端口：" + Port + "】"; }
        }
        private static double BitToDouble(byte[] data)
        {
            double f = BitConverter.ToDouble(data, 0);
            return f;
        }
        private static int BitToInt(byte[] data)
        {
            int f = BitConverter.ToInt32(data, 0);
            return f;
        }
        private static double  BitToFloat(string hexString)
        {
            uint num = uint.Parse(hexString, System.Globalization.NumberStyles.AllowHexSpecifier);
            byte[] floatVals = BitConverter.GetBytes(num);
            double f = BitConverter.ToDouble(floatVals, 0);
            return f;
        }
        private static string ByteToStr(byte[] reviceData)
        {
            string strFromat = "{0:X2}";
            string reviceMsg = "";
            foreach (byte data in reviceData)
            {
                reviceMsg = reviceMsg + " " + String.Format(strFromat, data);
            }return reviceMsg;
        }
        private static DateTime LastReviceDataTime = DateTime.Now;
        public static bool IsTimeOut()
        {
            TimeSpan span = DateTime.Now - LastReviceDataTime;
            if (span.TotalSeconds > 1)
            {
                return true;
            }return false;
        }
        private static void HandleMessage(byte[] reviceData)
        {
            if (LastMoveCMD.Equals("").Equals(false))
            {
                listenClient.sendLine(LastMoveCMD);
                LastMoveCMD = "";
            }
            if (!IsSpitPackage)
            {
                StopListen();
            }
            try
            { 
                byte[] msgSizeArray = reviceData.Skip(0).Take(4).ToArray();
                //byte[] timeArray = reviceData.Skip(4).Take(8).ToArray();
                //byte[] qTargetArray = reviceData.Skip(12).Take(48).ToArray();
                //byte[] qdTargetArray = reviceData.Skip(60).Take(48).ToArray();
                //byte[] qddArray = reviceData.Skip(108).Take(48).ToArray();
                //byte[] iTargetArray = reviceData.Skip(156).Take(48).ToArray();

                int messageSize = BitToInt(reviceData.Skip(0).Take(4).ToArray().Reverse<byte>().ToArray());
                int maxdouble = (messageSize - 4) / 8;
                List<double> doubleList = new List<double>();
                doubleList.Add(messageSize);
                if (maxdouble > 61)
                {
                    int[] array = new int[] { 55,56,57,58,59,60,94,101,103,132};
                    for (int i = 0; i < maxdouble; i++)
                    {
                        if (array.Contains(i))
                        {
                            if (reviceData.Length >= (12 + i * 8))
                            {
                                byte[] data = reviceData.Skip(4 + i * 8).Take(8).ToArray().Reverse<byte>().ToArray();
                                double value = BitToDouble(data);
                                //LogUtil.info("第【"+(i+1)+"】个double["+ByteToStr(data) +"]值："+value);
                                doubleList.Add(value);
                            }
                            else
                            {
                                break;
                            }
                        }
                        else
                        {
                            doubleList.Add(0);
                        }
                    }
                    if (doubleList.Count > 61)
                    {
                        LastReviceDataTime = DateTime.Now;
                        double x = doubleList[56] * 1000;
                        double y = doubleList[57] * 1000;
                        double z = doubleList[58] * 1000;
                        double rx = doubleList[59] * 1;
                        double ry = doubleList[60] * 1;
                        double rz = doubleList[61] * 1;
                        URPointValue newp = new URPointValue(x, y, z, rx, ry, rz);
                        int robotMode =(int) doubleList[95];
                        int safetyMode =(int) doubleList[102];
                        string robotModeStr = GetRobotMode(robotMode);
                        string safetyModeStr = GetSafetyMode(safetyMode);
                        int programState = (int)doubleList[132];
                        SafetModeFun?.Invoke(safetyModeStr);
                        RobotModeFun?.Invoke(robotModeStr);
                        if (!URRobotControl.IsSamePoint(newp, URRobotControl.LastPoint))
                        {
                            LogUtil.URLInfo(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]，【" + programState + "】【" + robotModeStr + "】【" + safetyModeStr + "】坐标" + newp.ToShowStr()+"");
                        } 
                        URRobotControl.LastPoint = newp;
                    }
                }
                else
                {
                    string reviceMsg = ByteToStr(reviceData); 
                    LogUtil.URLError(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]无法获取坐标，关闭重新连接");
                    if (IsSpitPackage)
                    {
                        StopListen();
                    }
                }
            }
            catch(Exception ex)
            {
                LogUtil.URLError(LogName+ "HandleMessage出错：" + ex.ToString());
                StopListen();
            }
        }
        public static string GetRobotMode(int type)
        {
            switch (type)
            {
                case 0:
                    return "ROBOT_MODE_DISCONNECTED";
                case 1:
                    return "ROBOT_MODE_CONFIRM_SAFETY";
                case 2:
                    return "ROBOT_MODE_BOOTING";
                case 3:
                    return "ROBOT_MODE_POWER_OFF";
                case 4:
                    return "ROBOT_MODE_POWER_ON";
                case 5:
                    return "ROBOT_MODE_IDLE";
                case 6:
                    return "ROBOT_MODE_BACKDRIVE";
                case 7:
                    return "ROBOT_MODE_RUNNING";
                case 8:
                    return "ROBOT_MODE_UPDATING_FIRMWARE";
                default:
                    return "";
            }
        }
        public static string GetSafetyMode(int type)
        {
            switch (type)
            {
                case 1:
                    return "SAFETY_MODE_NORMAL";
                case 2:
                    return "SAFETY_MODE_REDUCED";
                case 3:
                    return "SAFETY_MODE_PROTECTIVE_STOP";
                case 4:
                    return "SAFETY_MODE_RECOVERY";
                case 5:
                    return "SAFETY_MODE_SAFEGUARD_STOP";
                case 6:
                    return "SAFETY_MODE_SYSTEM_EMERGENCY_STOP";
                case 7:
                    return "SAFETY_MODE_ROBOT_EMERGENCY_STOP";
                case 8:
                    return "SAFETY_MODE_VIOLATION";
                case 9:
                    return "SAFETY_MODE_FAULT";
                default:
                    return "";
            }
        }

        public static void SendCMD(string moveCmd)
        {
            try
            {
                if (listenClient != null)
                {
                    listenClient.send(moveCmd);
                }
            }catch(Exception ex)
            {
                LogUtil.URSError(LogName+"发送命令【"+moveCmd+"】出错："+ex.ToString());
            }
        }
    }
}