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


namespace URSoldering.DeviceLibrary
{
    public class EpsonControl

    {
        public static readonly ILog LOGGER = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
        public static TcpClient controlTcp = new TcpClient();
        public static TcpClient moveTcp = new TcpClient();

        public static string EpsonIp = "";
        public static int ControlPort = 5000;
        public static int MovePointPort = 2000;
        public static double Robot_LIM_Z = (double)ConfigAppSettings.GetNumValue(Setting_Init.Soldering_LIM_Z);

        private static string CMD_login = "$login";
        private static string CMD_reset = "$reset";
        private static string CMD_getStatus = "$GetStatus";//收到数据：#GetStatus,00100010001,0000
        private static string CMD_stop = "$stop";
        private static string CMD_start0 = "$start,0";
        private static string MoveOK = "point ok";
        private static string FreeOK = "free ok";
        private static string LockOK = "lock ok";

        /// <summary>
        /// 记录最后一次收到OK的时间
        /// </summary>
        private static Dictionary<string, DateTime> LastOkMap = new Dictionary<string, DateTime>();
        private static System.Timers.Timer timer = null;
        //最后一次从机械臂读取到的坐标位置
        public static PointValue LastPoint = new PointValue(0, 0, 0, 0, 0);
        //最后一次软件控制移动到的位置
        public static PointValue LastSendPoint = new PointValue(0, 0, 0, 0, 0);

        
        /// <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>
        /// 上一次启动的时间，机械臂启动需要时间，默认需要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 bool InitEpson()
        {
            if (reconnectTimer == null)
            {
                reconnectTimer = new System.Timers.Timer();
                reconnectTimer.AutoReset = true;
                reconnectTimer.Interval = 30000;
                reconnectTimer.Elapsed += reconnectTimer_Elapsed;
                reconnectTimer.Enabled = false;
            }
            if (timer == null)
            {
                timer = new System.Timers.Timer();
                timer.Interval = 1000;
                timer.AutoReset = false;
                timer.Elapsed += timer_Elapsed;
            }
            IsStartConnect = true;
            if (IsRun)
            {
                return true;
            }
            return startTcp();
        }
        private static bool startTcp()
        {
            try
            {
                WarnMsg = "";
                if (startCount > 0)
                {
                    TimeSpan span = DateTime.Now - PreStartTime;
                    if (span.TotalMilliseconds < StartTimeOutSeconds)
                    {
                        LogUtil.info("机械臂正在连接中，不需要重连");
                        return true;
                    }
                    else
                    {
                        LogUtil.info("机械臂距离上次启动已经超过10秒，重新开始连接");
                        StopEpson();
                    }
                }
                PreStartTime = DateTime.Now;
                startCount++;

                LastOkMap = new Dictionary<string, DateTime>();
                LastOkMap.Add(FreeOK, DateTime.Now);
                LastOkMap.Add(MoveOK, DateTime.Now);
                LastOkMap.Add(LockOK, DateTime.Now);

                controlTcp = new TcpClient();
                bool result = controlTcp.connect(EpsonIp, ControlPort, new TcpClient.HandleMessage(OnControlRevice));
                if (!result)
                {
                    LogUtil.error(LOGGER, "连接【" + EpsonIp + "】【" + ControlPort + "】失败");
                    return false;
                }
                else
                {
                    LogUtil.info(LOGGER, "连接【" + EpsonIp + "】【" + ControlPort + "】成功");
                }
                //发送
                controlTcp.sendLine(CMD_login);
                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_getStatus);
                }
                if (moveTcp.IsConnected())
                {
                }
                else
                {
                    LogUtil.error("reconnectTimer_Elapsed检测到机械臂已经断开，需要重连！");
                    stopTcp();
                    startTcp();
                }
            }
        }

        public static bool StartMoveTcp()
        {
            if (IsRun)
            {
                return true;
            }
            moveTcp = new TcpClient();
            bool result = moveTcp.connect(EpsonIp, MovePointPort, new TcpClient.HandleMessage(OnMoveRevice));
            if (!result)
            {
                LogUtil.error(LOGGER, "连接【" + EpsonIp + "】【" + MovePointPort + "】失败");
                return false;
            }
            else
            {
                IsRun = true;
                reconnectTimer.Enabled = true;
                LogUtil.info(LOGGER, "连接【" + EpsonIp + "】【" + MovePointPort + "】成功");
                EpsonControl.GetPosition();
            }
            return true;
        }
        public static bool StopMoveTcp()
        {
            if (moveTcp.IsConnected())
            {
                moveTcp.close();
                controlTcp.sendLine(CMD_stop);
            }
            return true;
        }

        //public delegate void OperateEnd(string result);
        //private static event OperateEnd OnOperateEnd;
        //public static void SendMovePoint(double x, double y, double z, double u, bool IsHighSpeed, int hand, OperateEnd AfterOperateEnd)
        //{
        //    EpsonControl.OnOperateEnd = AfterOperateEnd;
        //    SendMovePoint(x, y, z, u, IsHighSpeed, hand);
        //}
         
        public static bool SendMovePoint(double x, double y, double z, double u, bool IsHighSpeed, int hand)
        { 
            return SendMovePoint(x, y, z, u, IsHighSpeed, hand, Robot_LIM_Z);
        }
        /// <summary>
        /// 发送坐标移动
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z">上下</param>
        /// <param name="u">旋转</param>
        /// <param name="IsHighSpeed"></param>
        /// <param name="hand">1=右手，2=左手</param> 
        public static bool SendMovePoint(double x, double y, double z, double u, bool IsHighSpeed, int hand,double limZ)
        {
            if (z > 0)
            {
                LogUtil.error("坐标错误：" + x + "," + y + "," + z + "," + u);
                return false;
            }
            if (limZ > EpsonDevice.Robot_LIM_Z)
            {
                limZ = EpsonDevice.Robot_LIM_Z;
            } 
            List<double> pointList = new List<double>();
            pointList.Add(x);
            pointList.Add(y);
            pointList.Add(z);
            pointList.Add(u);
            LastSendPoint = new PointValue(x, y, u, z, hand);

            string str = "move";
            foreach (double dd in pointList)
            {
                if (str.Equals(""))
                {
                    str += dd.ToString();
                }
                else
                {
                    str = str + "," + dd.ToString();
                }
            }
            if (IsHighSpeed)
            {
                str = str + ",h";
            }
            else
            {
                str = str + ",l";
            }
            if (hand == 2)
            {
                str = str + ",l";
            }
            else if (hand == 1)
            {
                str = str + ",r";
            }
            else
            {
                str = str + ",0";
            }

            str = str + "," + limZ;
            if (IsLock == false)
            {
                LogUtil.error(LOGGER, "机械臂Move之前，发现没有锁定轴【" + str + "】,直接返回不处理！");
                return false;
            }
            moveTcp.sendLine(str);
            return true;
        }
        public static bool LockAxis()
        {
            try
            {
                IsLock = true;
                moveTcp.sendLine(GetSendStr("lock"));
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "出错啦" + ex.ToString());
            }
            return true;
        }
        public static bool FreeAxis()
        {
            try
            {
                IsLock = false;
                moveTcp.sendLine(GetSendStr("free"));
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "出错啦" + ex.ToString());
            }
            return true;
        }
        public static bool GetPosition()
        {
            try
            {
                moveTcp.sendLine(GetSendStr("save"));
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "出错啦" + ex.ToString());
            }
            return true;
        }
        private static void OnControlRevice(string message)
        {
            if (message == null || message.Equals(""))
            {
                return;
            }
            try
            {
                LogUtil.debug(LOGGER, "【" + EpsonIp + "】【" + ControlPort + "】收到数据：" + message);
                if (message.IndexOf("#login") >= 0)
                {
                    //Thread.Sleep(100);
                    controlTcp.sendLine(CMD_stop);
                }
                else if (message.IndexOf("#stop") >= 0)
                {
                    //Thread.Sleep(100);
                    controlTcp.sendLine(CMD_reset);
                }
                else if (message.IndexOf("#reset") >= 0)
                {
                    Thread.Sleep(200);
                    controlTcp.sendLine(CMD_start0);
                }
                else if (message.IndexOf("#start") >= 0)
                {
                    LogUtil.info(LOGGER, "【" + EpsonIp + "】【" + ControlPort + "】收到数据：" + message+ "，开始StartMoveTcp");
                    timer.Start(); 
                }else if (message.ToLower().IndexOf("#getstatus") >= 0)
                {
                    StatusProcess(message);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "出错啦" + ex.ToString());
            }
        }
        
        private static void  StatusProcess(string message)
        {
            try
            {
                //GetStatus，[状态]，[错误，警告代码] 终端//例如） #GetStatus,aaaaaaaaaa,bbbb 
                string[] mesArray = message.Split(',');
                if (mesArray.Length == 3)
                {
                    string isOpen = "1";
                    //     在上例中，10 位数字“aaaaaaaaaa”用于以下 10 个标志。
                    //Test / Teach / Auto / Warning / SError / Safeguard / EStop / Error / Paused / Running / Ready
                    string statusStr = mesArray[1];
                    int code = Convert.ToInt32(mesArray[2]);
                    
                    bool Test = statusStr.Substring(0, 1).Equals(isOpen);
                    bool Teach = statusStr.Substring(1, 1).Equals(isOpen);
                    bool Auto = statusStr.Substring(2, 1).Equals(isOpen);
                    bool Warning = statusStr.Substring(3, 1).Equals(isOpen);
                    bool SError = statusStr.Substring(4, 1).Equals(isOpen);
                    bool Safeguard = statusStr.Substring(5, 1).Equals(isOpen);
                    bool EStop = statusStr.Substring(6, 1).Equals(isOpen);
                    bool Error = statusStr.Substring(7, 1).Equals(isOpen);
                    bool Paused = statusStr.Substring(8, 1).Equals(isOpen);
                    bool Running = statusStr.Substring(9, 1).Equals(isOpen);
                    bool Ready = statusStr.Substring(10, 1).Equals(isOpen);
                    if (EStop)
                    {
                        WarnMsg = "机械臂急停";
                        LogUtil.error("机械臂状态：" + message+","+WarnMsg+"，发送重置命令");
                        controlTcp.sendLine(CMD_reset);

                    }else if (Error)
                    {
                        WarnMsg = "机械臂错误码：" + code;
                        LogUtil.error("机械臂状态：" + message + "," + WarnMsg + "，发送重置命令");
                        controlTcp.sendLine(CMD_reset);
                    }
                    else
                    {
                        WarnMsg = "";
                    }
                }
            }catch(Exception ex)
            {
                LogUtil.error("EpsonControl解析机械臂状态【"+message+"】出错："+ex.ToString());
            }
//#
//  *2 * 3
//                0(Ver.7.1) 用户指南 Rev.1 32 * 1[0 | 1] I / O 位 开：1 / 关：0
//    * 2 状态
//1 为开 / 0 为关
//  如果 Teach 和 Auto 为开，则为 1100000000。
//*3 错误 / 警告代码
//以 4 位数字表示。如果没有错误和警告，则为 0000。
//例如）1： #GetStatus,0100000001,0000 
//  Auto 位和 Ready 位为开（1）。
//表示自动模式开启并处于准备就绪状态。已启用命令执行。 
//例如）2： #GetStatus,0110000010,0517 
//这意味着运行过程中发生警告。对警告代码采取适当的行动。（在这种
//情况下，警告代码为 0517） 
//标志 内容
//Test 在TEST模式下打开
//Teach 在TEACH模式下打开
//Auto 在远程输入接受条件下打开标志 内容
//Warnig
//在警告条件下打开
//甚至在警告条件下也可以像往常一样执行任务。但是，应尽快采取警
//告行动。 
//SError
//在严重错误状态下打开
//发生严重错误时，重新启动控制器，以便从错误状态中恢复。“Reset 输
//入”不可用。 
//Safeguard 安全门打开时打开
//EStop 在紧急状态下打开
//Error 在错误状态下打开
//使用“Reset 输入”从错误状态中恢复。 
//Paused 打开暂停的任务
//Running 执行任务时打开
//在“Paused 输出”为开时关闭。
//Ready 控制器完成启动且无任务执行时打开
//*4 返回要获取的编号中指定编号的值。
        }
        private static void timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            timer.Stop();
            StartMoveTcp();
        }
        private static void OnMoveRevice(string message)
        {
            WarnMsg = "";
            if (message == null || message.Equals(""))
            {
                return;
            }
            try
            {
                LogUtil.debug("【" + EpsonIp + "】【" + MovePointPort + "】收到数据：" + message.Replace("\r\n", ""));
                if (message.Contains(MoveOK))
                {
                    //LogUtil.debug(LOGGER, "【" + EpsonIp + "】【" + MovePointPort + "】收到数据：" + MoveOK);
                    if (LastOkMap.ContainsKey(MoveOK))
                    {
                        LastOkMap.Remove(MoveOK);
                    }
                    LastOkMap.Add(MoveOK, DateTime.Now);
                    //EpsonControl.OnOperateEnd?.Invoke("");
                }
                else if (message.Contains(FreeOK))
                {
                    //LogUtil.debug(LOGGER, "【" + EpsonIp + "】【" + MovePointPort + "】收到数据：" + FreeOK);
                    if (LastOkMap.ContainsKey(FreeOK))
                    {
                        LastOkMap.Remove(FreeOK);
                    }
                    LastOkMap.Add(FreeOK, DateTime.Now);
                    //EpsonControl.OnOperateEnd?.Invoke("");
                }
                else if (message.Contains(LockOK))
                {
                    //LogUtil.debug(LOGGER, "【" + EpsonIp + "】【" + MovePointPort + "】收到数据：" + LockOK);
                    if (LastOkMap.ContainsKey(LockOK))
                    {
                        LastOkMap.Remove(LockOK);
                    }
                    LastOkMap.Add(LockOK, DateTime.Now);
                    //EpsonControl.OnOperateEnd?.Invoke("");
                }
                else
                {
                    string[] posList = message.Split(',');
                    if (posList.Length == 5)
                    {
                        List<double> result = new List<double>();
                        foreach (string str in posList)
                        {
                            result.Add(Convert.ToDouble(str));
                        }
                        LastPoint = new PointValue(result[0], result[1], result[3], result[2], Convert.ToInt32(result[4]));
                    }
                }
            }
            catch (Exception ex)
            {
                LogUtil.error(LOGGER, "收到数据【"+message+"】出错啦" + ex.ToString());
                if (message.StartsWith("ERR"))
                {
                    WarnMsg = "Robot " + message;
                }
            }
        }

        public static string GetSendStr(string str)
        {
            return str + ",0,0,0,0,0,0,0";
        }

        private static void stopTcp()
        {
            try
            {
                IsRun = false;
                startCount--;
                controlTcp.sendLine(CMD_stop);
                moveTcp.close();
                controlTcp.close();
            }
            catch (Exception ex)
            {
                LogUtil.error("stopTcp出错啦" + ex.ToString());
            }
        }
        public static void StopEpson()
        {
            try
            {
                IsStartConnect = false ;
                reconnectTimer.Enabled = false;
                stopTcp();
            }
            catch (Exception ex)
            {
                LogUtil.error("StopEpson出错啦" + ex.ToString());
            }
        }
        public static DateTime MoveOKTime()
        {
            if (LastOkMap.ContainsKey(MoveOK))
            {
                return LastOkMap[MoveOK];
            }
            return new DateTime(0);
        }
    }
    public class PointValue
    { 
        public PointValue(double x, double y, double u, double z, int handDir)
        {
            // TODO: Complete member initialization
            this.X = x;
            this.Y = y;
            this.U = u;
            this.Z = z;
            this.HandDir = handDir;
            this.UpdateTime = DateTime.Now;
        }
        /// <summary>
        /// 更新的时间
        /// </summary>
        public DateTime  UpdateTime { get; set; }
        public double X { get; set; }
        public double Y { get; set; }
        public double U { get; set; }
        public double Z { get; set; }
        public int HandDir { get; set; }
    }
}