using System;
using System.Runtime;

namespace ln.ethercat.controller.drives
{
    public enum DriveStates {
        UNDEFINED,
        BOOT,
        INIT,
        ERROR,
        POWERED,
        OPERATIONAL,
    }

    public enum DriveMode {
        UNDEFINED,
        TORQUE,
        SPEED,
        POSITION
    }

    public abstract class DriveController
    {
        protected ECMaster ECMaster { get; }
        public int Slave { get; }

        public DriveController(ECMaster ecMaster, int slave)
        {
            ECMaster = ecMaster;
            Slave = slave;
        }

        /* called by controller before control loops and logic */
        public abstract void UpdateStates();
        /* called by controller after user logic */
        public abstract void UpdateDrive();

        public abstract DriveStates DriveState { get; }
        public abstract string OEMDriveState { get; }
        public abstract Int32 ErrorCode { get; }
        public abstract string ErrorText { get; }

        public abstract void ClearFault();

        public abstract DriveMode DriveMode { get; set; }

        public void PowerOn() => Power(true);
        public void PowerOff() => Power(false);
        public abstract void Power(bool poweron);

        public void EnableDrive() => EnableDrive(true);
        public void DisableDrive() => EnableDrive(false);
        public abstract void EnableDrive(bool enabled);

        public abstract decimal ActualPosition { get; }
        public abstract decimal ActualSpeed { get; }
        public abstract decimal ActualTorque { get; }
        public abstract decimal TargetPosition { get; set; }
        public abstract decimal TargetSpeed { get; set; }
        public abstract decimal TargetTorque { get; set; }
        

        public T GetSDOValue<T>(UInt16 index,byte subIndex)
        {
            if (ECMaster.GetSDO(new SDOAddr(Slave, index, subIndex), out SDO sdo))
                return sdo.GetValue<T>();
            throw new Exception("DriveController: failed to get sdo value");
        }
        public T GetSDOValue<T>(SDOAddr sa)
        {
            if (ECMaster.GetSDO(sa, out SDO sdo))
                return sdo.GetValue<T>();
            throw new Exception("DriveController: failed to get sdo value");
        }
        
        public void SetSDOValue<T>(UInt16 slave, UInt16 index, byte subIndex, T value) => SetSDOValue(new SDOAddr(slave,index,subIndex), value);
        public void SetSDOValue<T>(SDOAddr sa, T value)
        {
            if (ECMaster.GetSDO(sa, out SDO sdo))
                sdo.Value = value;
        }


    }

}