using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Threading;
using ln.ethercat.controller;
using ln.ethercat.controller.drives;
using ln.logging;
using ln.type;

namespace ln.ethercat
{

    public enum ECMasterState {
        INITIALIZED,
        STARTING,
        STOPPING,
        RUNNING,
    }

    public struct PDOMappingRequest
    {
        public ushort Slave;
        public ushort Index;
        public byte SubIndex;
        public bool RxPDO;

        public PDOMappingRequest(ushort slave,ushort index,byte subIndex,bool rxpdo)
        {
            Slave = slave;
            Index = index;
            SubIndex = subIndex;
            RxPDO = rxpdo;
        }

    }

    public delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
    public delegate void ProcessDataExchanged(ECMaster ecMaster);

    public class ECMaster
    {
        public int TIMEOUT_PREOP = 3000;
        public int TIMEOUT_SAFEOP = 10000;
        public int TIMEOUT_BACKTO_SAFEOP = 200;
        public int INTERVALL_PROCESSDATA = 1;
        public int INTERVALL_WATCHDOG = 250;


        public event ECStateChange OnStateChange;
        public event ProcessDataExchanged OnProcessDataExchanged;

        public string InterfaceName { get; }

        public int ExpectedWorkCounter { get; private set; }
        
        public int CountSlaves { get; private set; }
        
        public IntPtr IOMapPtr { get; private set; }
        public int IOMapSize { get; private set; }

        public ECMasterState MasterState { get; private set; } = ECMasterState.INITIALIZED;

        ECSlaveState ethercatState = ECSlaveState.NONE;
        public ECSlaveState EthercatState { 
                get =>ethercatState;
                private set {
                    if (value != ethercatState)
                    {
                        OnStateChange?.Invoke(this, value);
                        ethercatState = value;
                        Logging.Log(LogLevel.DEBUG, "ECMaster: EthercatState is now {0}", ethercatState);
                    }
                }

            }


        List<PDO> pdoMap = new List<PDO>();
        public PDO[] GetPDOMap() => pdoMap.ToArray();

        Dictionary<UInt16, ECSlave> slaves = new Dictionary<ushort, ECSlave>();

        public ECMaster(string interfaceName)
        {
            InterfaceName = interfaceName;
            
            int result = ECMBind.ecmbind_initialize(interfaceName);
            Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
            if (result<=0)
                throw new Exception("ecmbind_initialize failed");

            Controller = new Controller(this);
        }

        bool stopWatchdog;
        bool stopProcessData;

        Thread threadProcessData;
        Thread threadWatchdog;

        public Controller Controller { get; }
        List<DriveController> driveControllers = new List<DriveController>();
        public DriveController[] DriveControllers => driveControllers.ToArray();


        public void Start()
        {
            lock (this)
            {
                if (threadWatchdog?.IsAlive ?? false)
                    throw new Exception("already started");

                ethercatState = ECSlaveState.NONE;
                ExpectedWorkCounter = 0;

                stopWatchdog = false;

                threadWatchdog = new Thread(Watchdog);
                threadWatchdog.Start();
            }
        }

        public void Stop()
        {                
            lock (this)
            {
                if (threadWatchdog?.IsAlive ?? false)
                {
                    stopWatchdog = true;

                    lock (this)
                        Monitor.PulseAll(this);

                    threadWatchdog.Join();
                }
            }
        }

        void Watchdog()
        {
            while (!stopWatchdog)
            {
                IOMapSize = 0;
                if (threadProcessData?.IsAlive ?? false)
                {
                    stopProcessData = true;
                    while (threadProcessData.IsAlive)
                        threadProcessData.Join(250);
                }
                stopProcessData = false;

                EthercatState = ECSlaveState.INIT;

                CountSlaves = ECMBind.ecmbind_config_init();
                if (CountSlaves <= 0)
                {
                    Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected, scheduling restart...");
                    Thread.Sleep(2500);
                    continue;
                }


                if (!WaitForState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
                {
                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
                    Thread.Sleep(2500);
                    continue;
                }


                /* Bus State PRE_OP */
                ScanControllers();

                EthercatState = ECSlaveState.PRE_OP;

                ConfigureProcessDataMappings();
                SetupPDOMapping();
                UpdatePDOMap();
                
                threadProcessData = new Thread(ProcessData);
                threadProcessData.Start();

                Thread.Sleep(20);
                RequestSlaveState(0, ECSlaveState.SAFE_OP);

                if (!WaitForState(ECSlaveState.SAFE_OP, TIMEOUT_SAFEOP))
                {
                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach SAFE_OP. restart...");
                    Thread.Sleep(2500);
                    continue;
                }

                EthercatState = ECSlaveState.SAFE_OP;

                Thread.Sleep(20);
                RequestSlaveState(0, ECSlaveState.OPERATIONAL);
                
                Thread.Sleep(250);
                if (!WaitForState(ECSlaveState.OPERATIONAL, TIMEOUT_SAFEOP))
                {
                    Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach OPERATIONAL. restart...");
                    Thread.Sleep(2500);
                    continue;
                }

                EthercatState = ECSlaveState.OPERATIONAL;

                /*****  we handle the controller cycle from here on *****/

                Controller.Initialize();

                DateTime nextControllerRun = DateTime.Now;
                while (!stopWatchdog)
                {
                    EthercatState = IOLocked(()=>ECMBind.ecmbind_read_state());
                    if (EthercatState != ECSlaveState.OPERATIONAL)
                        break;

                    DateTime currentTime = DateTime.Now;
                    nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
                    if (currentTime > nextControllerRun)
                    {
                        Logging.Log(LogLevel.WARNING, "missed controller cycle");
                        while (currentTime > nextControllerRun)
                            nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
                    }
                    TimeSpan timeToNextCycle = nextControllerRun - currentTime;
                    try
                    {
                        Thread.Sleep(timeToNextCycle);
                        Controller.Cycle();
                    } catch (Exception e)
                    {
                        Logging.Log(e);
                    }
                }

            }

        }

        void ProcessData()
        {
            int wkc;

            lock (lockIOMap)
                ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();

            Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): start");

            while (!stopProcessData)
            {
                lock (lockIOMap)
                {
                    wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
                    if (wkc != ExpectedWorkCounter)
                    {
                        int success = ECMBind.ecmbind_recover();
                        if (success > 0)
                        {
                            Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery successfull...");
                            ExpectedWorkCounter = success;
                            //SetupPDOMapping();
                        } else
                        {
                            RequestSlaveState(0, ECSlaveState.INIT);
                            Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery failed, scheduling restart...");
                            ScheduleRestart();
                            stopProcessData = true;
                        }
                    } else {
                        OnProcessDataExchanged?.Invoke(this);
                    }
                }
                Thread.Sleep(INTERVALL_PROCESSDATA);
            }

            Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
        }

        public void ScanControllers()
        {
            driveControllers.Clear();

            for (UInt16 slave_id = 1; slave_id <= CountSlaves; slave_id++)
            {
                if (ReadSDO(slave_id, 0x1000, 0, out byte[] bDeviceType))
                {
                    ushort profilecode = BitConverter.ToUInt16(bDeviceType);
                    Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());

                    switch (profilecode)
                    {
                        case 402:
                            driveControllers.Add(new CIA402Controller(this, slave_id));
                            break;
                    }
                }
            }
        }


        bool WaitForState(ECSlaveState expectedState, int timeout)
        {
            int waitIncrement = 100;

            ECSlaveState currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
            while ((timeout > 0) && (currentState != expectedState))
            {
                Thread.Sleep(waitIncrement);
                timeout -= waitIncrement;
                currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
            }
            return currentState == expectedState;
        }



        object lockIOMap = new object();
        byte[] iomap = new byte[8192];

        void SetupPDOMapping()
        {
                IOMapSize = ECMBind.ecmbind_config_map();
                Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
                //UpdatePDOMap();
        }

        HashSet<PDOMappingRequest> mappingRequests = new HashSet<PDOMappingRequest>();

        public void ConfigureProcessDataMappings()
        {
            Logging.Log(LogLevel.INFO, "configure PDO mappings");

            for (UInt16 slave = 1; slave <= CountSlaves; slave++)
            {
                List<int> rxPDOs = new List<int>();
                List<int> txPDOs = new List<int>();

                foreach (PDOMappingRequest mappingRequest in mappingRequests)
                {
                    if (mappingRequest.Slave == slave)
                    {
                        if (GetSDOValue(mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex, out SDOValue sdoValue))
                        {
                            int mappingEntry = (mappingRequest.Index << 16) | (mappingRequest.SubIndex << 8) | sdoValue.BitLength;
                            Logging.Log(LogLevel.DEBUG, "PDO mapping: {0:X8}", mappingEntry);
                            
                            if (mappingRequest.RxPDO)
                                rxPDOs.Add(mappingEntry);
                            else
                                txPDOs.Add(mappingEntry);

                        } else {
                            Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not retrieve SDOValue {0}:{1:X4}.{2}", mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex);
                        }
                    }
                }

                MemoryStream rxStream = new MemoryStream();
                rxStream.WriteByte((byte)rxPDOs.Count);
                rxStream.WriteByte(0);
                foreach (int pdoMapping in rxPDOs)
                    rxStream.WriteBytes(pdoMapping.GetBytes());

                Logging.Log(LogLevel.DEBUG, "RxPDO mappings for slave {0} => {1}", slave, rxPDOs.Count);

                if (!WriteSDOCA(slave, 0x1600, 0, rxStream.ToArray()))
                {
                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write RxPDO mappings for slave {0}", slave);
                }

                MemoryStream txStream = new MemoryStream();
                txStream.WriteByte((byte)txPDOs.Count);
                txStream.WriteByte(0);
                foreach (int pdoMapping in txPDOs)
                    txStream.WriteBytes(pdoMapping.GetBytes());

                Logging.Log(LogLevel.DEBUG, "TxPDO mappings for slave {0} => {1}", slave, txPDOs.Count);

                if (!WriteSDOCA(slave, 0x1A00, 0, txStream.ToArray()))
                {
                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write TxPDO mappings for slave {0}", slave);
                }

                if (!WriteSDOCA(slave, 0x1c12, 0, new byte[]{ 0x01, 0x00, 0x00, 0x16 }))
                {
                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 2 PDO assignment for slave {0}", slave);
                }
                if (!WriteSDOCA(slave, 0x1c13, 0, new byte[]{ 0x01, 0x00, 0x00, 0x1A }))
                {
                    Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 3 PDO assignment for slave {0}", slave);
                }
            }
        }

        public void RequestPDOMapping(UInt16 slave,UInt16 index,byte subIndex, bool RxPDO) => RequestPDOMapping(new PDOMappingRequest(slave,index,subIndex,RxPDO));
        public void RequestPDOMapping(IEnumerable<PDOMappingRequest> mappingRequests) => RequestPDOMapping(mappingRequests.ToArray());
        public void RequestPDOMapping(params PDOMappingRequest[] mappingRequests)
        {
            foreach (PDOMappingRequest mappingRequest in mappingRequests)
                this.mappingRequests.Add(mappingRequest);
        }


        void ScheduleRestart()
        {
            ThreadPool.QueueUserWorkItem((o)=>{
                Stop();
                Thread.Sleep(500);
                Start();
            });
        }


        
        public bool ReadSDO(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
        {
            rawValue =
                new byte[128];
            int dataSize;

            lock (lockIOMap)
            {
                dataSize =
                    ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
                if (dataSize <= 0)
                {
                    Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) [{5}] == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString(), dataSize);
                    throw new IOException();
                }
            }
            rawValue = rawValue.Slice(0, dataSize);
            return true;
        }

        public bool ReadSDOCA(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
        {
            rawValue =
                new byte[128];
            int dataSize;

            lock (lockIOMap)
            {
                dataSize =
                    ECMBind.ecmbind_sdo_read_ca(slave, index, subIndex, rawValue, rawValue.Length );
                if (dataSize <= 0)
                {
                    Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read_ca({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
                    throw new IOException();
                }
            }
            rawValue = rawValue.Slice(0, dataSize);
            return true;
        }

        public bool WriteSDO(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
        {
            int result;
            lock (lockIOMap)
                result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
            //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
            return result > 0;
        }
        public bool WriteSDOCA(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
        {
            int result;
            lock (lockIOMap)
                result = ECMBind.ecmbind_sdo_write_ca(slave, index, subIndex, rawValue, rawValue.Length);
            //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
            return result > 0;
        }

        public ECSlaveState ReadSlaveState(int slave) => IOLocked(()=>ECMBind.ecmbind_get_slave_state(slave));
        int RequestSlaveState(int slave, ECSlaveState slaveState) => IOLocked(()=>ECMBind.ecmbind_write_slave_state(slave, slaveState));


        public bool GetSlave(UInt16 slave_id, out ECSlave slave)
        {
            if (!slaves.TryGetValue(slave_id, out slave))
            {
                if (slave_id > CountSlaves)
                    return false;

                slave = new ECSlave(this, slave_id);
                slaves.Add(slave_id, slave);
            }
            return true;
        }


        public bool GetSDOValue(SDOAddr address, out SDOValue sdoValue) => GetSDOValue(address.Slave, address.Index, address.SubIndex, out sdoValue);
        public bool GetSDOValue(UInt16 slave_id, UInt16 index,byte subIndex, out SDOValue sdoValue)
        {
            sdoValue = null;
            return GetSlave(slave_id, out ECSlave slave) && slave.GetDescriptor(index, out SDODescriptor descriptor) && descriptor.GetValue(subIndex, out sdoValue);
        }

        public bool GetIOmapData(int offset,int length,out byte[] rawData)
        {
            lock (lockIOMap)
            if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
            {
                rawData = iomap.Slice(offset, length);
                //Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
                return true;
            }
            Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
            rawData = null;
            return false;
        }
        public bool SetIOmapData(int offset,int length,byte[] rawData)
        {
            lock (lockIOMap)
            if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
            {
                //Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
                Buffer.BlockCopy(rawData, 0, iomap, offset, length);
                return true;
            }
            Logging.Log(LogLevel.ERROR, "SetIOmapData({0},{1}) failed", offset, length);
            rawData = null;
            return false;
        }

        public bool GetPDOItem(SDOValue sdoValue, out PDO pdo)
        {
            foreach (PDO _pdo in pdoMap)
            {
                if (_pdo.SDOValue.Equals(sdoValue))
                {
                    pdo = _pdo;
                    return true;
                }
            }
            pdo = null;
            return false;
        }

        public void UpdatePDOMap()
        {
            List<PDO> pdoList = new List<PDO>();
            lock (lockIOMap)
                ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
            if (addr_bit != 0)
                Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
            else
                pdoList.Add(new PDO(this,slave, index, subindex){
                    AddressOffset = addr_offset,
                    AddressBit = addr_bit,
                    BitLength = bitlength
                });
            });

            lock (lockIOMap)
            {
                IOMapPtr = ECMBind.ecmbind_get_iomap();
                pdoMap = pdoList;
            }
        }


        T IOLocked<T>(Func<T> f) {
            lock (lockIOMap)
                return f();
        }
    }
}