using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.Util;
using MECF.Framework.Common.Device.LinMot;
using CyberX8_Core;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.ToolLayout;
using System.Diagnostics;
using Aitex.Core.RT.SCCore;
namespace CyberX8_RT.Devices.LinMot
{
    public class LinMotAxis : BaseDevice, IDevice
    {
        #region 常量
        /// 
        /// 控制字
        /// 
        private const string STATUS_WORD = "StatusWord";
        private const string HOME = "Home";
        private const string SWITCH_ON = "SwitchOn";
        private const string DIRECTION = "Direction";
        private const string POSITION = "Position";
        private const string SCAN_COUNT = "ScanCount";
        /// 
        /// 下一指令判定条件与_startPosition的距离
        /// 
        private const double _nextInterval = 2;
        #endregion
        #region 内部变量
        /// 
        /// Home状态
        /// 
        private bool _isHomed;
        /// 
        /// SwitchOn状态
        /// 
        private bool _isSwitchOn;
        /// 
        /// Disabled状态
        /// 
        private bool _isDisabled;
        /// 
        /// 错误状态
        /// 
        private bool _isError;
        /// 
        /// 致命错误状态
        /// 
        private bool _isFatalError;
        /// 
        /// 运动状态
        /// 
        private bool _isMotorOn;
        /// 
        /// 是否到达目标
        /// 
        private bool _inTagertPosition = false;
        /// 
        /// 操作当前状态
        /// 
        private RState _status;
        /// 
        /// 当前操作指令 
        /// 
        private LinMotOperation _currentOperation = LinMotOperation.None;
        /// 
        /// 状态字
        /// 
        private short _statusWord;
        /// 
        /// LinMotId
        /// 
        private byte _linmotId;
        /// 
        /// 错误代码
        /// 
        private short _errorCode;
        /// 
        /// Reset Routine
        /// 
        private LinMotResetRoutine _resetRoutine;
        /// 
        /// Stop Motor Routine
        /// 
        private LinMotStopMotorRoutine _stopMotorRoutine;
        /// 
        /// Start Curve
        /// 
        private LinMotStartContinueCurveRoutine _startContinueCurveRoutine;
        /// 
        /// Start VAI
        /// 
        private LinMotStartVAIPositionRoutine _startVAIPositionRoutine;
        /// 
        /// Curve速度
        /// 
        private int _curverSpeed = 100;
        /// 
        /// Curver指定的Id
        /// 
        private byte _curveId = 1;
        /// 
        /// 指令发送时间(用于空闲时发送读取状态指令)
        /// 
        private DateTime _sendTime = DateTime.Now;
        /// 
        /// 运动时间(用于判定运动指令发送LinMot并没有执行上报超时)
        /// 
        private DateTime _motorTime = DateTime.Now;
        /// 
        /// 设备参数对象
        /// 
        private LinMotDevice _linMotDeviceParam;
        /// 
        /// 开始时的方向
        /// 
        private string _startDirection = "";
        /// 
        /// 当前GAI GO To Position方向 
        /// 
        private string _direction = "";
        /// 
        /// 当前位置
        /// 
        private double _currentPosition =0;
        /// 
        /// 当前位置(类型为整形)
        /// 
        private int _currentIntPosition = 0;
        /// 用于prewet页面UI的位置数据
        /// 
        private double _currentPrewetUIPostion = 0;
        /// 
        /// 
        /// 判定方向位置
        /// 
        private double _judgePosition = 0;
        /// 
        /// GAI运动前的位置
        /// 
        private double _startPosition = 0;
        /// 
        /// 状态字时间
        /// 
        private DateTime _statusWordDateTime=DateTime.Now;
        /// 
        /// linmot stop watch
        /// 
        private Stopwatch _linmotStopWatch = new Stopwatch();
        /// 
        /// 状态字上升沿信号 
        /// 
        private R_TRIG _statusTrigger = new R_TRIG();
        #endregion
        #region 属性
        /// 
        /// 操作当前状态
        /// 
        public RState Status { get { return _status; } }
        /// 
        /// Home
        /// 
        public bool IsHomed { get { return IsConnectd && _isHomed; } }
        /// 
        /// SwitchOn状态
        /// 
        public bool IsSwitchOn { get { return IsConnectd && _isSwitchOn; } }
        /// 
        /// 错误
        /// 
        public bool IsError { get { return _isError; } }
        /// 
        /// Disabled状态
        /// 
        public bool IsDisabled { get { return IsConnectd&&_isDisabled; } }
        /// 
        /// 运动状态
        /// 
        public bool IsMotorOn 
        { 
            get 
            {
                if(_isMotorOn)
                {
                    return true;
                }
                else
                {
                    if (_linmotStopWatch.ElapsedMilliseconds <= 2000)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                }
            } 
        }
        /// 
        /// 是否已经到达目标
        /// 
        public bool InTargetPosition { get { return _inTagertPosition; } }
        /// 
        /// 当前位置
        /// 
        public double CurrentPosition { get { return _currentPosition; } }
        /// 
        /// 方向 
        /// 
        public string Direction { get { return _direction; } }
        /// 
        /// 连接状态
        /// 
        public bool IsConnectd { get { return LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module.ToString()); } }
        /// 
        /// 扫描次数
        /// 
        public int ScanCount { get { return _startVAIPositionRoutine.CurrentScan; } }
        /// 
        /// 当前位置
        /// 
        public int CurrentIntPosition { get { return _currentIntPosition; } }
        /// 
        /// 最后是否SwitchOff
        /// 
        public bool LastSwitchOff { get; set; } = false;
        /// 
        /// curve速度
        /// 
        public int CurveSpeed
        {
            get { return _curverSpeed; }
        }
        /// 
        /// 错误代码
        /// 
        public short ErrorCode
        {
            get { return _errorCode; }
        }
        #endregion
        /// 
        /// 构造函数
        /// 
        /// 
        /// 
        public LinMotAxis(string moduleName) : base(moduleName, moduleName, moduleName, moduleName)
        {
            InitialData();
            InitializeOperation();
            InitializeRoute();
            _linmotStopWatch.Restart();
        }
        /// 
        /// 初始化Routine
        /// 
        private void InitializeRoute()
        {
            _resetRoutine = new LinMotResetRoutine(Module,this);
            _stopMotorRoutine=new LinMotStopMotorRoutine(Module,this);
            _startContinueCurveRoutine=new LinMotStartContinueCurveRoutine(Module,this);
            _startVAIPositionRoutine=new LinMotStartVAIPositionRoutine(Module,this);
        }
        /// 
        /// 初始化OP
        /// 
        private void InitializeOperation()
        {
            OP.Subscribe($"{Module}.Reset",(cmd,args)=> { return ResetOperation(cmd, LastSwitchOff); });
            OP.Subscribe($"{Module}.StartCurve", StartCurveOperation);
            OP.Subscribe($"{Module}.StartPosition", StartPosition);
            OP.Subscribe($"{Module}.Stop", StopOperation);
            OP.Subscribe($"{Module}.Abort", AbortOperation);
            OP.Subscribe($"{Module}.UpdateSpeedData",UpdateSpeedDataOperation);
            LinMotDeviceConfigManager.Instance.InitialDevice(Module.ToString());
            LinMotDeviceConfigManager.Instance.SubscribeModuleVariable(Module, UpdateStatusWord);
        }
        /// 
        /// 初始化数据
        /// 
        private void InitialData()
        {
            _linMotDeviceParam = LinMotDeviceConfigManager.Instance.GetLinMotDevice(Module);
            if (_linMotDeviceParam == null)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), "Get Address Id Error"); ;
                return;
            }
            if (byte.TryParse(_linMotDeviceParam.Address, out var tmpLinmotId))
            {
                _linmotId = tmpLinmotId;
            }
            DATA.Subscribe($"{Module}.{STATUS_WORD}", () => IsConnectd?_statusWord:0,SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{HOME}", () => IsHomed,SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{SWITCH_ON}", () => IsSwitchOn, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{DIRECTION}", () => _direction,SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{POSITION}", () => _currentPosition, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{SCAN_COUNT}",()=>ScanCount,SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.Speed", () => _curverSpeed, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.IsConnectd", () => IsConnectd, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.CurrentPrewetUIPostion", () => _currentPrewetUIPostion, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.LinmotDeviceData", () => _linMotDeviceParam.LinMotDeviceData, SubscriptionAttribute.FLAG.IgnoreSaveDB);
        }
        /// 
        /// 更新状态字
        /// 
        /// 
        /// 
        /// 
        private void UpdateStatusWord(short statusWord,short errorCode,int position)
        {
            _statusWordDateTime=DateTime.Now;
            _statusTrigger.CLK = _statusWord != statusWord;
            if (_statusTrigger.Q)
            {
                LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"statusword is {statusWord}");
            }
            _statusWord = statusWord;
            UpdateStatusWord(statusWord);
            short tmpErrorCode = errorCode;
            NotifyError(tmpErrorCode);
            //ContinueGAIGotoPosition();
            double tmpPosition = (double)position / 10000;
            if (Math.Abs(tmpPosition - _currentPosition) >= 0.1)
            {
                _currentPosition = tmpPosition;
                bool enableLog = SC.GetValue("Log.EnableLinmotLog");
                if (enableLog)
                {
                    LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"motor status {_isMotorOn} position {_currentPosition}");
                }
            }
            _currentIntPosition = position;
            if (_linMotDeviceParam.LinMotDeviceData != null && _linMotDeviceParam.LinMotDeviceData.TopPosition - _linMotDeviceParam.LinMotDeviceData.BottomPosition != 0)
            {
                double proportion = _linMotDeviceParam.LinMotDeviceData.TopPosition - _linMotDeviceParam.LinMotDeviceData.BottomPosition;
                _currentPrewetUIPostion = 150 - 90 * Math.Abs(position / proportion); //150是prewet页面底部位置,90是页面顶部和底部距离之差
            }
            JudgeDirection();
        }
        /// 
        /// 更新StatusWord
        /// 
        /// 
        private void UpdateStatusWord(short statusWord)
        {
            bool isHomed = (statusWord & 0b0100000000000) >> 11 == 1;//11位为Home
            if (isHomed != _isHomed)
            {
                _isHomed = isHomed;
            }
            _isDisabled = (statusWord & 0b0100) >> 2 == 0;
            _isSwitchOn = (statusWord & 0x01) == 1;
            _isError = ((statusWord & 0b01000) >> 3 == 1) || ((statusWord & 0b01000000000000) >> 12 == 1);
            _isFatalError = ((statusWord & 0b01000000000000) >> 12 == 1);
            bool isMotorOn = ((statusWord & 0b010000000000000) >> 13 == 1);//13是否正在运动
            if (isMotorOn != _isMotorOn)
            {
                _isMotorOn = isMotorOn;
                if (!_isMotorOn)
                {
                    _linmotStopWatch.Restart();
                }
            }
            _inTagertPosition = (statusWord & 0b010000000000) >> 10 == 1;//10位是否到达目标
        }
        /// 
        /// 通知错误
        /// 
        /// 
        private void NotifyError(short tmpErrorCode)
        {
            if (tmpErrorCode != _errorCode)
            {
                if (tmpErrorCode != 0)
                {
                    string errorMsg = LinMotErrorCodeManager.Instance.GetErrorCodeText(tmpErrorCode);
                    string str = string.IsNullOrEmpty(errorMsg) ? tmpErrorCode.ToString() : errorMsg;
                    if (!_isFatalError)
                    {
                        LOG.WriteLog(eEvent.ERR_LINMOT, Module, $"{str},please reset error");
                    }
                    else
                    {
                        LOG.WriteLog(eEvent.ERR_LINMOT, Module, $"{str},meets fatal error,please reboot {Module}");
                    }
                    _currentOperation = LinMotOperation.None;
                    _status = RState.Failed;
                }
                _errorCode = tmpErrorCode;
            }
        }
        /// 
        /// 判定方向 
        /// 
        private void JudgeDirection()
        {
            if(_linMotDeviceParam!=null&&_linMotDeviceParam.LinMotDeviceData!=null&&_currentOperation==LinMotOperation.StartVAIGoToPosition)
            {
                double interval = _currentPosition - _judgePosition;
                if(Math.Abs(interval)>=1)
                {
                    string tmpDirection = "";
                    if(interval>0)
                    {
                        tmpDirection = "down";
                    }
                    else
                    {
                        tmpDirection = "up";
                    }
                    if(tmpDirection!=_direction)
                    {
                        //_sendNextGAIPosition = false;
                        _direction = tmpDirection;
                    }
                    _judgePosition = _currentPosition;
                }
            }
        }
        /// 
        /// Reset
        /// 
        /// 
        /// 
        /// 
        public bool ResetOperation(string cmd, bool lastNeedSwitchoff)
        {
            if(_status==RState.Running)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT,Module.ToString(), $"{Module} current execute {_currentOperation},cannot Reset");
                return false;
            }
            if(!LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module))
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), $"device is not connected");
                return false;
            }
            _currentOperation = LinMotOperation.Reset;
            _status= _resetRoutine.Start(lastNeedSwitchoff);
            _direction = "";
            return _status==RState.Running;
        }
        /// 
        /// 开始Curve
        /// 
        /// 
        /// 
        /// 
        public bool StartCurveOperation(string cmd, object[] args)
        {
            if (_status == RState.Running)
            {
                EV.PostAlarmLog(Module.ToString(), eEvent.INFO_LINMOT, $"{Module} current execute {_currentOperation},cannot curve");
                return false;
            }
            _motorTime = DateTime.Now;
            object[] param= (object[])args[1];
            int speed = (int)param[0];
            return StartCurve(speed);
        }
        /// 
        /// 开始Curve
        /// 
        /// 
        /// 
        public bool StartCurve(int speed)
        {
            _currentOperation = LinMotOperation.StartCurve;
            _curverSpeed = speed;
            _status= _startContinueCurveRoutine.Start(_curverSpeed);
            LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"Start Curve With Speed {_curverSpeed}%");
            return _status==RState.Running;
        }
        /// 
        /// 开始Curve速度
        /// 
        /// 
        /// 
        public bool ChangeCurveSpeed(int speed)
        {
            _curverSpeed= speed;
            return WriteRamIntValue(0xCB, 0x14, speed * 100);
        }
        /// 
        /// 开始移动(GAI Goto Position)
        /// 
        /// 
        /// 
        /// 
        public bool StartPosition(string cmd, object[] args)
        {
            if (_linMotDeviceParam == null || _linMotDeviceParam.LinMotDeviceData == null)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), $"{Module} config error");
                return false;
            }
            if (_status == RState.Running)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT,Module.ToString(), $"{Module} current execute {_currentOperation},cannot Start VAI Go To Position");
                return false;
            }
            _currentOperation = LinMotOperation.StartVAIGoToPosition;
            if (_linMotDeviceParam.LinMotDeviceData.TopPosition < 0)
            {
                _direction = "up";
            }
            else
            {
                _direction = "down";
            }
            _startDirection = _direction;
            _judgePosition = _currentPosition;
            _startPosition = _currentPosition;
            int totalScan = (int)args[0] ;
            _status= _startVAIPositionRoutine.Start(totalScan, _linMotDeviceParam.LinMotDeviceData);
            return _status==RState.Running;
        }
        /// 
        /// SwitchOff
        /// 
        /// 
        /// 
        /// 
        private bool AbortOperation(string cmd, object[] args)
        {            
            if(_currentOperation==LinMotOperation.StartVAIGoToPosition)
            {
                _startVAIPositionRoutine.Abort();
            }
            _currentOperation = LinMotOperation.None;
            _status = RState.End;
            _direction = "";
            LOG.WriteLog(eEvent.INFO_LINMOT, Module, "switch off");
            return SendOperation(LinMotOperation.SwitchOff);
        }
        
        /// 
        /// 更新linmot速度参数
        /// 
        /// 
        /// 
        /// 
        private bool UpdateSpeedDataOperation(string cmd, object[] args)
        {
            string name = args[0].ToString();
            string direction = args[1].ToString();
            double maxSpeed = (double)args[2];
            int maxAcc = (int)args[3];
            int maxDec = (int)args[4];
            LinMotDeviceConfigManager.Instance.UpdateSpeedData(name, direction, maxSpeed,maxAcc,maxDec);
            LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"{Module} Speed related paramater was updated");
            return true;
        }
        /// 
        /// 发送Curve指令 
        /// 
        private void SendCurveCommand()
        {
            _motorTime = DateTime.Now;
            _sendTime = DateTime.Now;
            LinMotDeviceConfigManager.Instance.SendCurveOperation(Module.ToString(), _linmotId, _curveId,(ushort)_curverSpeed);
        }
        /// 
        /// 发送GAI GoToPosition
        /// 
        /// 
        /// 
        /// 
        /// 
        public bool SendVAIGoToPosition(int position,int speed,int accel,int decel)
        {
            _motorTime = DateTime.Now;
            _sendTime = DateTime.Now;
            return LinMotDeviceConfigManager.Instance.SendVAIGotoPosition(Module.ToString(), _linmotId, position, speed,accel, decel);
        }
        /// 
        /// Parameter Write RAM
        /// 
        /// 
        /// 
        /// 
        /// 
        public bool WriteRamIntValue(byte lowByte,byte highByte,int intValue)
        {
            _sendTime = DateTime.Now;
            return LinMotDeviceConfigManager.Instance.SendWriteRamIntValue(Module.ToString(), _linmotId, lowByte, highByte, intValue);
        }
        /// 
        /// 发送Command Table
        /// 
        /// 
        /// 
        public bool SendCommandTableOperation(short entryId)
        {
            _motorTime = DateTime.Now;
            _sendTime = DateTime.Now;
            return LinMotDeviceConfigManager.Instance.SendCommandTable(Module.ToString(), _linmotId, entryId);
        }
        /// 
        /// 发送GAI GoToPositionAfterActualCommand
        /// 
        /// 
        /// 
        /// 
        /// 
        public bool SendGAIGoToPositionAfterActualCommand(int position, int speed, int accel, int decel)
        {
            _motorTime = DateTime.Now;
            _sendTime = DateTime.Now;
           return  LinMotDeviceConfigManager.Instance.SendVAIGotoPositionAfterActualCommand(Module.ToString(), _linmotId,position, speed, accel, decel);
        }
        /// 
        /// 停止
        /// 
        /// 
        /// 
        /// 
        public bool StopOperation(string cmd, object[] args)
        {
            if (_currentOperation==LinMotOperation.StopMotor)
            {
                LOG.WriteLog(eEvent.WARN_LINMOT, Module, "current operation is stop motor,cannot repet stop motor");
                return false;
            }
            if (_currentOperation==LinMotOperation.StartVAIGoToPosition)
            {
                _startVAIPositionRoutine.Abort();
            }
            _status = RState.Running;
            _currentOperation = LinMotOperation.StopMotor;
            _stopMotorRoutine.Start(LastSwitchOff);
            _direction = "";
            return true;
        }
        /// 
        /// 定时器
        /// 
        /// 
        public bool OnTimer()
        {
            if (_status == RState.Running)
            {
                IRoutine routine = GetCurrentRoutine();
                if (routine != null)
                {
                    RState rsState = routine.Monitor();
                    if (rsState == RState.Failed || rsState == RState.Timeout)
                    {
                        _status = RState.Failed;
                        _currentOperation = LinMotOperation.None;
                    }
                    else if (rsState == RState.End)
                    {
                        _status = RState.End;
                        _currentOperation = LinMotOperation.None;
                    }
                }
            }
            SendReadStatusCommand();
            if(DateTime.Now.Subtract(_statusWordDateTime).TotalSeconds>=2)
            {
                _statusWord = 0;
                UpdateStatusWord(_statusWord);
            }
            return true;
        }
        /// 
        /// 当前Routine;
        /// 
        /// 
        private IRoutine GetCurrentRoutine()
        {
            switch(_currentOperation)
            {
                case LinMotOperation.StartVAIGoToPosition:
                    return _startVAIPositionRoutine;
                case LinMotOperation.StartCurve:
                    return _startContinueCurveRoutine;
                case LinMotOperation.Reset:
                    return _resetRoutine;
                case LinMotOperation.StopMotor:
                    return _stopMotorRoutine;
                default:
                    return null;
            }
        }
        /// 
        /// 监控
        /// 
        public void Monitor()
        {
           
        }
        /// 
        /// 发送读取状态命令
        /// 
        private void SendReadStatusCommand()
        {
            if (LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module.ToString()))
            {
                if (DateTime.Now.Subtract(_sendTime).TotalMilliseconds >= 300)
                {
                    SendOperation(LinMotOperation.ReadStatus);
                }
            }
            else
            {
                _statusWord = 0;
                UpdateStatusWord(_statusWord);
            }
        }
        /// 
        /// 发送消息
        /// 
        /// 
        public bool SendOperation(LinMotOperation operation)
        {
            _sendTime = DateTime.Now;
             return LinMotDeviceConfigManager.Instance.SendOperation(Module.ToString(), _linmotId,operation);
        }
        /// 
        /// 掉电
        /// 
        /// 
        public bool SwitchOff()
        {
            return SendOperation(LinMotOperation.SwitchOff);
        }
        /// 
        /// 中止
        /// 
        public void AbortCurrentRoutine()
        {
            StopOperation("", null);
        }
        public bool Initialize()
        {
            return true;
        }
        public void Terminate()
        {
            StopOperation("", null);
        }
        public void Reset()
        {
        }
    }
}