using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.Serialization;
using System.Text;
using System.Threading;
using System.Xml;
using Aitex.Core.Common.DeviceData;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.IOCore;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;

namespace Aitex.Core.RT.Device.Unit
{
    public class IoServoMotor : BaseDevice, IDevice
    {
        #region FSM Entity
        protected void Transition<T, V>(T state, V msg, FsmFunc func, T next)
        {
            Debug.Assert(typeof(T).IsEnum && typeof(V).IsEnum);

            int _state = Convert.ToInt32(state);
            int _next = Convert.ToInt32(next);
            int _msg = Convert.ToInt32(msg);

            Transition(_state, _msg, func, _next);
        }

        protected void Transition(int state, int msg, FsmFunc func, int next)
        {
            _fsm.Transition(state, msg, func, next);
        }

        protected void AnyStateTransition(int msg, FsmFunc func, int next)
        {
            _fsm.AnyStateTransition(msg, func, next);
        }

        protected void AnyStateTransition<T, V>(V msg, FsmFunc func, T next)
        {
            Debug.Assert(typeof(T).IsEnum && typeof(V).IsEnum);

            int _next = Convert.ToInt32(next);
            int _msg = Convert.ToInt32(msg);
            AnyStateTransition(_msg, func, _next);

        }

        protected void EnterExitTransition<T, V>(T state, FsmFunc enter, Nullable<V> msg, FsmFunc exit) where V : struct
        {
            Debug.Assert(typeof(T).IsEnum && ((msg == null) || typeof(V).IsEnum));

            int _state = Convert.ToInt32(state);

            int _msg = msg == null ? (int)FSM_MSG.NONE : Convert.ToInt32(msg);
            EnterExitTransition(_state, enter, _msg, exit);
        }

        protected void EnterExitTransition(int state, FsmFunc enter, int msg, FsmFunc exit)
        {
            _fsm.EnterExitTransition(state, enter, msg, exit);
        }


        public void PostMsg<T>(T msg, params object[] args) where T : struct
        {
            Debug.Assert(typeof(T).IsEnum);
            int id = Convert.ToInt32(msg);
            PostMsg(id, args);
        }

        public void PostMsg(int msg, params object[] args)
        {
            _fsm.PostMsg(msg, args);
            
        }
        #endregion

        public int State
        {
            get { return _fsm.State; }
        }

        public bool IsServoOn
        {
            get { return _diServoOnFeedback.Value; }
        }

        public bool IsAlarm
        {
            get { return _diAlarm.Value; }
        }

        public bool IsPulseOn
        {
            get { return _diPulseOutputOn.Value; }
        }

        public bool IsWithoutOrigin
        {
            get { return _diWithoutOrigin.Value; }
        }

        public float Acceleration
        {
            get { return _aoAcceleration.Value / 100; }
            set { _aoAcceleration.Value = value * 100; }
        }

        public float Deceleration
        {
            get { return _aoDeceleration.Value / 100; }
            set { _aoDeceleration.Value = value * 100; }
        }

        public float StartFrequency
        {
            get { return _aoStartFrequency.Value / 100; }
            set { _aoStartFrequency.Value = value * 100; }
        }

        public float ServoSpeedSetPoint
        {
            get { return _aoServoSpeedSetPoint.Value / 100; }
            set { _aoServoSpeedSetPoint.Value = value * 100; }
        }

        public float ManualSpeedSetPoint
        {
            get { return _aoManualSpeedSetPoint.Value / 100; }
            set { _aoManualSpeedSetPoint.Value = value * 100; }
        }

        public float PositionSetPoint
        {
            get { return _aoPositionSetPoint.Value / 100; }
            set { _aoPositionSetPoint.Value = value * 100; }
        }

        public float PulseFeedback
        {
            get { return _aiPulseFeedback.Value / 100; }
        }

        public float ServoSpeedFeedback
        {
            get { return _aiServoSpeedFeedback.Value / 100; }
        }

        public float ManualSpeedFeedback
        {
            get { return _aiManualSpeedFeedback.Value / 100; }
        }

        public float PositionFeedback
        {
            get { return _aiPositionFeedback.Value / 100; }
        }

        private DIAccessor _diOriginSignal;
        private DIAccessor _diCWLimitSignal;
        private DIAccessor _diCCWLimitSignal;
        private DIAccessor _diLocationComplete;
        private DIAccessor _diAlarm;
        private DIAccessor _diPulseOutputOn;
        private DIAccessor _diPulseOutputComplete;
        private DIAccessor _diWithoutOrigin;
        private DIAccessor _diServoOnFeedback;

        private DOAccessor _doBreakRelay;
        private DOAccessor _doServoOn;
        private DOAccessor _doMoveUp;
        private DOAccessor _doMoveDown;
        private DOAccessor _doMoveToPosition;
        private DOAccessor _doDeviationCounterReset;
        private DOAccessor _doAlarmReset;
        private DOAccessor _doStopMoveUp;
        private DOAccessor _doStopMoveDown;
        private DOAccessor _doLookingForOrigin;

        private AOAccessor _aoServoSpeedSetPoint;
        private AOAccessor _aoManualSpeedSetPoint;
        private AOAccessor _aoAcceleration;
        private AOAccessor _aoDeceleration;
        private AOAccessor _aoStartFrequency;
        private AOAccessor _aoPositionSetPoint;

        private AIAccessor _aiPulseFeedback;
        private AIAccessor _aiServoSpeedFeedback;
        private AIAccessor _aiManualSpeedFeedback;
        private AIAccessor _aiPositionFeedback;
        private AIAccessor _aiAccelerationFeedback;
        private AIAccessor _aiDecelerationFeedback;
        private AIAccessor _aiStartFrequencyFeedback;

        private DeviceTimer _timerServoOn = new DeviceTimer();
        private DeviceTimer _timerServoOff = new DeviceTimer();
        private DeviceTimer _timerHome = new DeviceTimer();
        private DeviceTimer _timerResetAlarm = new DeviceTimer();
        private DeviceTimer _timerMove = new DeviceTimer();
        private DeviceTimer _timerStop = new DeviceTimer();

        private SCItem<double> _scServoOnTimeout = null;
        private SCItem<double> _scResetAlarmTimeout = null;
        private SCItem<double> _scMoveTimeout = null;
        private SCItem<double> _scStopTimeout = null;
        private SCItem<double> _scHomeTimeout = null;
        private SCItem<double> _scAcceleration = null;
        private SCItem<double> _scDeceleration = null;
        private SCItem<double> _scStartFrequency = null;
        private SCItem<double> _scDefaultServoSpeed = null;
        private SCItem<double> _scDefaultManualSpeed = null;

        private R_TRIG _trigCWLimit = new R_TRIG();
        private R_TRIG _trigCCWLimit = new R_TRIG();

        private IStateMachine _fsm;
        protected Thread _thread = null;

        RD_TRIG _trigPulseOutput = new RD_TRIG();

        public IoServoMotor(string module, XmlElement node)
        {
            _fsm = new StateMachine<IoServoMotor>("ServoMotorFSM", (int)ServoState.ServoOff, 500);
            
            Transition(ServoState.ServoOff, FSM_MSG.TIMER, MonitorServoOff, ServoState.ServoOff);
            Transition(ServoState.ServoOff, ServoMsg.ServoOn, DoServoOn, ServoState.ServoOff);
            Transition(ServoState.ServoOff, ServoMsg.TrigServoOn, null, ServoState.NotHomed);

            Transition(ServoState.NotHomed, FSM_MSG.TIMER, MonitorNotHomed, ServoState.NotHomed);
            Transition(ServoState.NotHomed, ServoMsg.Home, DoHome, ServoState.Homing);
            Transition(ServoState.NotHomed, ServoMsg.ServoOff, DoServoOff, ServoState.NotHomed);
            Transition(ServoState.NotHomed, ServoMsg.TrigServoOff, null, ServoState.ServoOff);
            Transition(ServoState.NotHomed, ServoMsg.TrigHomed, null, ServoState.Homed);
            Transition(ServoState.NotHomed, ServoMsg.TrigError, null, ServoState.Error);

            Transition(ServoState.Homing, FSM_MSG.TIMER, MonitorHoming, ServoState.Homing);
            Transition(ServoState.Homing, ServoMsg.ServoOff, DoServoOff, ServoState.Homing);
            Transition(ServoState.Homing, ServoMsg.Error, null, ServoState.Error);
            Transition(ServoState.Homing, ServoMsg.TrigServoOff, null, ServoState.ServoOff);
            Transition(ServoState.Homing, ServoMsg.TrigHomed, null, ServoState.Homed);
            Transition(ServoState.Homing, ServoMsg.TrigError, null, ServoState.Error);
 
            Transition(ServoState.Homed, FSM_MSG.TIMER, MonitorHomed, ServoState.Homed);
            Transition(ServoState.Homed, ServoMsg.ServoOff, DoServoOff, ServoState.Homed);
            Transition(ServoState.Homed, ServoMsg.MoveTo, DoMoveTo, ServoState.Moving);
            Transition(ServoState.Homed, ServoMsg.MoveOffset, DoMoveOffset, ServoState.Moving);
            Transition(ServoState.Homed, ServoMsg.TrigServoOff, null, ServoState.ServoOff);
            Transition(ServoState.Homed, ServoMsg.TrigNotHomed, null, ServoState.NotHomed);
            Transition(ServoState.Homed, ServoMsg.TrigError, null, ServoState.Error);
            Transition(ServoState.Homed, ServoMsg.Home, DoHome, ServoState.Homing);

            Transition(ServoState.Moving, ServoMsg.Home, DoHome, ServoState.Homing);
            Transition(ServoState.Moving, FSM_MSG.TIMER, MonitorMoving, ServoState.Moving);
            Transition(ServoState.Moving, ServoMsg.Stop, DoStop, ServoState.Homed);
            Transition(ServoState.Moving, ServoMsg.ServoOff, DoServoOff, ServoState.Moving);
            Transition(ServoState.Moving, ServoMsg.Error, null, ServoState.Error);
            Transition(ServoState.Moving, ServoMsg.TrigServoOff, null, ServoState.ServoOff);
            Transition(ServoState.Moving, ServoMsg.TrigNotHomed, null, ServoState.Error);
            Transition(ServoState.Moving, ServoMsg.TrigError, null, ServoState.Error);
            Transition(ServoState.Moving, ServoMsg.TrigAtPosition, null, ServoState.Homed);
             
            Transition(ServoState.Reseting, FSM_MSG.TIMER, MonitorResetting, ServoState.Reseting);
            Transition(ServoState.Reseting, ServoMsg.ServoOff, DoServoOff, ServoState.Reseting);
            Transition(ServoState.Reseting, ServoMsg.Error, null, ServoState.Error);
            Transition(ServoState.Reseting, ServoMsg.TrigServoOff, null, ServoState.ServoOff);
            Transition(ServoState.Reseting, ServoMsg.TrigRecovered, null, ServoState.NotHomed);
            Transition(ServoState.Reseting, ServoMsg.TrigError, null, ServoState.Error);

            Transition(ServoState.Error, FSM_MSG.TIMER, MonitorError, ServoState.Error);
            Transition(ServoState.Error, ServoMsg.Reset, DoReset, ServoState.Reseting);
            Transition(ServoState.Error, ServoMsg.ServoOff, DoServoOff, ServoState.Error);
            Transition(ServoState.Error, ServoMsg.TrigServoOff, null, ServoState.ServoOff);

            base.Module = module;
            base.Name = node.GetAttribute("id");
            base.Display = node.GetAttribute("display");
            base.DeviceID = node.GetAttribute("schematicId");

            _diOriginSignal = ParseDiNode("diOriginSignal", node);
            _diCWLimitSignal = ParseDiNode("diCWLimitSignal", node);
            _diCCWLimitSignal = ParseDiNode("diCCWLimitSignal", node);
            _diLocationComplete = ParseDiNode("diLocationComplete", node);
            _diAlarm = ParseDiNode("diAlarm", node);
            _diPulseOutputOn = ParseDiNode("diPulseOutputOn", node);
            _diPulseOutputComplete = ParseDiNode("diPulseOutputComplete", node);
            _diWithoutOrigin = ParseDiNode("diWithoutOrigin", node);
            _diServoOnFeedback = ParseDiNode("diServoOnFeedback", node);

            _doBreakRelay = ParseDoNode("doBreakRelay", node);
            _doServoOn = ParseDoNode("doServoOn", node);
            _doMoveUp = ParseDoNode("doMoveUp", node);
            _doMoveDown = ParseDoNode("doMoveDown", node);
            _doMoveToPosition = ParseDoNode("doMoveToPosition", node);
            _doDeviationCounterReset = ParseDoNode("doDeviationCounterReset", node);
            _doAlarmReset = ParseDoNode("doAlarmReset", node);
            _doStopMoveUp = ParseDoNode("doStopMoveUp", node);
            _doStopMoveDown = ParseDoNode("doStopMoveDown", node);
            _doLookingForOrigin = ParseDoNode("doLookingForOrigin", node);

            _aoServoSpeedSetPoint = ParseAoNode("aoServoSpeedSetPoint", node);
            _aoManualSpeedSetPoint = ParseAoNode("aoManualSpeedSetPoint", node);
            _aoAcceleration = ParseAoNode("aoAcceleration", node);
            _aoDeceleration = ParseAoNode("aoDeceleration", node);
            _aoStartFrequency = ParseAoNode("aoStartFrequency", node);
            _aoPositionSetPoint = ParseAoNode("aoPositionSetPoint", node);

            _aiPulseFeedback = ParseAiNode("aiPulseFeedback", node);
            _aiServoSpeedFeedback = ParseAiNode("aiServoSpeedFeedback", node);
            _aiManualSpeedFeedback = ParseAiNode("aiManualSpeedFeedback", node);
            _aiPositionFeedback = ParseAiNode("aiPositionFeedback", node);
            _aiAccelerationFeedback = ParseAiNode("aiAccelerationFeedback", node);
            _aiDecelerationFeedback = ParseAiNode("aiDecelerationFeedback", node);
            _aiStartFrequencyFeedback = ParseAiNode("aiStartFrequencyFeedback", node);

            _scAcceleration = ParseScNodeDouble("scAcceleration", node);
            _scDeceleration = ParseScNodeDouble("scDeceleration", node);
            _scStartFrequency = ParseScNodeDouble("scStartFrequency", node);
            _scDefaultServoSpeed = ParseScNodeDouble("scDefaultServoSpeed", node);
            _scDefaultManualSpeed = ParseScNodeDouble("scDefaultManualSpeed", node);
            _scServoOnTimeout = ParseScNodeDouble("scServoOnTimeout", node);
            _scMoveTimeout = ParseScNodeDouble("scMoveTimeout", node);
            _scStopTimeout = ParseScNodeDouble("scStopTimeout", node);
            _scResetAlarmTimeout = ParseScNodeDouble("scResetAlarmTimeout", node);
            _scHomeTimeout = ParseScNodeDouble("scHomeTimeout", node);
        }

        public bool ServoOn( )
        {
            PostMsg(ServoMsg.ServoOn);

            return true;
        }

        public bool IsServoOnState( )
        {
            return _fsm.State != (int)ServoState.ServoOff;
        }

        public bool IsHomedState( )
        {
            return _fsm.State == (int)ServoState.Homed;
        }

        public bool InvokeStop()
        {
 
            PostMsg(ServoMsg.Stop);

            return true;
        }

        public bool Home(out string reason )
        {
            if( (_fsm.State == (int)ServoState.ServoOff) || (_fsm.State == (int)ServoState.Error) )
            {
                reason = string.Format("Can not home {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                return false;
            }
            reason = string.Format("{0} Home", Display);

            PostMsg(ServoMsg.Home);

            return true;
        }

        public bool MoveTo(double position, out string reason)
        {
            if (_fsm.State != (int)ServoState.Homed)
            {
                reason = string.Format("Can not move {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                return false;
            }

            double speed = _scDefaultServoSpeed.Value;

            reason = string.Format("{0} Move to {1} at speed {2}", Display, position, speed);

            PostMsg(ServoMsg.MoveTo, speed, position);

            return true;
        }

        private bool DoServoOn(object[] objs)
        {
            string reason = string.Empty;
            if (!_doServoOn.SetValue(true, out reason))
            {
                return false;
            }

            _timerServoOn.Start(_scServoOnTimeout.Value * 1000);

            return true;
        }

        private bool MonitorServoOff(object[] objs)
        {
            if (_doServoOn.Value && _timerServoOn.IsTimeout())
            {
                _doServoOn.Value = false;
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not turn {0} servo on in {1} seconds", Display, _scServoOnTimeout.Value));
            }

            if (IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOn);
                return true;
            }

            return true;
        }

        private bool DoHome(object[] objs)
        {
            string reason = string.Empty;
            if (!_doLookingForOrigin.SetValue(true, out reason))
            {
                return false;
            }

            _timerHome.Start(_scHomeTimeout.Value * 1000);

            return true;
        }
        private bool DoServoOff(object[] objs)
        {
            string reason = string.Empty;
            if (!_doServoOn.SetValue(false, out reason))
            {
                LOG.Write(reason);
                return false;
            }

            if (!_doMoveToPosition.SetValue(false, out reason))
            {
                return false;
            }

            _timerServoOff.Start(_scServoOnTimeout.Value * 1000);

            return true;
        }

        private bool MonitorNotHomed(object[] objs)
        {
            if (!_doServoOn.Value && _timerServoOff.IsTimeout())
            {
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not turn {0} servo off in {1} seconds", Display, _scServoOnTimeout.Value));
                _doServoOn.Value = true;
            }

            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }

            if (!IsWithoutOrigin)
            {
                PostMsg(ServoMsg.TrigHomed);
                return true;
            }

            if (IsAlarm)
            {
                PostMsg(ServoMsg.TrigError);
                return true;
            }

            return true;
        }
        private bool MonitorHoming(object[] objs)
        {
            if (!_doServoOn.Value && _timerServoOff.IsTimeout())
            {
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not turn {0} servo off in {1} seconds", Display, _scServoOnTimeout.Value));
                _doServoOn.Value = true;
            }
            if (_doLookingForOrigin.Value && _timerHome.IsTimeout())
            {
                _doLookingForOrigin.Value = false;
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not home {0} in {1} seconds", Display, _scHomeTimeout.Value));
            }

            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }

            if (!IsWithoutOrigin)
            {
                if (_timerHome.GetElapseTime() < 1000)
                {
                    return true; // Delay 1秒钟。 
                }
                PostMsg(ServoMsg.TrigHomed);
                _doLookingForOrigin.Value = false;
                return true;
            }

            if (IsAlarm)
            {
                PostMsg(ServoMsg.TrigError);
                return true;
            }
            return true;
        }
        private bool MonitorHomed(object[] objs)
        {
            if (!_doServoOn.Value && _timerServoOff.IsTimeout())
            {
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not turn {0} servo off in {1} seconds", Display, _scServoOnTimeout.Value));
                _doServoOn.Value = true;
            }

            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }

            if (IsWithoutOrigin)
            {
                PostMsg(ServoMsg.TrigNotHomed);
                return true;
            }

            if (IsAlarm)
            {
                PostMsg(ServoMsg.TrigError);
                return true;
            }
            return true;
        }

        private bool MonitorMoving(object[] objs)
        {
            bool isWroteToPlc = ((Math.Abs(ServoSpeedSetPoint - ServoSpeedFeedback) < 0.0001)
                                 && (Math.Abs(PositionSetPoint - PositionFeedback) < 0.0001));

            bool isAtPosition = Math.Abs(PositionSetPoint - PulseFeedback) < 0.0001;

            string reason = string.Empty;

            if (isAtPosition && !_diPulseOutputOn.Value)
            {
                _doMoveToPosition.SetValue(false, out reason);
                PostMsg(ServoMsg.TrigAtPosition);

                LOG.Write(string.Format("{0} Moved to position {1}", Display, PositionSetPoint ));

                return true;
            }

            if (isWroteToPlc && !_doMoveToPosition.Value)
            {
                _doMoveToPosition.SetValue(true, out reason);
                _timerMove.Start(_scMoveTimeout.Value * 1000);

                LOG.Write(string.Format("{0} Send moving command ", Display ));

            }

            if (_doMoveToPosition.Value && _timerMove.IsTimeout())
            {
                _doMoveToPosition.SetValue(false, out reason);
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not move {0} to position in {1} seconds", Display, _scServoOnTimeout.Value));
                PostMsg(ServoMsg.Error);
                return true;
            }

            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }

            if (IsWithoutOrigin)
            {
                PostMsg(ServoMsg.TrigNotHomed);
                return true;
            }

            if (IsAlarm)
            {
                PostMsg(ServoMsg.TrigError);
                return true;
            }

            return true;
        }

        private bool DoMoveTo(object[] objs)
        {
            ServoSpeedSetPoint = (float)(double)objs[0];
            PositionSetPoint = (float)(double)objs[1];

            //string reason;
            //if (!_doMoveToPosition.SetValue(true, out reason))
            //{
            //    LOG.Write(reason);
            //    return false;
            //}

            LOG.Write(string.Format("{0} Moving to {1}", Display, PositionSetPoint));

            return true;
        }

        private bool DoMoveOffset(object[] objs)
        {
            double speed = (double)objs[0];
            double offset = (double)objs[1];

            double position = PulseFeedback + offset;
 
            PositionSetPoint = (float)position;
            ServoSpeedSetPoint = (float)speed;

            return true;
        }

        private bool DoStop(object[] objs)
        {
            string reason = string.Empty;
            _doMoveToPosition.SetValue(false, out reason);

            return true;
        }

        private bool MonitorError(object[] objs)
        {
            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }
            return true;
        }
        private bool MonitorResetting(object[] objs)
        {
            string reason;
            if (_doAlarmReset.Value && _timerResetAlarm.IsTimeout())
            {
                _doAlarmReset.SetValue(false, out reason);
                EV.PostMessage("System", EventEnum.DefaultWarning, string.Format("Can not recover {0} in {1} seconds", Display, _scResetAlarmTimeout.Value));
                PostMsg(ServoMsg.Error);
                return true;
            }

            if (!IsServoOn)
            {
                PostMsg(ServoMsg.TrigServoOff);
                return true;
            }
 
            if (IsAlarm)
            {
                PostMsg(ServoMsg.TrigError);
                return true;
            }
            else
            {
                PostMsg(ServoMsg.TrigRecovered);
            }

            return true;
        }

 

        private bool DoReset(object[] objs)
        {
            string reason;
            _doAlarmReset.SetValue(true, out reason);
            _timerResetAlarm.Start(_scResetAlarmTimeout.Value * 1000);

            return true;
        }

        public bool Initialize()
        {
            Acceleration = (float)_scAcceleration.Value;
            Deceleration = (float)_scDeceleration.Value;
            StartFrequency = (float)_scStartFrequency.Value;
            ManualSpeedSetPoint = (float)_scDefaultManualSpeed.Value;
            ServoSpeedSetPoint = (float)_scDefaultServoSpeed.Value;

            DATA.Subscribe(string.Format("Device.{0}.{1}", Module, Name), () =>
            {
                AITServoMotorData data = new AITServoMotorData()
                {
                    DeviceName = Name,
                    DeviceSchematicId = DeviceID,
                    DisplayName = Display,

                    IsServoOn = IsServoOn,
                    IsAlarm = IsAlarm,
                    IsWithoutOrigin = IsWithoutOrigin,
                    IsPulseOn = IsPulseOn,

                    PositionFeedback = PositionFeedback,
                    PositionSetPoint = PositionSetPoint,
                    PulseFeedback = PulseFeedback,
                    ManualSpeedSetPoint = ManualSpeedSetPoint,
                    ManualSpeedFeedback = ManualSpeedFeedback,
                    State = State,
                    ServoSpeedFeedback = ServoSpeedFeedback,
                    ServoSpeedSetPoint = ServoSpeedSetPoint,
                };
                return data;
            }, SubscriptionAttribute.FLAG.IgnoreSaveDB);


            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.SetServoOn),
                (out string reason, int time, object[] param) =>
                {
                    reason = string.Format("{0} Servo on", Display);

                    PostMsg(ServoMsg.ServoOn);

                    return true;
                });

            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.Home),
                (out string reason, int time, object[] param) =>
                {
                    if( (_fsm.State == (int)ServoState.ServoOff) || (_fsm.State == (int)ServoState.Error) )
                    {
                        reason = string.Format("Can not home {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                        return false;
                    }
                    reason = string.Format("{0} Home", Display);

                    PostMsg(ServoMsg.Home);

                    return true;
                });

            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.SetServoOff),
                (out string reason, int time, object[] param) =>
                {
                    reason = string.Format("{0} Servo off", Display);

                    PostMsg(ServoMsg.ServoOff);

                    return true;
                });

            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.MoveTo),
                (out string reason, int time, object[] param) =>
                {
 
                    if (_fsm.State != (int)ServoState.Homed)
                    {
                        reason = string.Format("Can not move {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                        return false;
                    }

                    double speed = Convert.ToDouble(param[0]);
                    double position = Convert.ToDouble(param[1]);

                    reason = string.Format("{0} Move to {1} at speed {2}", Display, position, speed);

                    PostMsg(ServoMsg.MoveTo, speed, position);

                    return true;
                });
            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.MoveOffset),
                (out string reason, int time, object[] param) =>
                {
                    if (_fsm.State != (int)ServoState.Homed)
                    {
                        reason = string.Format("Can not move {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                        return false;
                    }

                    double speed = Convert.ToDouble(param[0]);
                    double offset = Convert.ToDouble(param[1]);

                    reason = string.Format("{0} Move offset {1} at speed {2}", Display, offset, speed);
                    
                    PostMsg(ServoMsg.MoveOffset, speed, offset);

                    return true;
                });
            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.Reset),
                (out string reason, int time, object[] param) =>
                {
                    if (_fsm.State != (int)ServoState.Error)
                    {
                        reason = string.Format("Can not reset {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                        return false;
                    }

                    reason = string.Format("{0} Reset", Display);
                    PostMsg(ServoMsg.Reset);

                    return true;
                });
            DEVICE.Register(string.Format("{0}.{1}", Name, AITServoMotorOperation.Stop),
                (out string reason, int time, object[] param) =>
                {
                    if (_fsm.State != (int)ServoState.Moving)
                    {
                        reason = string.Format("Can not stop {0} while in {1} status.", Display, ((ServoState)_fsm.State));
                        return false;
                    }
                    reason = string.Format("{0} Stop move", Display);

                    PostMsg(ServoMsg.Stop);

                    return true;
                });

            _fsm.Start();
            _thread = new Thread(new ThreadStart(_fsm.Loop));
            _thread.Name = _fsm.Name;
            _thread.Start();

            while (!_thread.IsAlive)
                Thread.Sleep(1);
            return true;
        }

        public void Terminate()
        {
            _fsm.Stop();
            if (_thread.IsAlive)
            {
                Thread.Sleep(100);
                if (_thread.IsAlive)
                {
                    try
                    {
                        _thread.Abort();
                    }
                    catch (Exception ex)
                    {
                        LOG.Error(String.Format("IoServoMotor terminate has exception."), ex);
                    }
                }
            }
        }

 

        public void Monitor()
        {
            _trigCCWLimit.CLK = _diCCWLimitSignal.Value;
            if (_trigCCWLimit.Q)
            {
                EV.PostMessage(Module, EventEnum.DefaultWarning, string.Format("{0} in CCW Limit", Display));
            }

            _trigCWLimit.CLK = _diCWLimitSignal.Value;
            if (_trigCWLimit.Q)
            {
                EV.PostMessage(Module, EventEnum.DefaultWarning, string.Format("{0} in CW Limit", Display));
            }
            
            Acceleration = (float)_scAcceleration.Value;
            Deceleration = (float)_scDeceleration.Value;
            StartFrequency = (float)_scStartFrequency.Value;
            ManualSpeedSetPoint = (float)_scDefaultManualSpeed.Value;
            ServoSpeedSetPoint = (float)_scDefaultServoSpeed.Value;

            _trigPulseOutput.CLK = _diPulseOutputOn.Value;

            if (_trigPulseOutput.R)
            {
                LOG.Write(string.Format("{0} Moving", Display ));

            }

            if (_trigPulseOutput.T)
            {
                LOG.Write(string.Format("{0} Stop Moving", Display ));

            }



        }

        public void Reset()
        {
            PostMsg(ServoMsg.Reset);
        }

        public bool IsStoppedAtPosition(double position)
        {
 
                return (Math.Abs(position - PulseFeedback) < 0.01) 
                       && (Math.Abs(position - PositionSetPoint) < 0.01) 
                       && !_diPulseOutputOn.Value && IsHomedState( );
 
        }
    }
}