using log4net;
using URSoldering.Common;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using URToolKit;

namespace URSoldering.DeviceLibrary
{

    public class URRobotControl
    {
        private static string spiltStr = ",";
        public static readonly ILog LOGGER = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
        public static TcpClient controlTcp = new TcpClient();
        public static URPointValue LastPoint = new URPointValue();
        public static URPointValue LastSendPoint = new URPointValue();
        public static string RobotIp = "";
        public static int ControlPort = 29999;

        public static double Robot_LIM_Z = (double)ConfigAppSettings.GetNumValue(Setting_Init.Soldering_LIM_Z);

        private static string CMD_powerOn = "power on";
        private static string CMD_powerOff = "power off";
        private static string CMD_robotMode = "robotmode";
        private static string CMD_brakeRelease = "brake release";
        private static string CMD_Safetymode = "Safetymode";

        private static string REV_PowerOn = "powering on";
        private static string REV_BrakeRelease = "brake releasing";
        private static string REV_RobotMode = "robotmode";
        private static string REV_SafetyMode = "safetymode";
        /// <summary>
        /// 记录最后一次收到OK的时间
        /// </summary>
        private static Dictionary<string, DateTime> LastOkMap = new Dictionary<string, DateTime>();

        /// <summary>
        /// 是否锁住轴
        /// </summary>
        public static bool IsLock = true;
        /// <summary>
        /// 是否运行中
        /// </summary>
        public static bool IsRun = false;

        public static bool IsStartConnect = false;
        private static int startCount = 0;
        /// <summary>
        /// 上一次启动的时间，UR机械臂启动需要时间，默认需要3分钟，若三分钟后还没有链接，需要重新启动
        /// </summary>
        private static DateTime PreStartTime = DateTime.Now;
        private static int StartTimeOutSeconds = 20000;

        public static string WarnMsg = "";
        private static System.Timers.Timer reconnectTimer = null;

        public static void InitConfig(string ip)
        {
            RobotIp = ip;
            URRobotClient.SafetModeFun += SafetyEProcess;
            URRobotClient.RobotModeFun += ModeEProcess;
        }
        public static string LogName
        {
            get { return "【" + RobotIp + " ，" + ControlPort + "】"; }
        }

        public static string GetStatus()
        {
            if (WarnMsg.Equals(""))
            {
                if (IsRun)
                {
                    return "机器人正常连接中";
                }
                else
                {
                    if (IsStartConnect)
                    {
                        TimeSpan span = DateTime.Now - PreStartTime;
                        if (span.TotalMilliseconds > StartTimeOutSeconds)
                        {
                            return "机器人启动超时";
                        }
                        else
                        {

                            return "机器人启动中";
                        }
                    }
                    else
                    {
                        return "机器人未连接";
                    }
                }
            }
            else
            {
                return WarnMsg;
            }
        }

        public static void Reset()
        {
            URWithOutTP a = new URWithOutTP();          //创建对象
            a.restartURControl(RobotIp);         //重置UR控制器，然后可以利用dashboard，poweron,brake release  
            LogUtil.info(LogName + "重置完成");
            Thread.Sleep(100);
        }
        /// <summary>
        /// 开始启动连接机器人
        /// </summary>
        /// <returns></returns>
        public static bool StartRobot()
        {
            if (IsRun)
            {
                return true;
            }
            if (reconnectTimer == null)
            {
                reconnectTimer = new System.Timers.Timer();
                reconnectTimer.AutoReset = true;
                reconnectTimer.Interval = 400;
                reconnectTimer.Elapsed += reconnectTimer_Elapsed;
                reconnectTimer.Enabled = false;
            }
            IsStartConnect = true;
            return StartConnect();
        }

        private static bool StartConnect()
        {
            try
            {
                if (RobotIp.Equals(""))
                {
                    LogUtil.info("未初始化UR机械臂的IP地址");
                    return false;
                }
                if (startCount > 0)
                {
                    TimeSpan span = DateTime.Now - PreStartTime;
                    if (span.TotalMilliseconds < StartTimeOutSeconds)
                    {
                        LogUtil.info("" + LogName + "正在连接中，不需要重连");
                        return true;
                    }
                    else
                    {
                        LogUtil.info("" + LogName + "距离上次启动已经超过10秒，重新开始连接");
                        StopRobot();
                    }
                }
                WarnMsg = "";
                IsRun = false;
                ClearPreCMD();
                LogUtil.info("" + LogName + "开始连接");
                PreStartTime = DateTime.Now;
                startCount++;

                controlTcp = new TcpClient();
                controlTcp.TimeOutTime = 2000;
                bool result = controlTcp.connect(RobotIp, ControlPort, new TcpClient.HandleMessage(OnControlRevice));
                if (!result)
                {
                    LogUtil.error(LOGGER, "连接" + LogName + "失败");
                    return false;
                }
                else
                {
                    LogUtil.info(LOGGER, "连接" + LogName + "成功");
                }
                //发送
                controlTcp.sendLine(CMD_Safetymode);
                //controlTcp.sendLine(CMD_robotMode);
                preCheckTime = DateTime.Now;
                return true;
            }
            catch (Exception ex)
            {
                LogUtil.error("" + LogName + "starttcp error:" + ex.ToString());
                return false;
            }
        }
        private static DateTime preCheckTime = DateTime.Now;
        private static bool IsInPorcess = false;
        private static void reconnectTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            if (IsInPorcess)
            {
                return;
            }
            IsInPorcess = true;
            try
            {
                if (IsRun && IsStartConnect)
                {
                    TimeSpan span = DateTime.Now - preCheckTime;
                    if (span.TotalSeconds > 2)
                    {
                        preCheckTime = DateTime.Now;
                        //判断500在连接上，则获取急停状态
                        if (controlTcp.IsConnected())
                        {
                            //controlTcp.sendLine(CMD_Safetymode);
                        }
                        else
                        {
                            LogUtil.error("" + LogName + "reconnectTimer_Elapsed检测到 机械臂已经断开，需要重连！");
                            stopTcp();
                            StartConnect();
                        }
                    }
                }
                if (IsRun)
                {
                    //如果监听的断开，直接重新连接
                    if (!URRobotClient.IsConnected())
                    {
                        URRobotClient.StartListen(RobotIp);
                    }else if (URRobotClient.IsTimeOut())
                    {
                        LogUtil.URSError( URRobotClient.LogName+ "接收数据超时，断开后重连" );
                        URRobotClient.StopListen();
                        URRobotClient.StartListen(RobotIp);
                    }
                }
            }
            catch (Exception ex)
            {
                LOGGER.Error("" + LogName + "  定时器出错：" + ex.ToString());
            }
            IsInPorcess = false;
        }
        public static bool SendCMD(string cmd, int msendons)
        {
            try
            {
                LogUtil.URSInfo(LogName + "将于【" + msendons + "】后发送数据：" + cmd);
                if (msendons > 0)
                {
                    Task.Factory.StartNew(delegate ()
                    {
                        Thread.Sleep(msendons);
                        controlTcp.sendLine(cmd);
                    });
                }
                else
                {
                    controlTcp.sendLine(cmd);
                }
            }
            catch (Exception ex)
            {
                LogUtil.URSError("" + LogName + "SendCMD出错啦cmd[" + cmd + "]msendons[" + msendons + "]" + ex.ToString());
            }
            return true;
        }


        private static void OnControlRevice(string message)
        {
            if (message == null || message.Equals(""))
            {
                return;
            }
            try
            {
                //Connected: Universal Robots Dashboard Server
                message = message.Replace("\n", "");
                if (IsRun.Equals(false))
                {
                    LogUtil.URSInfo(LogName + "收到数据：" + message);
                }
                if (message.ToLower().IndexOf(REV_PowerOn) >= 0)
                {
                    SendCMD(CMD_brakeRelease, 1000);
                }
                else if (message.ToLower().IndexOf(REV_BrakeRelease) >= 0)
                {
                    SendCMD(CMD_robotMode, 2000);
                }
                else if (message.ToLower().IndexOf(REV_RobotMode) >= 0)
                {
                    ModeProcess(message);
                }
                else if (message.ToLower().IndexOf(REV_SafetyMode) >= 0)
                {
                    SafetyProcess(message);
                }
                else
                {
                    //急停模式
                }
            }
            catch (Exception ex)
            {
                LogUtil.URSError(LogName + " OnControlRevice出错啦" + ex.ToString());
            }
        }
        private static void ModeProcess(string message)
        {
            if (IsRun)
            {
                return;
            }
            string msg = message.ToLower().Replace(REV_RobotMode + ": ", "").ToUpper().Trim();
            if (msg.Equals(URStatus.POWER_ON))
            {
                SendCMD(CMD_brakeRelease, 1000);
            }
            else if (msg.Equals(URStatus.RUNNING) || msg.Equals(URStatus.IDLE))
            {
                WarnMsg = "";

                LogUtil.URSInfo(LogName + " 当前状态：" + msg + "，机器人连接成功！");
                LogUtil.info(LogName + " 当前状态：" + msg + "，机器人连接成功！");
                IsRun = true;
                reconnectTimer.Enabled = true; 
            }
            else
            {
                if (!IsTimeOut(message))
                {
                    LogUtil.URSInfo(LogName + "当前状态：" + msg + "，发送打开命令" + CMD_powerOn);
                    SendCMD(CMD_powerOn, 1000);
                }
                else
                {
                    WarnMsg = "机器人状态[" + msg + "]，启动超时";
                    StopRobot();
                }
            } 
        }

        private static void SafetyProcess(string message)
        {
            if (IsRun)
            {
                return;
            }
            string msg = message.ToLower().Replace(REV_SafetyMode + ": ", "").ToUpper().Trim();

            if (msg.Equals(URStatus.SFETY_POWER_OFF) || msg.Equals(URStatus.ROBOT_EMERGENCY_STOP))
            {
                if (!IsTimeOut(message))
                {
                    WarnMsg = "机器人急停中[" + msg + "]，请先打开急停";
                    SendCMD(CMD_Safetymode, 1000);
                }
                else
                {
                    WarnMsg = "机器人急停中[" + msg + "]，启动超时";
                    StopRobot();
                }
            }
            else
            {
                SendCMD(CMD_robotMode, 500);
            }
        }
        private static void ModeEProcess(string mode)
        {
            if (!IsRun)
            {
                return;
            }
            mode = mode.Replace("ROBOT_MODE_", "").ToUpper().Trim();
            if (mode.Equals(URStatus.POWER_ON))
            {
                SendCMD(CMD_brakeRelease, 1000);
            }
            else if (mode.Equals(URStatus.RUNNING) || mode.Equals(URStatus.IDLE))
            {
            }
            else
            {
                WarnMsg = "机器人状态[" + mode + "]，尝试自动复位";
                LogUtil.info(LogName + WarnMsg);
                IsRun = false;
                IsStartConnect = true;
                PreStartTime = DateTime.Now;
                SendCMD(CMD_powerOn, 500);
            }

        }
        private static void SafetyEProcess(string safetyStatus)
        {
            if (!IsRun)
            {
                return;
            }
            safetyStatus = safetyStatus.Replace("SAFETY_MODE_", "").ToUpper().Trim();
            if (safetyStatus.Equals(URStatus.SFETY_POWER_OFF))
            {
                WarnMsg = "机器人急停中[" + safetyStatus + "]，请复位";
                LogUtil.info(LogName + WarnMsg);
                StopRobot();
            }
            else if (safetyStatus.Equals(URStatus.ROBOT_EMERGENCY_STOP))
            {
                WarnMsg = "机器人急停中[" + safetyStatus + "]，请复位";
                LogUtil.info(LogName + WarnMsg);
                StopRobot();
            }
            else
            {
                //    SendCMD(CMD_robotMode, 500);
            }
        }
        private static bool IsTimeOut(string status)
        {
            TimeSpan span = DateTime.Now - PreStartTime;
            if (span.TotalMilliseconds > StartTimeOutSeconds)
            {
                string logMsg = LogName + "【" + status + "】 已持续" + Math.Round(span.TotalMilliseconds) + "，启动超时！";
                LogUtil.info(logMsg);
                LogUtil.URSInfo(logMsg);
                return true;
            }
            return false;
        }


        private static void stopTcp()
        {
            try
            {
                IsRun = false;
                startCount--;
                //controlTcp.sendLine(CMD_powerOff);
                controlTcp.close();
            }
            catch (Exception ex)
            {
                LogUtil.error(LogName + "stopTcp出错啦" + ex.ToString());
            }
        }
        /// <summary>
        /// 停止机器人，断开连接
        /// </summary>
        public static void StopRobot()
        {
            try
            {
                ClearPreCMD();
                IsStartConnect = false;
                reconnectTimer.Enabled = false;
                URRobotClient.StopListen();
                stopTcp();
            }
            catch (Exception ex)
            {
                LogUtil.error(LogName + "StopEpson出错啦" + ex.ToString());
            }
        }
        /// <summary>
        /// 释放所有轴，切换为手移方式
        /// </summary>
        public static void FreeAxis()
        {
        }

        /// <summary>
        /// 锁定轴，改为程序模式
        /// </summary>
        public static void LockAxis()
        {
        }

        public static bool PointIsValid(URPointValue point)
        {
            if (point.X != 0 && point.Y != 0 && point.Z != 0)
            {
                return true;
            }
            return false;
        }
        public static void MoveTo(URPointValue point, bool isHigh)
        {
            if (!PointIsValid(point))
            {
                LogUtil.info(LogName + "试图移动到：" + point.ToShowStr() + ",此坐标无效，直接返回");
                return;
            }
            //movep(p[0.062, -0.030, 0.325, 0.0299, 2.2263, 2.2171], a = 1.2, v = 0.25, r = 0)

            //movej([0, 1.57, -1.57, 3.14, -1.57, 1.57],a = 1.4, v = 1.05, t = 0, r = 0)
            string x = Math.Round(point.X / 1000F, 5).ToString();
            string y = Math.Round(point.Y / 1000F, 5).ToString();
            string z = Math.Round(point.Z / 1000F, 5).ToString();

            string moveCmd = "movep(p[" + x + spiltStr + y + spiltStr + z + spiltStr + point.RX + spiltStr + point.RY + spiltStr + point.RZ + "],a=0.1, v=0.1,  r=0)";
            if (isHigh)
            {
                moveCmd = "movep(p[" + x + spiltStr + y + spiltStr + z + spiltStr + point.RX + spiltStr + point.RY + spiltStr + point.RZ + "],a=1.0, v=1.0,  r=0)";
            }
            ClearPreCMD();

            URRobotClient.LastMoveCMD = moveCmd;
            LastSendPoint = point;
        }
        public static void ClearPreCMD()
        {
            if (!URRobotClient.LastMoveCMD.Equals(""))
            {
                LogUtil.info(LogName + ",清除上次未发送指令：【" + URRobotClient.LastMoveCMD + "】");
                URRobotClient.LastMoveCMD = "";
            }
        }
        public static void MoveTo(URPointValue point)
        {
            MoveTo(point, false);
        }
        public static bool MoveOK(URPointValue point, DateTime moveTime)
        {
            URPointValue lastPoint = GetLastPosition();
            if (lastPoint.UpdateTime > moveTime)
            {
                if (IsSamePoint(point, lastPoint))
                {
                    return true;
                }
            }
            return false;
        }
        public static bool IsSamePoint(URPointValue p1, URPointValue p2)
        {
            if (p1 == null && p2 == null)
            {
                return false;
            }
            double xCha = Math.Abs(p1.X - p2.X);
            double yCha = Math.Abs(p1.Y - p2.Y);
            double zCha = Math.Abs(p1.Z - p2.Z);

            //double rxCha = Math.Abs(p1.RX - p2.RX);
            //double ryCha = Math.Abs(p1.RY - p2.RY);
            //double rzCha = Math.Abs(p1.RZ - p2.RZ);
            double RPow1 = Math.Pow(p1.RX, 2) + Math.Pow(p1.RY, 2) + Math.Pow(p1.RZ, 2);
            double RPow2 = Math.Pow(p2.RX, 2) + Math.Pow(p2.RY, 2) + Math.Pow(p2.RZ, 2);

            if (xCha < 1 && yCha < 1 && zCha < 1)
            {
                return true;
            }
            //if (xCha < 1 && yCha < 1 && zCha < 1 && rxCha < 0.02 && ryCha < 0.05 && rzCha < 0.05)
            //{
            //    return true;
            //}
            return false;
        }

        public static URPointValue GetLastPosition()
        {
            return LastPoint;
        }
       
    }
    public class URPointValue
    {
        public URPointValue()
        {
            this.X = 0;
            this.Y = 0;
            this.Z = 0;
            this.RX = 0;
            this.RY = 0;
            this.RZ = 0;
            this.UpdateTime = DateTime.Now;
        }
        public URPointValue(double x, double y, double z, double rx, double ry, double rz)
        {
            // TODO: Complete member initialization
            this.X = Math.Round(x, 2);
            this.Y = Math.Round(y, 2);
            this.Z = Math.Round(z, 2);
            this.RX = Math.Round(rx, 4);
            this.RY = Math.Round(ry, 4);
            this.RZ = Math.Round(rz, 4);
            this.UpdateTime = DateTime.Now;
        }
        /// <summary>
        /// 更新的时间
        /// </summary>
        public DateTime UpdateTime { get; set; }
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
        public double RX { get; set; }
        public double RY { get; set; }
        public double RZ { get; set; }

        public string ToShowStr()
        {
            return "[X: " + X + ",Y: " + Y + ",Z: " + Z + ",RX: " + RX + ",RY:" + RY + ",RZ:" + RZ + "]";
        }

        public string ToJosonStr()
        {
            string jsonStr = JsonHelper.SerializeObject(this);
            return jsonStr;
        }

        public static URPointValue ToObject(string json)
        {
            return JsonHelper.DeserializeJsonToObject<URPointValue>(json);
        }
       
        }
    }
    public struct URStatus
    {
        public static string NO_CONTROLLER = "NO_CONTROLLER";
        public static string DISCONNECTED = "DISCONNECTED";
        public static string CONFIRM_SAFETY = "CONFIRM_SAFETY";
        public static string BOOTING = "BOOTING";
        public static string POWER_OFF = "POWER_OFF";
        public static string POWER_ON = "POWER_ON";
        public static string IDLE = "IDLE";
        public static string BACKDRIVE = "BACKDRIVE";
        public static string RUNNING = "RUNNING";
        /// <summary>
        /// ROBOT_EMERGENCY_STOP 急停
        /// </summary>
        public static string ROBOT_EMERGENCY_STOP = "ROBOT_EMERGENCY_STOP";
        /// <summary>
        /// FAULT 急停
        /// </summary>
        public static string FAULT = "FAULT";
        /// <summary>
        /// POWER_OFF 外部急停
        /// </summary>
        public static string SFETY_POWER_OFF = "POWER_OFF";
    }