using System;
using ln.logging;

namespace ln.ethercat.controller.drives
{
    public enum CIA402States {
        UNDEFINED,
        NOT_READY_TO_SWITCH_ON,
        SWITCH_ON_DISABLED,
        READY_TO_SWITCH_ON,
        SWITCHED_ON,
        OPERATION_ENABLED,
        QUICK_STOP_ACTIVE,
        FAULT_REACTION_ACTIVE,
        FAULT
    }

    public enum CIA402ModesOfOperation : byte
    {
        NO_MODE_CHANGE = 0,
        VL_VELOCITY_MODE = 2,
        HOMING_MODE = 6,
        INTERPOLATED_POSITION_MODE = 7,
        CYCLIC_SYNC_POSITION = 8,
        CYCLIC_SYNC_VELOCITY = 9,
        CYCLIC_SYNC_TORQUE = 10
    }

    public class CIA402Controller : DriveController
    {
        public readonly SDOAddr saControlWord;
        public readonly SDOAddr saStatusWord;
        public readonly SDOAddr saErrorCode;
        public readonly SDOAddr saTargetPosition;
        public readonly SDOAddr saTargetSpeed;
        public readonly SDOAddr saTargetTorque;
        public readonly SDOAddr saActualPosition;
        public readonly SDOAddr saActualSpeed;
        public readonly SDOAddr saActualTorque;
        public readonly SDOAddr saModesOfOperation;
        public readonly SDOAddr saModesOfOperationDisplay;
        
        public CIA402Controller(ECMaster ecMaster,int slave)
        :base(ecMaster, slave)
        {
            saErrorCode = new SDOAddr(slave, 0x603f);
            saControlWord = new SDOAddr(slave, 0x6040);
            saStatusWord = new SDOAddr(slave, 0x6041);
            
            saTargetPosition = new SDOAddr(slave, 0x607A);
            saTargetSpeed = new SDOAddr(slave, 0x60FF);
            saTargetTorque = new SDOAddr(slave, 0x6071);
            
            saActualPosition = new SDOAddr(slave, 0x6064);
            saActualSpeed = new SDOAddr(slave, 0x606C);
            saActualTorque = new SDOAddr(slave, 0x6077);

            saModesOfOperation = new SDOAddr(slave, 0x6060);
            saModesOfOperationDisplay = new SDOAddr(slave, 0x6061);
        }

        public override void UpdateStates() { }
        public override void UpdateDrive() { }

        public override DriveStates DriveState 
        {
            get {
                switch (GetCIA402State())
                {
                    case CIA402States.NOT_READY_TO_SWITCH_ON:
                    case CIA402States.SWITCH_ON_DISABLED:
                    case CIA402States.READY_TO_SWITCH_ON:
                        return DriveStates.INIT;
                    case CIA402States.SWITCHED_ON:
                    case CIA402States.QUICK_STOP_ACTIVE:
                        return DriveStates.POWERED;
                    case CIA402States.OPERATION_ENABLED:
                        return DriveStates.OPERATIONAL;
                    case CIA402States.FAULT_REACTION_ACTIVE:
                    case CIA402States.FAULT:
                        return DriveStates.ERROR;
                    default:
                        return DriveStates.UNDEFINED;
                }
            }
        }

        public override string OEMDriveState {
            get => GetCIA402State().ToString();
        }

        public override DriveMode DriveMode {
            get {
                switch (ModeOfOperation)
                {
                    case CIA402ModesOfOperation.CYCLIC_SYNC_POSITION:
                        return DriveMode.POSITION;
                    case CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY:
                        return DriveMode.SPEED;
                    case CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE:
                        return DriveMode.TORQUE;
                    default:
                        return DriveMode.UNDEFINED;
                }
            }
            set {
                switch (value)
                {
                    case DriveMode.POSITION:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION;
                        break;
                    case DriveMode.SPEED:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION;
                        break;
                    case DriveMode.TORQUE:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION;
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: DriveMode {0} not supported", value);
                        break;
                }
            }
        }

        public CIA402ModesOfOperation ModeOfOperation {
            get => (CIA402ModesOfOperation)GetSDOValue<byte>(saModesOfOperationDisplay);
            set => SetSDOValue<byte>(saModesOfOperation, (byte)value);
        }


        public override decimal ActualPosition => GetSDOValue<int>(saActualPosition);
        public override decimal ActualSpeed => GetSDOValue<int>(saActualSpeed);
        public override decimal ActualTorque => GetSDOValue<int>(saActualTorque);

        public override decimal TargetPosition {
            get => GetSDOValue<int>(saTargetPosition);
            set => SetSDOValue(saTargetPosition, value);
        }

        public override decimal TargetSpeed {
            get => GetSDOValue<int>(saTargetSpeed);
            set => SetSDOValue(saTargetSpeed, value);
        }
        public override decimal TargetTorque {
            get => GetSDOValue<int>(saTargetTorque);
            set => SetSDOValue(saTargetTorque, value);
        }

        public override int ErrorCode => GetSDOValue<int>(saErrorCode);

        public override string ErrorText => ErrorCode.ToString();

        public override void EnableDrive(bool enable)
        {
            if (enable)
            {
                switch (CIA402State)
                {
                    case CIA402States.SWITCHED_ON:
                        SetSDOValue(saControlWord, (UInt16)0x000F);
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't enable drive", CIA402State.ToString());
                        break;
                }
            } else {
                switch (CIA402State)
                {
                    case CIA402States.OPERATION_ENABLED:
                        SetSDOValue(saControlWord, (UInt16)0x0007);
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't disable drive", CIA402State.ToString());
                        break;
                }
            }
        }

        public override void Power(bool poweron)
        {
            if (poweron)
            {
                switch (CIA402State)
                {
                    case CIA402States.FAULT:
                    case CIA402States.FAULT_REACTION_ACTIVE:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive in fault state, not ready to switch power on");
                        break;
                    case CIA402States.NOT_READY_TO_SWITCH_ON:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive not ready to switch power on");
                        break;
                    case CIA402States.SWITCH_ON_DISABLED:    
                    case CIA402States.READY_TO_SWITCH_ON:
                        SetSDOValue(saControlWord, (UInt16)0x0007); // Switch ON
                        break;
                }
            } else {
                SetSDOValue(saControlWord, (UInt16)0x0006);
            }
        }

        public override void ClearFault()
        {
            switch (CIA402State)
            {
                case CIA402States.FAULT:
                    SetSDOValue(saControlWord, (UInt16)0x0086);
                    break;
            }
        }

        CIA402States CIA402State => GetCIA402State();
        public CIA402States GetCIA402State()
        {
            if (ECMaster.GetSDO(saStatusWord, out SDO sdoStatusWord))
            {
                UInt16 statusword = sdoStatusWord.GetValue<UInt16>();

                if ((statusword & 0x004F)==0)
                    return CIA402States.NOT_READY_TO_SWITCH_ON;
                if ((statusword & 0x004F)==0x0040)
                    return CIA402States.SWITCH_ON_DISABLED;
                if ((statusword & 0x006F)==0x0021)
                    return CIA402States.READY_TO_SWITCH_ON;
                if ((statusword & 0x006F)==0x0023)
                    return CIA402States.SWITCHED_ON;
                if ((statusword & 0x006F)==0x0027)
                    return CIA402States.OPERATION_ENABLED;
                if ((statusword & 0x006F)==0x0007)
                    return CIA402States.QUICK_STOP_ACTIVE;
                if ((statusword & 0x004F)==0x000F)
                    return CIA402States.FAULT_REACTION_ACTIVE;
                if ((statusword & 0x004F)==0x0008)
                    return CIA402States.FAULT;
            }
            return CIA402States.UNDEFINED;
        }

    }
}