using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MotorMaster.Sdk;

namespace Neotel
{
    /// <summary>
    /// 增广智能电动夹爪（RM-C控制器）
    /// </summary>
    public class Rmaxis
    {
        private RMAxis device;
        private readonly log4net.ILog LOG;
        private readonly bool initOK;
        public readonly string[] ErrorString = new string[] { "正常", "通用错误", "电机失相", "位置超差", "速度超差", "电机堵转", "初相励磁错误" };
        
        /// <summary>
        /// 增广智能电动夹爪（RM-C控制器）
        /// </summary>
        /// <param name="logName">日志名</param>
        public Rmaxis(string logName = "Rmaxis")
        {
            LOG = log4net.LogManager.GetLogger(logName);

            try
            {
                //使用SDK前，动态加载DLL和Config
                RMAxis.Initialize(Environment.CurrentDirectory + "\\SDK\\MotorMaster.Sdk.dll");
                RMAxis.SetConfigPath(Environment.CurrentDirectory + "\\SDK\\MotorMaster.config");
                LOG.Info("Rmaxis Init OK");
                initOK = true;
            }
            catch (Exception ex)
            {
                LOG.Error("Rmaxis Init", ex);
                initOK = false;
            }
        }

        /// <summary>
        /// 串口是否连接
        /// </summary>
        public bool IsPortOpen { private set; get; } = false;

        ///// <summary>
        ///// 伺服是否打开
        ///// </summary>
        //public bool IsServoOn
        //{
        //    get => device.ServoOnOff;
        //}

        /// <summary>
        /// 轴是否在运动
        /// </summary>
        public bool IsMoving { get => device.IsMoving(); }

        /// <summary>
        /// 轴是否到达运动目标
        /// </summary>
        public bool IsReached { get => device.IsReached(); }

        /// <summary>
        /// 轴是否推空（可以在Push完成后检测是否推压/夹持为空）
        /// </summary>
        public bool IsPushEmpty { get => device.IsPushEmpty(); }

        /// <summary>
        /// 错误代码
        /// </summary>
        public int ErrorCode { get => (int)device.ErrorCode; }

        //========================================
        /// <summary>
        /// 打开串口
        /// </summary>
        /// <param name="portName">串口名</param>
        /// <param name="axisNo">轴号</param>
        /// <returns></returns>
        public bool OpenPort(string portName, ushort axisNo = 0)
        {
            if (initOK)
            {
                try
                {
                    device = RMAxis.CreateModbus("\\\\.\\" + portName.ToUpper(), 115200, axisNo);
                    LOG.Info("OpenPort OK");
                    IsPortOpen = true;
                }
                catch (Exception ex)
                {
                    LOG.Error("OpenPort", ex);
                    IsPortOpen = false;
                }
            }
            else
            {
                IsPortOpen = false;
            }
            return IsPortOpen;
        }

        /// <summary>
        /// 关闭串口
        /// </summary>
        public void ClosePort()
        {
            if (!initOK) return;
            if (device == null) return;
            IsPortOpen = false;

            try
            {
                RMAxis.Destroy(device);
                device = null;
                LOG.Info("ClosePort OK");
            }
            catch (Exception ex)
            {
                LOG.Error("ClosePort", ex);
            }
        }

        /// <summary>
        /// 重置控制器错误
        /// </summary>
        public void ResetError()
        {
            if (!IsPortOpen) return;
            try
            {
                device.ResetError();
                LOG.Info("ResetError OK");
            }
            catch (Exception ex)
            {
                LOG.Error("ResetError", ex);
            }
        }

        ///// <summary>
        ///// 打开伺服
        ///// </summary>
        //public void ServoOn()
        //{
        //    if (!IsPortOpen) return;
        //    try
        //    {
        //        if (!device.ServoOnOff)
        //            device.ServoOnOff = true;
        //        LOG.Info("ServoOn OK");
        //    }
        //    catch (Exception ex)
        //    {
        //        LOG.Error("ServoOn", ex);
        //    }
        //}

        ///// <summary>
        ///// 关闭伺服
        ///// </summary>
        //public void ServoOff()
        //{
        //    if (!IsPortOpen) return;
        //    try
        //    {
        //        if (device.ServoOnOff)
        //            device.ServoOnOff = false;
        //        LOG.Info("ServoOff OK");
        //    }
        //    catch (Exception ex)
        //    {
        //        LOG.Error("ServoOff", ex);
        //    }
        //}

        /// <summary>
        /// 停止轴运动
        /// </summary>
        public void StopAxis()
        {
            if (!IsPortOpen) return;
            try
            {
                device.Stop();
                LOG.Info("StopAxis OK");
            }
            catch (Exception ex)
            {
                LOG.Error("StopAxis", ex);
            }
        }

        //========================================
        /// <summary>
        /// 控制器版本号
        /// </summary>
        /// <returns></returns>
        public string GetVersion()
        {
            RMAxis.Native.version_t version = device.Version;
            return string.Format("{0}.{1}.{2}.{3}", version.major, version.minor, version.build, version.type);
        }

        /// <summary>
        /// 获取当前设置的出力值
        /// </summary>
        /// <returns>出力，%</returns>
        public float GetTorque()
        {
            float n = -1;
            if (!IsPortOpen) return n;
            try
            {
                n = device.Torque;
                LOG.Info("GetTorque OK");
            }
            catch (Exception ex)
            {
                n = -1;
                LOG.Error("GetTorque", ex);
            }
            return n;
        }

        ///// <summary>
        ///// 获取电机运动中的出力
        ///// </summary>
        ///// <returns>出力，%</returns>
        //public float GetForce()
        //{
        //    float n = -1;
        //    if (!IsPortOpen) return n;
        //    try
        //    {
        //        n = device.ForceSensor;
        //        LOG.Info("GetForce OK");
        //    }
        //    catch (Exception ex)
        //    {
        //        n = -1;
        //        LOG.Error("GetForce", ex);
        //    }
        //    return n;
        //}

        /// <summary>
        /// 获取当前电机速度
        /// </summary>
        /// <returns>速度，mm/s</returns>
        public float GetVelocity()
        {
            float n = -1;
            if (!IsPortOpen) return n;
            try
            {
                n = device.Velocity;
                LOG.Info("GetVelocity OK");
            }
            catch (Exception ex)
            {
                n = -1;
                LOG.Error("GetVelocity", ex);
            }
            return n;
        }

        /// <summary>
        /// 获取当前位置
        /// </summary>
        /// <returns>位置，mm</returns>
        public float GetPosition()
        {
            float n = -1;
            if (!IsPortOpen) return n;
            try
            {
                n = device.Position;
                LOG.Info("GetPosition OK");
            }
            catch (Exception ex)
            {
                n = -1;
                LOG.Error("GetPosition", ex);
            }
            return n;
        }

        //========================================
        /// <summary>
        /// 回原点
        /// </summary>
        public void GoHome()
        {
            if (!IsPortOpen) return;
            LOG.Info("GoHome Start");

            try
            {
                device.GoHome();
                LOG.Info("GoHome OK");
            }
            catch (Exception ex)
            {
                LOG.Error("GoHome", ex);
            }
        }

        /// <summary>
        /// 推压运动
        /// </summary>
        /// <param name="force">出力，%</param>
        /// <param name="distance">距离，mm</param>
        /// <param name="velocity">速度，mm/s</param>
        public void Push(float force, float distance, float velocity)
        {
            if (!IsPortOpen) return;
            LOG.Info("Push Start");

            try
            {
                device.Push(force, distance, velocity);
                LOG.Info("Push OK");
            }
            catch (Exception ex)
            {
                LOG.Error("Push", ex);
            }
        }

        /// <summary>
        /// 绝对运动
        /// </summary>
        /// <param name="position">位置，mm</param>
        /// <param name="velocity">速度，mm/s</param>
        /// <param name="acceleration">加速度，mm/s^2</param>
        /// <param name="deacceleration">减速度，mm/s^2</param>
        /// <param name="band">定位范围，mm</param>
        public void MoveAbsolute(float position, float velocity, float acceleration, float deacceleration, float band)
        {
            if (!IsPortOpen) return;
            LOG.Info("MoveAbsolute Start");

            try
            {
                device.MoveAbsolute(position, velocity, acceleration, deacceleration, band);
                LOG.Info("MoveAbsolute OK");
            }
            catch (Exception ex)
            {
                LOG.Error("MoveAbsolute", ex);
            }
        }

        /// <summary>
        /// 直接运动到指定位置
        /// </summary>
        /// <param name="position">位置，mm</param>
        public void MoveTo(float position)
        {
            if (!IsPortOpen) return;
            LOG.Info("MoveTo Start");

            try
            {
                device.MoveTo(position);
                LOG.Info("MoveTo OK");
            }
            catch (Exception ex)
            {
                LOG.Error("MoveTo", ex);
            }
        }

        /// <summary>
        /// 直接运动的参数
        /// </summary>
        /// <param name="velocity">速度，mm/s</param>
        /// <param name="acceleration">加速度，mm/s^2</param>
        /// <param name="deacceleration">减速度，mm/s^2</param>
        public void MoveConfig(float velocity, float acceleration, float deacceleration)
        {
            if (!IsPortOpen) return;
            try
            {
                device.ConfigMotion(velocity, acceleration, deacceleration);
                LOG.Info("MoveConfig OK");
            }
            catch (Exception ex)
            {
                LOG.Error("MoveConfig", ex);
            }
        }

        /// <summary>
        /// 回原点，并等到到位
        /// </summary>
        /// <param name="timeout">超时时间，ms</param>
        /// <returns></returns>
        public bool GoHomeWait(int timeout)
        {
            if (!IsPortOpen) return false;
            LOG.Info("GoHomeWait Start");
            bool result;

            try
            {
                device.GoHome();
                //device.WaitComplete((uint)timeout);
                //result = device.IsReached();
                result = Wait(timeout);
                if (result)
                    LOG.Info("GoHomeWait OK");
                else
                    LOG.Info("GoHomeWait Timeout");
            }
            catch (Exception ex)
            {
                result = false;
                LOG.Error("GoHomeWait", ex);
            }
            return result;
        }

        /// <summary>
        /// 推压运动，并等到到位
        /// </summary>
        /// <param name="force">出力，%</param>
        /// <param name="distance">距离，mm</param>
        /// <param name="velocity">速度，mm/s</param>
        /// <param name="timeout">超时时间，ms</param>
        /// <returns></returns>
        public bool PushWait(float force, float distance, float velocity, int timeout)
        {
            if (!IsPortOpen) return false;
            LOG.Info("PushWait Start");
            bool result;

            try
            {
                device.Push(force, distance, velocity);
                //device.WaitComplete((uint)timeout);
                //result = device.IsReached();
                result = Wait(timeout);
                if (result)
                    LOG.Info("PushWait OK");
                else
                    LOG.Info("PushWait Timeout");
            }
            catch (Exception ex)
            {
                result = false;
                LOG.Error("PushWait", ex);
            }
            return result;
        }

        /// <summary>
        /// 绝对运动，并等到到位
        /// </summary>
        /// <param name="position">位置，mm</param>
        /// <param name="velocity">速度，mm/s</param>
        /// <param name="acceleration">加速度，mm/s^2</param>
        /// <param name="deacceleration">减速度，mm/s^2</param>
        /// <param name="band">定位范围，mm</param>
        /// <param name="timeout">超时时间，ms</param>
        /// <returns></returns>
        public bool MoveAbsoluteWait(float position, float velocity, float acceleration, float deacceleration, float band, int timeout)
        {
            if (!IsPortOpen) return false;
            LOG.Info("MoveAbsoluteWait Start");
            bool result;

            try
            {
                device.MoveAbsolute(position, velocity, acceleration, deacceleration, band);
                //device.WaitComplete((uint)timeout);
                //result = device.IsReached();
                result = Wait(timeout);
                if (result)
                    LOG.Info("PushWait OK");
                else
                    LOG.Info("PushWait Timeout");
            }
            catch (Exception ex)
            {
                result = false;
                LOG.Error("MoveAbsoluteWait", ex);
            }
            return result;
        }

        /// <summary>
        /// 直接运动到指定位置，并等到到位
        /// </summary>
        /// <param name="position">位置，mm</param>
        /// <param name="timeout">超时时间，ms</param>
        /// <returns></returns>
        public bool MoveToWait(float position, int timeout)
        {
            if (!IsPortOpen) return false;
            LOG.Info("MoveToWait Start");
            bool result;

            try
            {
                device.MoveTo(position);
                //device.WaitComplete((uint)timeout);
                //result = device.IsReached();
                result = Wait(timeout);
                if (result)
                    LOG.Info("PushWait OK");
                else
                    LOG.Info("PushWait Timeout");
            }
            catch (Exception ex)
            {
                result = false;
                LOG.Error("MoveToWait", ex);
            }
            return result;
        }



        private bool Wait(int timeout)
        {
            int n = 0;
            int mm = 10;
            bool result = true;
            System.Threading.Thread.Sleep(mm);

            while (!device.IsReached())
            {
                if (n >= timeout)
                {
                    result = false;
                    break;
                }
                n += mm;
                System.Threading.Thread.Sleep(mm);
                System.Windows.Forms.Application.DoEvents();
            }
            return result;
        }



    }
}