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

namespace PUSICANLibrary
{
    public class PUSICANControl
    {
        #region 方法定义
        public delegate PUSIResult EventCall_T(CB_TYPE callbackType, IntPtr pData);
        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_Close", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        private static extern PUSIResult PUSICO_Close();
        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_AddNode", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_AddNode(uint nodeid);
        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_RemoveNode", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_RemoveNode(uint nodeid);

        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_SetEventCallback", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_SetEventCallback(EventCall_T callback);
        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_GetNodeStatus", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_GetNodeStatus(uint nodeid, ref SLAVE_STATUS status);

        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_ReadSDO", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_ReadSDO(uint nodeid, uint index, uint subindex,
            byte[] data, DATA_TYPE datatype, ref uint recvlen, uint timeout);


        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_WriteSDO", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_WriteSDO(uint nodeid, uint index, uint subindex,
            byte[] data, DATA_TYPE datatype, uint sendlen, uint timeout);
        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_SetDriver", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_SetDriver(string dllname, out IntPtr lib);

        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_Open", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_Open(string busname, string buadrate);

        [DllImport("kernel32.dll")]
        public extern static IntPtr GetProcAddress(IntPtr lib, string funcName);

        [DllImport("CanOpen.dll", EntryPoint = "PUSICO_SetNodeState", CallingConvention = CallingConvention.StdCall, CharSet = CharSet.Ansi)]
        public static extern PUSIResult PUSICO_SetNodeState(uint nodeid, SLAVE_MANAGEMENT slaveManagement);


        #endregion

        private static Dictionary<uint, Dictionary<uint, int>> sdoLastData = new Dictionary<uint, Dictionary<uint, int>>();

        #region 读写SDO

        private static void UpdateSDOData(uint nodeId, uint addr, int value)
        {
            if (sdoLastData.ContainsKey(nodeId))
            {
                if (sdoLastData[nodeId].ContainsKey(addr))
                {
                    sdoLastData[nodeId][addr] = value;
                }
                else
                {
                    sdoLastData[nodeId].Add(addr, value);
                }
            }
            else
            {
                Dictionary<uint, int> data = new Dictionary<uint, int>();
                data.Add(addr, value);
                sdoLastData.Add(nodeId, data);
            }
        }
        private static int GetSDOData(uint nodeId, uint addr)
        {
            if (sdoLastData.ContainsKey(nodeId))
            {
                if (sdoLastData[nodeId].ContainsKey(addr))
                {
                    return sdoLastData[nodeId][addr];
                }
            }
            return -1;
        }
        public static PUSIResult WriteSDO(uint nodeId, uint sdoAddr, int value)
        {
            PUSIResult result = PUSIResult.RET_READSDO_FAIL;
            try
            {
                if (!CAN_Address.SDOAddrMap().ContainsKey(sdoAddr))
                {
                    return PUSIResult.RET_WRITESDO_FAIL;
                }
                SDOAddrInfo addr = CAN_Address.SDOAddrMap()[sdoAddr];
                byte[] data = new byte[10];
                for (int i = 0; i < 10; i++)
                {
                    data[i] = 0;
                }
                //相对运动 
                data = BitConverter.GetBytes(value);
                result = PUSICANControl.PUSICO_WriteSDO(nodeId, addr.sdoAddr, addr.subIndex, data, addr.dataType, addr.dataLength, addr.timeOut);
                if (PUSIResult.RET_SUCCESS == result)
                {
                    Console.WriteLine("write "+ nodeId + " CAN_Address." + sdoAddr + " sdo ok.");

                    UpdateSDOData(nodeId, sdoAddr, value);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("write "+ nodeId + " CAN_Address." + sdoAddr + " error ." + ex.ToString());
            }
            return result;
        }

        public static PUSIResult ReadSDO(uint nodeId, uint sdoAddr, out int outValue)
        {
            outValue = 0;
            PUSIResult result = PUSIResult.RET_READSDO_FAIL;
            try
            {
                if (!CAN_Address.SDOAddrMap().ContainsKey(sdoAddr))
                {
                    return PUSIResult.RET_WRITESDO_FAIL;
                }
                SDOAddrInfo addr = CAN_Address.SDOAddrMap()[sdoAddr];
                byte[] data = new byte[10];
                for (int i = 0; i < 10; i++)
                {
                    data[i] = 0;
                }
                //相对运动 
                uint length = addr.dataLength;
                result = PUSICANControl.PUSICO_ReadSDO(nodeId, addr.sdoAddr, addr.subIndex, data, addr.dataType, ref length, addr.timeOut);
                if (PUSIResult.RET_SUCCESS == result)
                {
                    outValue = BitConverter.ToInt32(data, 0);
                    UpdateSDOData(nodeId, sdoAddr, outValue);
                    Console.WriteLine("read  " + addr.sdoAddr + "  sdo ok." + outValue);
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("read " + nodeId + " CAN_Address." + sdoAddr + " error ." + ex.ToString());
            }
            return result;
        }

        #endregion


        public delegate void SetTcpClient_drvier(string ip, int nPort);
        public delegate void SetSerial_drvier(int nPort, int nBaud);
        public static bool ISOpen = false;
        public static string Open()
        {
            if (ISOpen)
            {
                return "";
            }
            int adapterIndex = 4;
            int baudIndex = 4;
            string Ip = "192.168.0.178";
            int port = 4001;
            Ip = ConfigAppSettings.GetValue(Setting_Init.PUSICAN_Ip);
            port = ConfigAppSettings.GetIntValue(Setting_Init.PUSICAN_Port);
            return Open(adapterIndex, baudIndex, Ip, port);

        }
        public static void Close()
        {
            try
            {
                if (ISOpen)
                {
                    PUSICO_Close();
                    ISOpen = false;
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("Close出错：" + ex.ToString());
            }
        }
        public static string Open(int adapterIndex, int baudIndex, string Ip, int port)
        {
            try
            {
                string busname = "1";
                string[] strDriver = { "can_lanma.dll", "can_ledia.dll", "can_yl9100.dll","can_ixxat.dll",
                                     "can_gc.dll","can_zlg.dll","can_zlg.dll","can_zlg.dll","can_cx.dll","can_rs232.dll"};
                string[] baudlist = { "10K", "20K", "50K", "100K", "125K", "250K", "500K", "800K", "1M" };
                switch (adapterIndex)  //周立功USBCAN-E-U
                {
                    case 6:
                        busname = "3";
                        break;
                    case 7:
                        busname = "4";
                        break;
                    case 8:
                        busname = "2";
                        break;

                }

                IntPtr lib = new IntPtr();
                if (PUSIResult.RET_SUCCESS == PUSICANControl.PUSICO_SetDriver(strDriver[adapterIndex], out lib))
                {
                    Console.WriteLine("加载驱动成功.");
                    if (lib != null)
                    {
                        IntPtr api = PUSICANControl.GetProcAddress(lib, "canSetTcpClient_driver");
                        if (api != IntPtr.Zero)
                        {
                            SetTcpClient_drvier SetTcpClient = (SetTcpClient_drvier)Marshal.GetDelegateForFunctionPointer(api, typeof(SetTcpClient_drvier));
                            SetTcpClient(Ip, port);
                        }
                        IntPtr api1 = PUSICANControl.GetProcAddress(lib, "canSetSerial_driver");
                        if (api1 != IntPtr.Zero)
                        {
                            SetSerial_drvier SetSerial = (SetSerial_drvier)Marshal.GetDelegateForFunctionPointer(api1, typeof(SetSerial_drvier));
                            SetSerial(1, 115200);  //端口，波特率
                        }
                    }
                }
                else
                {
                    Console.WriteLine("加载驱动失败！");
                    ISOpen = false;
                    return "加载驱动失败！";
                }
                if (PUSIResult.RET_SUCCESS == PUSICANControl.PUSICO_Open(busname, baudlist[baudIndex]))
                {
                    ISOpen = true;
                    Console.WriteLine("打开端口成功.");
                }
                else
                {
                    Console.WriteLine("打开端口失败！");
                    ISOpen = false;
                    return "打开端口失败！";
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("Open出错：" + ex.ToString());
            }
            return "";
        }
        /// <summary>
        /// 初始化节点
        /// </summary> 
        public static bool InitNode(uint nodeid)
        {
            //添加节点
            PUSIResult result = PUSICANControl.PUSICO_AddNode(Convert.ToUInt32(nodeid));
            //启动节点
            result = PUSICANControl.PUSICO_SetNodeState(nodeid, SLAVE_MANAGEMENT.SLAVE_START);
            return SetOutStopType(nodeid);
        }

        public static bool InitRNodeConfig(uint nodeId, bool isLineSlv = false)
        {
            WriteOutStop(nodeId);

            int addSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_AddSpeed, nodeId.ToString());
            int delSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_DelSpeed, nodeId.ToString());
            int maxPhase = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_MaxPhaseCurrent, nodeId.ToString());
            int startSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_HomeStartSpeed, nodeId.ToString());
            int stopSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_HomeStopSpeed, nodeId.ToString());
            int maxSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_MaxSpeed, nodeId.ToString());
            int microValue = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_MicroStepping, nodeId.ToString());
            int delValue = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_DelPhaseCurrent, nodeId.ToString());
            if (isLineSlv)
            {
                if (ConfigAppSettings.GetIntValue(Setting_Init.Line_HomeStartSpeed, nodeId.ToString()) != 0)
                {
                    startSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_HomeStartSpeed, nodeId.ToString());
                    stopSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_HomeStopSpeed, nodeId.ToString());
                    maxSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_MaxSpeed, nodeId.ToString());
                }
                if (ConfigAppSettings.GetIntValue(Setting_Init.Line_MaxPhaseCurrent, nodeId.ToString()) > 0)
                {
                    addSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_AddSpeed, nodeId.ToString());
                    delSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_DelSpeed, nodeId.ToString());
                    maxPhase = ConfigAppSettings.GetIntValue(Setting_Init.Line_MaxPhaseCurrent, nodeId.ToString());

                    microValue = ConfigAppSettings.GetIntValue(Setting_Init.Line_MicroStepping, nodeId.ToString());
                    delValue = ConfigAppSettings.GetIntValue(Setting_Init.Line_DelPhaseCurrent, nodeId.ToString());
                }
            }

            LogUtil.info("InitRNodeConfig  NodeId【" + nodeId+ "】isLineSlv【"+ isLineSlv + "】SetAddSpeed【" + addSpeed + "】SetDelSpeed【" + delSpeed + "】SetStartSpeed【" + startSpeed
                + "】SetStopSpeed【" + stopSpeed + "】SetMaxSpeed【" + maxSpeed + "】SetMicroStepping【" + microValue + "】SetDelPhaseCurrent【" + delValue + "】SetMaxPhaseCurrent【" + maxPhase + "】");
            PUSICANControl.SetAddSpeed(nodeId, addSpeed);
            PUSICANControl.SetDelSpeed(nodeId, delSpeed);
            PUSICANControl.SetStartSpeed(nodeId, startSpeed);
            PUSICANControl.SetStopSpeed(nodeId, stopSpeed);
            PUSICANControl.SetMaxSpeed(nodeId, maxSpeed);
            PUSICANControl.SetMicroStepping(nodeId, microValue);

            PUSICANControl.SetDelPhaseCurrent(nodeId, delValue);

            PUSICANControl.SetMaxPhaseCurrent(nodeId, maxPhase);

            if (isLineSlv)
            {
                for (int i = 1; i <= 3; i++)
                {
                    int value = PUSICANControl.GetMaxPhaseCurrent(nodeId);
                    if (value.Equals(maxPhase))
                    {
                        LogUtil.info("InitRNodeConfig  NodeId【" + nodeId + "】isLineSlv【" + isLineSlv + "】GetMaxPhaseCurrent "+i+"= " + value);
                        break;
                    }
                    else
                    {
                        LogUtil.info("InitRNodeConfig  NodeId【" + nodeId + "】isLineSlv【" + isLineSlv + "】GetMaxPhaseCurrent " + i + "= " + value+ "，重新设置 SetMaxPhaseCurrent ="+maxPhase);
                        PUSICANControl.SetMaxPhaseCurrent(nodeId, maxPhase);
                        Thread.Sleep(10);
                    }
                }
            }

            //清理驱动器状态
            PUSIResult result = PUSICANControl.WriteSDO(nodeId, CAN_Address.ControlStatus, 1);
            return true;
        } 
        /// <summary>
        /// 写外部停止信号，默认所有轴的停止1停止2都启动
        /// </summary>
        /// <param name="nodeid"></param>
        /// <returns></returns>
        public static bool WriteOutStop(uint nodeid)
        {
            PUSIResult result = PUSIResult.RET_SUCCESS;
            uint sdoAddr = CAN_Address.OutStop;
            if (!CAN_Address.SDOAddrMap().ContainsKey(sdoAddr))
            {
                return false;
            }
            SDOAddrInfo addr = CAN_Address.SDOAddrMap()[sdoAddr];
            int value = 3;
            try
            {
               
                byte[] data = new byte[10];
                for (int i = 0; i < 10; i++)
                {
                    data[i] = 0;
                }
                //相对运动 
                data = BitConverter.GetBytes(value);
                result = PUSICANControl.PUSICO_WriteSDO(nodeid, addr.sdoAddr, 0x01, data, addr.dataType, addr.dataLength, addr.timeOut);
                Thread.Sleep(100);
                if (PUSIResult.RET_SUCCESS == result)
                {
                    Console.WriteLine("write "+ nodeid + " CAN_Address.OutStop sdo ok.");  
                }
                data = new byte[10];
                value = 0;
                for (int i = 0; i < 10; i++)
                {
                    data[i] = 0;
                }
                //相对运动 
                data = BitConverter.GetBytes(value);
                result = PUSICANControl.PUSICO_WriteSDO(nodeid, addr.sdoAddr, 0x02, data, addr.dataType, addr.dataLength, addr.timeOut);
                Thread.Sleep(100);
                if (PUSIResult.RET_SUCCESS == result)
                {
                    Console.WriteLine("write "+ nodeid + " CAN_Address.OutStop sdo ok.");
                }
            }
            catch (Exception ex)
            {
                LogUtil.error("write "+ nodeid + " CAN_Address.OutStop error:" + ex.ToString());
            }
            return true ;
        }
        public static bool SetOutStopType(uint nodeid)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.OutStop_ParamCount, 2);
            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.OutStop_Set, 3);
            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.OutStop_Type, 0);
            return CheckWriteResult(result, "CAN_Address.SetOutStopType");
        }
        public static EventCall_T evencb = new EventCall_T(EventCall);
        public static PUSIResult EventCall(CB_TYPE callbackType, IntPtr pData)
        {
            switch (callbackType)
            {
                case CB_TYPE.CBT_ConsoleWriteLine:
                    {
                        /*string strConsole.WriteLine = Marshal.PtrToStringAnsi(pData);
                        if (mainform != null)
                            mainform.Console.WriteLine(strConsole.WriteLine);*/
                    }
                    break;
                case CB_TYPE.CBT_SLAVE_STATUS_CHANGE:
                    break;
                case CB_TYPE.CBT_PDO_DATA:
                    break;
                default:
                    break;
            }
            return PUSIResult.RET_SUCCESS;
        }
        public static int GetPosition(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.CurrPosition, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error( " node ["+ nodeid + "] GetPosition失败，结果：" + result);
            }
            return value;
        }
        public static int GetMaxSpeed(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.MaxSpeed, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetMaxSpeed失败，结果：" + result);
            }
            return value;
        }

        public static int GetAddSpeed(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.AddSpeed, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetAddSpeed失败，结果：" + result);
            }
            return value;
        }

        public static int GetDelSpeed(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.DelSpeed, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetDelSpeed失败，结果：" + result);
            }
            return value;
        }
        public static int GetStartSpeed(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.StartSpeed, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetStartSpeed失败，结果：" + result);
            }
            return value;
        }
        public static int GetStopSpeed(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.StopSpeed, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetStopSpeed失败，结果：" + result);
            }
            return value;
        }
        /// <summary>
        /// 获取细分数
        /// </summary> 
        public static int GetMicroStepping(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.MicroStepping, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetMicroStepping失败，结果：" + result);
            } return value;
        }
        /// <summary>
        /// 获取峰值电流
        /// </summary> 
        public static int GetMaxPhaseCurrent(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.MaxPhaseCurrent, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetMaxPhaseCurrent失败，结果：" + result);
            } return value;
        }
        /// <summary>
        /// 获取电流衰减
        /// </summary> 
        public static int GetDelPhaseCurrent(uint nodeid)
        {
            int value;
            PUSIResult result = PUSICANControl.ReadSDO(nodeid, CAN_Address.DelPhaseCurrent, out value);
            if (!PUSIResult.RET_SUCCESS.Equals(result))
            {
                LogUtil.error(" node [" + nodeid + "] GetDelPhaseCurrent失败，结果：" + result);
            } return value;
        }
        private static bool CheckWriteResult(PUSIResult result, string Addr)
        {
            if (result.Equals(PUSIResult.RET_SUCCESS))
            {
                return true;
            }
            else
            {
                LogUtil.error(" WriteSDO " + Addr + "失败，结果：" + result);
                return false;
            }
        }
        public static bool SetPosition(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.CurrPosition, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.CurrPosition");
        }
        public static bool SetMaxSpeed(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.MaxSpeed, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.MaxSpeed");
        }

        public static bool SetAddSpeed(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.AddSpeed, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.AddSpeed");
        }

        public static bool SetDelSpeed(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.DelSpeed, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.DelSpeed");
        }
        public static bool SetStartSpeed(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.StartSpeed, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.StartSpeed");
        }
        public static bool SetStopSpeed(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.StopSpeed, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.StopSpeed");
        }

        public static bool SetMicroStepping(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.MicroStepping, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.MicroStepping");
        }

        public static bool SetMaxPhaseCurrent(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.MaxPhaseCurrent, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.MaxPhaseCurrent");
        }

        public static bool SetDelPhaseCurrent(uint nodeid, int value)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.DelPhaseCurrent, value);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.DelPhaseCurrent");
        }
        public static bool AbsMove(uint nodeid, int position)
        {
            if (PUSICANControl.CheckStop2IsHigh(nodeid))
            {
                PUSICANControl.ClearStatus(nodeid);
                Thread.Sleep(100);
            }
            
            //写模式 
            PUSIResult result;
            //if (!(GetSDOData(nodeid, CAN_Address.MoveType).Equals(0)))
            {
                PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveType, 0);
            }

            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.AbsoluteMove, position);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.AbsoluteMove");
        }
        /// <summary>
        /// 相对运动
        /// </summary> 
        public static bool RelMove(uint nodeId, int position)
        {
            if (PUSICANControl.CheckStop2IsHigh(nodeId))
            {
                PUSICANControl.ClearStatus(nodeId);
                Thread.Sleep(100);
            }
            //写模式 
            PUSIResult result;
            if (!(GetSDOData(nodeId, CAN_Address.MoveType).Equals(0)))
            {
                PUSICANControl.WriteSDO(nodeId, CAN_Address.MoveType, 0);
            }
            if (position > 0)
            {
                PUSICANControl.WriteSDO(nodeId, CAN_Address.MoveDirection, 1);
            }
            else
            {
                PUSICANControl.WriteSDO(nodeId, CAN_Address.MoveDirection, 0);
            }
            int value = GetMaxSpeed(nodeId);
            result = PUSICANControl.WriteSDO(nodeId, CAN_Address.RelativeMove, Math.Abs(position));
            return CheckWriteResult(result, " node [" + nodeId + "] CAN_Address.RelativeMove");
        }
        /// <summary>
        /// 匀速运动
        /// </summary> 
        public static bool VolMove(uint nodeid, int speed)
        {
            //写模式 
            PUSIResult result; 
            PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveType, 1); 
            PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveDirection, 0);
            SetMaxSpeed(nodeid, speed);
            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.RelativeMove, (speed));
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.RelativeMove");
        }
        /// <summary>
        /// 原点返回
        /// </summary> 
        public static bool HomeMove(uint nodeid, int speed)
        {
            //写模式 
            PUSIResult result;
            PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveType, 1);
            if (speed < 0)
            { 
                PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveDirection, 1);
            }
            else
            {
                PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveDirection, 0);
            }
            SetMaxSpeed(nodeid, speed);
            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.RelativeMove, Math.Abs(speed));
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.RelativeMove");
        }
        /// <summary>
        /// 原点返回 ,旋转轴原点返回使用的方法
        /// </summary> 
        public static bool HomeMove(uint nodeid,bool isLineSlv=false  )
        {
            int speed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_HomeSpeed, nodeid.ToString());
            if (isLineSlv)
            {
                speed = ConfigAppSettings.GetIntValue(Setting_Init.Line_HomeSpeed, nodeid.ToString());
            }
           //写模式 
            PUSIResult result;
            PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveType, 1);
            if (speed < 0)
            { 
                PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveDirection, 1);
            }
            else
            {
                PUSICANControl.WriteSDO(nodeid, CAN_Address.MoveDirection, 0);
            }
            SetMaxSpeed(nodeid,  (speed));
            result = PUSICANControl.WriteSDO(nodeid, CAN_Address.RelativeMove, Math.Abs(speed));
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.RelativeMove");
        }

        /// <summary>
        /// 判断外部停止2是否亮
        /// </summary>
        /// <param name="nodeId"></param>
        /// <returns></returns>
        public static bool CheckStop2IsHigh(uint nodeId)
        {
            //判断 外部停止2=1
            int value = 0;
            //获取状态
            if (PUSIResult.RET_SUCCESS == PUSICANControl.ReadSDO(nodeId, CAN_Address.ControlStatus, out value))
            { 
                string stop2Str = Convert.ToString(value, 2).PadLeft(5, '0').Substring(3, 1);
                if (  stop2Str.Equals("1"))
                {
                    return true;
                }
                else
                {
                    return false;
                }
            } return false;
        }
        public static void ClearStatus(uint nodeId)
        {
            //清理驱动器状态
            PUSIResult result = PUSICANControl.WriteSDO(nodeId, CAN_Address.ControlStatus, 1); 
        }
        public static bool RHomeMoveEnd(uint nodeId)
        { 
            //清理驱动器状态
            PUSIResult result = PUSICANControl.WriteSDO(nodeId, CAN_Address.ControlStatus, 1);
            
            Thread.Sleep(100);
            //重置位置
            SetPosition(nodeId, 0);

            ////重置速度
            //int startSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_StartSpeed, nodeId.ToString());
            //PUSICANControl.SetStartSpeed(nodeId, startSpeed);
            //int stopSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_StopSpeed, nodeId.ToString());
            //PUSICANControl.SetStopSpeed(nodeId, stopSpeed);

            //int maxSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_MaxSpeed, nodeId.ToString());
            //PUSICANControl.SetMaxSpeed(nodeId, maxSpeed);

            return true;
        }

        public static bool SetSpeed(uint nodeId, bool isLineSlv = false)
        {
            //重置速度
            int startSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_StartSpeed, nodeId.ToString());
            int stopSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_StopSpeed, nodeId.ToString());
            int maxSpeed = ConfigAppSettings.GetIntValue(Setting_Init.RotateNode_MaxSpeed, nodeId.ToString());

            if (isLineSlv)
            {

                if (ConfigAppSettings.GetIntValue(Setting_Init.Line_StartSpeed, nodeId.ToString()) > 0)
                {
                    startSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_StartSpeed, nodeId.ToString());
                    stopSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_StopSpeed, nodeId.ToString());
                    maxSpeed = ConfigAppSettings.GetIntValue(Setting_Init.Line_MaxSpeed, nodeId.ToString());
                }
            }
            PUSICANControl.SetStartSpeed(nodeId, startSpeed);
            PUSICANControl.SetStopSpeed(nodeId, stopSpeed);
            PUSICANControl.SetMaxSpeed(nodeId, maxSpeed);

            return true;
        }
        public static bool StopMove(uint nodeid)
        {
            PUSIResult result = PUSICANControl.WriteSDO(nodeid, CAN_Address.StopMove, 0);
            return CheckWriteResult(result, " node [" + nodeid + "] CAN_Address.StopMove");
        }

        public static string GetStatus(SLAVE_STATUS status)
        {
            string strStatus;
            switch (status)
            {
                case 0x00:
                    strStatus = "Initialisation";
                    break;
                case SLAVE_STATUS.SLAVESTATUS_INIT:
                    strStatus = "Disconnected";
                    break;
                case SLAVE_STATUS.Connecting:
                    strStatus = "Connecting";
                    break;
                case SLAVE_STATUS.SLAVESTATUS_STOP:
                    strStatus = "Stopped";
                    break;
                case SLAVE_STATUS.SLAVESTATUS_WORK:
                    strStatus = "Operational";
                    break;
                case SLAVE_STATUS.SLAVESTATUS_PREWORK:
                    strStatus = "Pre_operational";
                    break;
                case SLAVE_STATUS.SLAVESTATUS_DISCONNECT:
                    strStatus = "Unknown_state";
                    break;
                default:
                    strStatus = "error";
                    break;
            }
            return strStatus;
        }
        public static string GetStatus(uint status)
        {
            string strStatus;
            switch (status)
            {
                case 0x00:
                    strStatus = "Initialisation";
                    break;
                case 0x01:
                    strStatus = "Disconnected";
                    break;
                case 0x02:
                    strStatus = "Connecting";
                    break;
                case 0x04:
                    strStatus = "Stopped";
                    break;
                case 0x05:
                    strStatus = "Operational";
                    break;
                case 0x7f:
                    strStatus = "Pre_operational";
                    break;
                case 0x0f:
                    strStatus = "Unknown_state";
                    break;
                default:
                    strStatus = "error";
                    break;
            }
            return strStatus;
        }
        /// <summary>
        /// 判断原点返回是否完成
        /// </summary> 
        public static bool IsHomeEnd(uint nodeId)
        {
            //判断忙碌状态=0并且外部停止2=1
            int value = 0;
            //获取状态
            if (PUSIResult.RET_SUCCESS == PUSICANControl.ReadSDO(nodeId, CAN_Address.ControlStatus, out value))
            {
                string busyStr = Convert.ToString(value, 2).PadLeft(5, '0').Substring(1, 1);
                string stop2Str = Convert.ToString(value, 2).PadLeft(5, '0').Substring(3, 1);
                string stop1Str = Convert.ToString(value, 2).PadLeft(5, '0').Substring(4, 1);
                if (busyStr.Equals("0") && (stop2Str.Equals("1") || stop1Str.Equals("1")))
                {
                    return true ;
                }
                else
                {
                    return false ;
                } 
            }  
            return false ;
        }
        public  static void DefatutPosMove(uint node, int targetDefaultPosition, int movetype = 0,bool isLineSlv=false )
        {
            PUSICANControl.RHomeMoveEnd(node);
            Thread.Sleep(10);
            //设置为位置模式
            PUSICANControl.WriteSDO(node, CAN_Address.MoveType, movetype);
            Thread.Sleep(100);
            bool isLineslv = node.Equals(isLineSlv);
            PUSICANControl.SetSpeed(node);
            Thread.Sleep(10);
            //所有轴走到待机点
            PUSICANControl.AbsMove(node, targetDefaultPosition);
        }
        /// <summary>
        /// 运动是否结束
        /// </summary> 
        public static bool MoveIsEnd(uint nodeId)
        {
            //判断忙碌状态=0 当前坐标=targetPosition
            int value = 0;
            //获取状态
            if (PUSIResult.RET_SUCCESS == PUSICANControl.ReadSDO(nodeId, CAN_Address.ControlStatus, out value))
            {
                string busyStr = Convert.ToString(value, 2).PadLeft(5, '0').Substring(1, 1);
                if (busyStr.Equals("1"))
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
            return false;
        }
        /// <summary>
        /// 运动是否结束
        /// </summary> 
        public static bool IsDuZhuan(uint nodeId)
        {
            //判断忙碌状态=0 当前坐标=targetPosition
            int value = 0;
            //获取状态
            if (PUSIResult.RET_SUCCESS == PUSICANControl.ReadSDO(nodeId, CAN_Address.ControlStatus, out value))
            {
                string str = Convert.ToString(value, 2).PadLeft(5, '0');

                string busyStr = Convert.ToString(value, 2).PadLeft(5, '0').Substring(2, 1);
                if (busyStr.Equals("1"))
                {
                    return true ;
                }
                else
                {
                    return false ;
                }
            }
            return false;
        }
        /// <summary>
        /// 运动是否结束
        /// </summary> 
        public static bool MoveIsEnd(uint nodeId, int targetPosition,out string waitMsg)
        {
            //判断忙碌状态=0 当前坐标=targetPosition
            int currPosition = GetPosition(nodeId);
            int cha = Math.Abs(targetPosition - currPosition);
            waitMsg = "当前坐标[" + currPosition + "]";
            if (cha < 10)
            {
                int value = 0;
                //获取状态
                if (PUSIResult.RET_SUCCESS == PUSICANControl.ReadSDO(nodeId, CAN_Address.ControlStatus, out value))
                {
                    string busyStr = Convert.ToString(value, 2).PadLeft(5, '0').Substring(1, 1);
                    waitMsg = waitMsg + "忙碌状态["+busyStr+"]";
                    if (busyStr.Equals("1"))
                    {
                        return false;
                    }
                    else
                    {
                        return true;
                    }
                    //lblControlStatus.Text = ("状态：外部停止3:[" + str.Substring(0, 1) + "],busy状态:[" + str.Substring(1, 1) + "],堵转状态:["
                    //    + str.Substring(2, 1) + "],外部停止2:[" + str.Substring(3, 1) + "],外部停止1:[" + str.Substring(4, 1) + "]");
                }  
            }
            return false ;
        }
        public static List<string> getAdapterList()
        {
            List<string> list = new List<string>();
            list.Add("蓝马电子A1+");
            list.Add("乐的USB-CAN");
            list.Add("YL9100");
            list.Add("IXXAT");
            list.Add("广成USBCAN-I");
            list.Add("周立功USBCAN-I");
            list.Add("周立功USBCAN-E-U");
            list.Add("周立功CANET-TCPC");
            list.Add("创芯CANalyst-II");
            list.Add("周立功CANCOM-100IE");
            return list;
        }
        public static List<string> getBoauList()
        {
            List<string> list = new List<string>();
            list.Add("10Kbps");
            list.Add("20Kbps");
            list.Add("50Kbps");
            list.Add("100Kbps");
            list.Add("125Kbps");
            list.Add("250Kbps");
            list.Add("500Kbps");
            list.Add("800Kbps");
            list.Add("1000Kbps");
            return list;
        }
        public static List<string> getXifenList()
        {
            List<string> list = new List<string>();
            list.Add("0");
            list.Add("2");
            list.Add("4");
            list.Add("8");
            list.Add("16");
            list.Add("32");
            list.Add("64");
            list.Add("128");
            list.Add("256");
            return list;
        }

    }

}