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 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 = 10000;

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

        public static void InitConfig(string ip)
        {
            RobotIp = ip;
        }
        public static string LogName
        {
            get { return "【" + RobotIp + " ，" + ControlPort + "】"; }
        }
        public static void Test()
        {
            URWithOutTP a = new URWithOutTP();          //创建对象
            a.restartURControl(RobotIp);         //重置UR控制器，然后可以利用dashboard，poweron,brake release  
        }
        /// <summary>
        /// 开始启动连接机器人
        /// </summary>
        /// <returns></returns>
        public static bool StartRobot()
        {
            if (reconnectTimer == null)
            {
                reconnectTimer = new System.Timers.Timer();
                reconnectTimer.AutoReset = true;
                reconnectTimer.Interval = 1000;
                reconnectTimer.Elapsed += reconnectTimer_Elapsed;
                reconnectTimer.Enabled = false;
            }
            IsStartConnect = true;
            if (IsRun)
            {
                return 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 = "";
                PreStartTime = DateTime.Now;
                startCount++;


                controlTcp = new TcpClient();
                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 (IsStartConnect)
                {
                    TimeSpan span = DateTime.Now - preCheckTime;
                    if (span.TotalSeconds > 30)
                    {
                        preCheckTime = DateTime.Now;
                        //判断500在连接上，则获取状态
                        if (controlTcp.IsConnected())
                        {
                            controlTcp.sendLine(CMD_robotMode);
                        }
                        else
                        {
                            LogUtil.error("" + LogName + "reconnectTimer_Elapsed检测到 机械臂已经断开，需要重连！");
                            stopTcp();
                            StartConnect();
                        }
                    }
                }
                if (IsRun)
                {
                    //如果监听的断开，直接重新连接
                    if (!URRobotClient.IsConnected())
                    {
                        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;
        }
        public static bool GetRobotMode()
        {
            return SendCMD(CMD_robotMode, 0);
        }

       
        private static void OnControlRevice(string message)
        {
            if (message == null || message.Equals(""))
            {
                return;
            }
            try
            {
                //Connected: Universal Robots Dashboard Server
                message = message.Replace("\n", "");
                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)
                {
                    SafetymodeProcess(message);
                }
                else if (message.ToLower().IndexOf(REV_SafetyMode) >= 0)
                {
                    StatusProcess(message);
                }
                else
                {
                    //急停模式
                }
            }
            catch (Exception ex)
            {
                LogUtil.URSError(LogName + " OnControlRevice出错啦" + ex.ToString());
            }
        }
        private static void SafetymodeProcess(string message)
        {
            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);
                }
            }
        }
        private static void StatusProcess(string message)
        {
            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 + "]，启动超时";
                    IsRun = false;
                }
            }
            else
            {
                SendCMD(CMD_Safetymode, 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
            {
                IsStartConnect = false;
                reconnectTimer.Enabled = false;
                stopTcp();
            }
            catch (Exception ex)
            {
                LogUtil.error(LogName + "StopEpson出错啦" + ex.ToString());
            }
        }
        /// <summary>
        /// 释放所有轴，切换为手移方式
        /// </summary>
        public static void FreeAxis()
        {
        }

        /// <summary>
        /// 锁定轴，改为程序模式
        /// </summary>
        public static void LockAxis()
        {
        }
        /// <summary>
        /// 移动到指定的坐标
        /// </summary>
        /// <param name="point"></param>
        public static bool MoveTo(URPointValue point)
        {
            //movej([0, 1.57, -1.57, 3.14, -1.57, 1.57],a = 1.4, v = 1.05, t = 0, r = 0)
            string moveCmd = "movej([" + point.X + spiltStr + point.X + spiltStr + point.X + spiltStr + point.X + spiltStr + point.X + spiltStr + point.X + "],a=1.4, v=1.05, t=0, r=0)";
            URRobotClient.LastMoveCMD = moveCmd;
            return true;
        }
        /// <summary>
        /// 获取机器人最后一次获取的坐标
        /// </summary>
        /// <returns></returns>
        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";
    }
}