using ACS.SPiiPlusNET;
using Aurora;
using Aurora.Api;
using Aurora.Api.Devices;
using AuroraMotor;
using Hermes;
using PropertyTools.DataAnnotations;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using Axis = ACS.SPiiPlusNET.Axis;

namespace AuroraACS
{
    public enum JogDirection
    {
        plus = 1,
        minus = -1,
    }
    public enum Speed
    {
        L,
        M,
        H
    }

    public class ACSMotor : AuroraMotorBase    // BaseMotor
    {
        private readonly Logger Log = new Logger("ACSMotor");

        public bool IsInit = false;

        public int BufferNo;
        public Axis AxisNo;
        private readonly string AxisName;
        private double TargetPosition;
        private double TargetPositionABS;
        private TargetPosType TargetType;
        ACSDevice ACSLibrary;
        private ACSMotorConfig _Config;
        bool IsHomeComplete = false;

        public ACSMotor(string deviceId, ACSDevice acs, int bufferNo, int axisNo, string axisName = null) : base(deviceId)
        {
            ACSLibrary = acs;
            BufferNo = bufferNo;
            AxisNo = (Axis)axisNo;
            AxisName = axisName;
        }

        protected override int DoActivate(IDeviceConfig config)
        {
            if (IsActivated) return E1.OK;

            _Config = config as ACSMotorConfig;
            if (!ACSLibrary.IsActivated) {
                Log.E("ACS Not activated.");
                return E1.NETWORK;
            }
            if (ACSLibrary.IsSimul) {
                IsSimul = ACSLibrary.IsSimul;
                AxisNo += 4;
                ServoOn(true);
            }
            return E1.OK;
        }

        protected override int DoDeactivate()
        {
            if (!IsActivated) return E1.OK;

            if (!ACSLibrary.IsActivated) {
                return E1.OK;
            }

            IsInit = false;
            IsHomeComplete = false;
            return E1.OK;
        }

        public override int ClearAlarm()
        {
            ACSLibrary.ClearAlam(AxisNo);
            return E1.OK;
        }

        public override bool IsAlarm()
        {
            var state = ACSLibrary.GetMotorAlarmCode(AxisNo);
            var fault = ACSLibrary.GetFault(AxisNo);
            if (state == 0 && fault == 0) {
                return false;
            }

            return true;
        }

        public override IList<string> GetAlarm()
        {
            List<string> alarms = new List<string>();
            if (!IsActivated) {
                return alarms;
            }
            var mask = ACSLibrary.GetMotorAlarmCode(AxisNo);
            if (mask == 0) {
                return alarms;
            }

            alarms.Add(ACSLibrary.GetMotorAlarmDesc(mask));

            return alarms;
        }

        public override double GetActualPos()
        {
            return ACSLibrary.ReadMotorPosition(AxisNo);
        }

        public double GetReferencePosition()
        {
            return ACSLibrary.ReadRPosition(AxisNo);
        }

        public override bool IsTargetPosLimit()
        {
            if (_Config == null) return true;
            if (!IsActivated) return true;

            return (TargetPositionABS < _Config.MoveRange.Min) || (TargetPositionABS > _Config.MoveRange.Max);
        }

        public override Range2<double> GetPosLimit()
        {
            if (_Config == null) return null;
            if (!IsActivated) return null;

            return new Range2<double>(_Config.MoveRange.Min, _Config.MoveRange.Max);
        }

        public override double GetTargetPos()
        {
            return TargetPositionABS;
        }

        public override double GetActualVel()
        {
            var vel = ACSLibrary.GetVelocity(AxisNo);
            if (vel < 0) {
                return 0;
            }

            return (uint)vel;
        }

        public double GetFeedbackVelocity()
        {
            return ACSLibrary.GetFeedbackVelocity(AxisNo);
        }

        public override Range<double> GetVelLimit()
        {
            return new Range<double>(_Config.VelocityRange.Min, _Config.VelocityRange.Max);
        }

        public override int Home()
        {
            if (ACSLibrary.IsSimul) {
                ACSLibrary.SetZeroPosition(AxisNo);
                IsHomeComplete = true;
                return E1.OK;
            }

            if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
                Log.E("Motor initiallize fail : " + DeviceId);
                return E1.GENERIC;
            }
            return E1.OK;
        }
        public int HomeStop()
        {
            ACSLibrary.StopBuffer((ProgramBuffer)BufferNo);
            Log.E("Motor initiallize Stop : " + DeviceId);

            return E1.OK;
        }

        public int Init()
        {
            if (!IsServoOn()) {
                if (ServoOn(true) != E1.OK) {
                    Log.E("ServoOn Error : " + DeviceId);
                    return E1.GENERIC;
                }
            }

            if (ACSLibrary.IsSimul) {
                if (Home() != E1.OK) {
                    return E1.GENERIC;
                }
            }

            IsInit = true;
            return E1.OK;

            //if (!IsInit) {
            //    if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
            //        Log.E("Motor initiallize fail : " + DeviceId);
            //        return E1.GENERIC;
            //    }
            //    IsHomeComplete = true;
            //}

            //return E1.OK;
        }

        public override bool IsHomeCompleted()
        {
            if (ACSLibrary.IsSimul) {
                return true;
            }

            return true;

            //string strInit = "initComp_" + AxisName;
            //return ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
        }

        public bool IsInitiallized()
        {
            if (ACSLibrary.IsSimul) {
                return IsInit;
            }

            string strInit = "initComp_" + AxisName;
            IsInit = ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
            return IsInit;
        }

        public override bool IsMoving()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_MOVE) != 0) {
                return true;
            }

            return false;
        }

        public override bool IsServoOn()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_ENABLE) == 0) {
                return false;
            }

            return true;
        }

        public double GetJogSpeed(Speed speed)
        {
            switch (speed) {
                case Speed.L: return _Config.LowJogSpeed;
                case Speed.M: return _Config.MidJogSpeed;
                case Speed.H: return _Config.HighJogSpeed;
                default: return _Config.LowJogSpeed;
            }
        }
        public int MoveToSWLimit(JogDirection dir, Speed speed)
        {
            double limit = double.NaN;
            if(IsSimul)
                limit = dir == JogDirection.plus ? _Config.MoveRange.Max : _Config.MoveRange.Min;
            else {
                limit = ACSLibrary.GetMotorSoftLimit(AxisNo, dir == JogDirection.plus);
                if(limit == double.NaN)
                    return E2.UNKNOWN;
            }

            SetTargetPos(limit, TargetPosType.ABS);
            SetVelocityImm(GetJogSpeed(speed));
            SetAccelerationImm(GetAcceleration(speed));
            SetDecelerationImm(GetAcceleration(speed));
            SetJerkImm(GetJerk(speed));
            return Move();
        }

        public override int Jog(int direction, double velocity)
        {
            return ACSLibrary.Jog(AxisNo, velocity * direction);
        }

        public int Jog(JogDirection dir, Speed speed)
        {
            return Jog((int)dir, GetJogSpeed(speed));
        }

        public override int Jog(int direction)
        {
            var jogSpeed = (_Config as ACSMotorConfig).MidJogSpeed;
            if (SelectedJogSpeed == eJogSpeed.L) {
                jogSpeed = (_Config as ACSMotorConfig).LowJogSpeed;
            } else if (SelectedJogSpeed == eJogSpeed.H) {
                jogSpeed = (_Config as ACSMotorConfig).HighJogSpeed;
            }

            //return ACSLibrary.Jog(AxisNo, (GlobalDirection)direction);
            return ACSLibrary.Jog(AxisNo, jogSpeed * direction);
        }

        public override int JogIncrement(double increment, double velocity)
        {
            SetTargetSPD(velocity);
            SetTargetPos(GetActualPos() + increment);
            Move();
            return -1;
        }

        public override int Move()
        {
            if (TargetPositionABS < _Config.MoveRange.Min) {
                Log.E($"Limit Over Error : Requested[{TargetPositionABS}] < - Limit[{_Config.MoveRange.Min}]");
                return -1;
            } else if (TargetPositionABS > _Config.MoveRange.Max) {
                Log.E($"Limit Over Error : Requested[{TargetPositionABS}] > - Limit[{_Config.MoveRange.Max}]");
                return -1;
            }

            if (TargetType == TargetPosType.ABS) {
                return ACSLibrary.MoveABS(AxisNo, TargetPosition);
            } else {
                return ACSLibrary.MoveINC(AxisNo, TargetPosition);
            }
        }

        public int DoMove(int velocity)
        {
            SetTargetSPD(velocity);
            return Move();
        }

        public override int ServoOn(bool onOff)
        {
            ACSLibrary.SetMotorEnable(AxisNo, onOff);
            return E1.OK;
        }

        public override int SetTargetPos(double targetPos, TargetPosType type = TargetPosType.ABS)
        {
            TargetType = type;
            TargetPosition = targetPos;
            if (TargetType == TargetPosType.ABS) {
                TargetPositionABS = TargetPosition;
            } else {
                TargetPositionABS = GetActualPos() + TargetPosition;
            }

            return E1.OK;
        }

        public override int SetTargetSPD(double tarVel)
        {
            if (ACSLibrary.SetVelocity(AxisNo, tarVel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public override double GetTargetSPD()
        {
            return ACSLibrary.GetVelocity(AxisNo);
        }

        private eJogSpeed SelectedJogSpeed = eJogSpeed.M;
        public override int SetJogSPD(eJogSpeed jogSPD)
        {
            SelectedJogSpeed = jogSPD;
            return 0;
        }

        public int SetVelocityImm(double velocity)
        {
            if (ACSLibrary.SetVelocityImm(AxisNo, velocity)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public int SetMoveVelocityImm(Speed speed)
        {
            double velocity = 100;
            switch (speed)
            {
                case Speed.L:
                    velocity = _Config.LowMoveSpeed;
                    break;
                case Speed.M:
                    velocity = _Config.MidMoveSpeed;
                    break;
                case Speed.H:
                    velocity = _Config.HighMoveSpeed;
                    break;
                default:
                    break;
            }
            return SetVelocityImm(velocity);
        }

        public int SetAccelerationImm(double accel)
        {
            if (ACSLibrary.SetAccelerationImm(AxisNo, accel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }
        public int AccelerationImm(Speed speed)
        {
            return SetAccelerationImm(GetAcceleration(speed));
        }

        public int SetAcceleration(double accel)
        {
            if (ACSLibrary.setAcceleration(AxisNo, accel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public double GetAcceleration()
        {
            return ACSLibrary.GetAcceleration(AxisNo);
        }

        public int SetDeceleration(double decel)
        {
            if (ACSLibrary.SetDeceleration(AxisNo, decel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public double GetDeceleration()
        {
            return ACSLibrary.GetDeceleration(AxisNo);
        }

        public int SetDecelerationImm(double decel)
        {
            if (ACSLibrary.SetDecelerationImm(AxisNo, decel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }
        public double GetAcceleration(Speed speed)
        {
            switch (speed) {
                case Speed.L: return _Config.LowMoveAcc;
                case Speed.M: return _Config.MidMoveAcc;
                case Speed.H: return _Config.HighMoveAcc;
                default: return _Config.LowMoveAcc;
            }
        }

        public int DecelerationImm(Speed speed)
        {
            return SetDecelerationImm(GetAcceleration(speed));
        }

        public double GetKillDeceleration()
        {
            return ACSLibrary.GetKillDeceleration(AxisNo);
        }
        public double GetJerk(Speed speed)
        {
            switch (speed) {
                case Speed.L: return _Config.LowMoveAcc * 10;
                case Speed.M: return _Config.MidMoveAcc * 10;
                case Speed.H: return _Config.HighMoveAcc * 10;
                default: return _Config.LowMoveAcc * 10;
            }
        }
        public bool SetJerkImm(double dVel)
        {
            if (!IsActivated)
                return false;

            return ACSLibrary.SetJerkImm(AxisNo, dVel);
        }
        public bool JerkImm(Speed speed)
        {
            return SetJerkImm(GetJerk(speed));
        }
        public double GetJerk()
        {
            return ACSLibrary.GetJerk(AxisNo);
        }

        public override int Stop()
        {
            ACSLibrary.SetHalt(AxisNo);
            return E1.OK;
        }

        public override int StopJog()
        {
            ACSLibrary.SetHalt(AxisNo);
            return E1.OK;
        }

        public int DoStopAll()
        {
            ACSLibrary.SetHaltAll();
            return E1.OK;
        }

        public int EmergencyStop()
        {
            ACSLibrary.SetKill(AxisNo);
            return E1.OK;
        }

        public virtual bool ReadInposition()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_INPOS) != 0) {
                return true;
            }

            return false;
        }

        public bool IsAccelerating()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_ACC) != 0) {
                return true;
            }

            return false;
        }

        public bool IsHPLimit()
        {
            return ACSLibrary.IsHPLimit(AxisNo);
        }


        public bool IsHNLimit()
        {
            return ACSLibrary.IsHNLimit(AxisNo);
        }

        public bool IsSPLimit()
        {
            return ACSLibrary.IsSPLimit(AxisNo);
        }

        public bool IsSNLimit()
        {
            return ACSLibrary.IsSNLimit(AxisNo);
        }


        public void RunBuffer(string label, Action<int> programEnd)
        {
            ACSLibrary.RunBuffer((ProgramBuffer)BufferNo, label, programEnd);
        }

        public override bool IsInpos()
        {
            return ACSLibrary.ReadInposition(AxisNo);
        }
        public double GetCurrentOverload()
        {
            return ACSLibrary.GetAxisCurrentOverload(AxisNo);
        }

        public bool IsReachedTargetPosition(double targetPosition)
        {
            if (ApplicationExt.IsSimulation)
                return true;

            if (!IsActivated)
                return false;

            if (Math.Abs(GetActualPos() - targetPosition) < 0.1 && IsInpos())
                return true;

            return false;
        }
    }
}