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;

namespace URSoldering.DeviceLibrary
{
   
    public class URRobotControl
    {
        public static readonly ILog LOGGER = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
        public static TcpClient controlTcp = new TcpClient();


        public static string RobotIp = "192.168.1.120";
        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";



        /// <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 = new System.Timers.Timer();

        public static void InitConfig(string ip)
        {
            RobotIp = ip;
        }
        /// <summary>
        /// 开始启动连接机器人
        /// </summary>
        /// <returns></returns>
        public static bool StartRobot()
        {
            if (reconnectTimer == null)
            {
                reconnectTimer = new System.Timers.Timer();
                reconnectTimer.AutoReset = true;
                reconnectTimer.Interval = 30000;
                reconnectTimer.Elapsed += reconnectTimer_Elapsed;
                reconnectTimer.Enabled = false;
            }
            IsStartConnect = true;
            if (IsRun)
            {
                return true;
            }
            return StartConnect();
        }
         

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


                controlTcp = new TcpClient();
                bool result = controlTcp.connect(RobotIp, ControlPort, new TcpClient.HandleMessage(OnControlRevice));
                if (!result)
                {
                    LogUtil.error(LOGGER, "连接【" + RobotIp + "】【" + ControlPort + "】失败");
                    return false;
                }
                else
                {
                    LogUtil.info(LOGGER, "连接【" + RobotIp + "】【" + ControlPort + "】成功");
                }
                //发送
                controlTcp.sendLine(CMD_robotMode);
                return true;
            }
            catch (Exception ex)
            {
                LogUtil.error("starttcp error:" + ex.ToString());
                return false;
            }
        }
        private static void reconnectTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            if (IsStartConnect)
            {
                //判断500在连接上，则获取状态
                if (controlTcp.IsConnected())
                {
                    controlTcp.sendLine(CMD_robotMode);
                }
                else
                {
                    LogUtil.error("reconnectTimer_Elapsed检测到UR机械臂已经断开，需要重连！");
                    stopTcp();
                    StartConnect();
                }
            }
        }
        public static bool SendCMD(string cmd, int msendons)
        {
            try
            {
                if (msendons > 0)
                {
                    Task.Factory.StartNew(delegate ()
                    {
                        Thread.Sleep(msendons);
                        controlTcp.sendLine(cmd);
                    });
                }
                else
                {
                    controlTcp.sendLine(cmd);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "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
            {
                LogUtil.info(LOGGER, "【" + RobotIp + "】【" + ControlPort + "】收到数据：" + message);
                if (message.ToLower().IndexOf("powering on") >= 0)
                {
                    SendCMD(CMD_brakeRelease, 1000);
                }
                else if (message.ToLower().IndexOf("brake releasing") >= 0)
                {
                    SendCMD(CMD_robotMode, 2000);
                }
                else if (message.ToLower().IndexOf("robotmode") >= 0)
                {
                    StatusProcess(message);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "出错啦" + ex.ToString());
            }
        }

        private static void StatusProcess(string message)
        {
            string msg = message.ToLower().Replace("robotmode: ", "").ToUpper().Trim();
            //LogUtil.info("收到机器人状态："+message);
            if (msg.Equals(URStatus.POWER_ON))
            {
                SendCMD(CMD_brakeRelease, 1000);
            }
            else if (msg.Equals(URStatus.RUNNING) || msg.Equals(URStatus.IDLE))
            {
                LogUtil.info("优傲机器人当前状态：" + msg + "，机器人连接成功！");
                IsRun = true;
            }
            else
            {
                LogUtil.info("优傲机器人当前状态：" + msg + "，发送打开命令" + CMD_powerOn);
                SendCMD(CMD_powerOn, 1000);
            }
        }

        private static void stopTcp()
        {
            try
            {
                IsRun = false;
                startCount--;
                controlTcp.sendLine(CMD_powerOff);
                controlTcp.close();
            }
            catch (Exception ex)
            {
                LogUtil.error("stopTcp出错啦" + ex.ToString());
            }
        }
       /// <summary>
       /// 停止机器人，断开连接
       /// </summary>
        public static void StopRobot()
        {
            try
            {
                IsStartConnect = false;
                reconnectTimer.Enabled = false;
                stopTcp();
            }
            catch (Exception ex)
            {
                LogUtil.error("StopEpson出错啦" + ex.ToString());
            }
        } 
        /// <summary>
        /// 释放所有轴，切换为手移方式
        /// </summary>
        public static void FreeAxis()
        {
        }
      
        /// <summary>
        /// 锁定轴，改为程序模式
        /// </summary>
        public static void LockAxis()
        {
        }
        /// <summary>
        /// 移动到指定的坐标
        /// </summary>
        /// <param name="point"></param>
        public static void MoveTo(URPointValue point)
        {
        }
        /// <summary>
        /// 获取机器人最后一次获取的坐标
        /// </summary>
        /// <returns></returns>
        public static URPointValue GetLastPosition()
        {
            return new URPointValue();
        }
    }
    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 = x;
            this.Y = y;
            this.Z = z;
            this.RX = rx;
            this.RY = ry;
            this.RZ = rz;
            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";
    }
}