using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using System.Threading;
using System.Windows.Input;

namespace FAtoPA
{
    // Source : https://www.simplymodbus.ca/TCP.htm
    // Source : https://www.modbustools.com/modbus.html

    internal class ModbusSlave
    {
        public static bool Started = false;
        TcpListener listener;
        Dictionary<String, ModbusClientRecord> clientmap;
        UInt16[] Registers;
        Boolean running = false;
        public byte ID { get; set; } = 1;
        
        private ModbusSlaveEvent _event;

        /// <summary>
        /// Modbus Slave is actually a server (TCPListener),<br/>
        /// that contain data and can be read or written by <br/>
        /// a Modbus Master (which actually a TCPClient)
        /// </summary>
        /// <param name="maxregister">Maximum Register that can be read or written</param>
        public ModbusSlave(ModbusSlaveEvent callback, UInt16 maxregister)
        {
            _event = callback;
            clientmap = new Dictionary<String, ModbusClientRecord>();
            Registers = new UInt16[maxregister];
        }

        /// <summary>
        /// Get Register Count available in Modbus Slave
        /// </summary>
        /// <returns>Register Count</returns>
        public UInt16 GetRegisterCount()
        {
            return (UInt16)Registers.Length;
        }

        /// <summary>
        /// Set Register Value
        /// </summary>
        /// <param name="address">start from 0 until GetRegisterCount() - 1</param>
        /// <param name="value">between 0 until 65535</param>
        public void SetRegister(UInt16 address, UInt16 value)
        {
            if (address < Registers.Length)
            {
                Registers[address] = value;
            }
        }

        /// <summary>
        /// Get Register Value
        /// </summary>
        /// <param name="address">start from 0 until GetRegisterCount() - 1 </param>
        /// <returns>between 0 until 65535</returns>
        public UInt16 GetRegister(UInt16 address)
        {
            if (address < Registers.Length)
            {
                return Registers[address];
            }
            return 0;
        }

        /// <summary>
        /// Set Bit Value in Register
        /// </summary>
        /// <param name="address">start from 0 until GetRegisterCount() -1 </param>
        /// <param name="bit">start from 0 until 15</param>
        /// <param name="value">true = ON, false = OFF</param>
        public void SetBit(UInt16 address, UInt16 bit, Boolean value)
        {
            if (address < Registers.Length)
            {
                UInt16 reg = Registers[address];
                if (value)
                {
                    reg |= (UInt16)(1 << bit);
                }
                else
                {
                    reg &= (UInt16)~(1 << bit);
                }
                Registers[address] = reg;
            }
        }

        /// <summary>
        /// Get Bit Value in Register
        /// </summary>
        /// <param name="address">start from 0 until 65535</param>
        /// <param name="bit"></param>
        /// <returns></returns>
        public Boolean GetBit(UInt16 address, UInt16 bit)
        {
            if (address < Registers.Length)
            {
                UInt16 reg = Registers[address];
                return (reg & (1 << bit)) != 0;
            }
            return false;
        }

        /// <summary>
        /// Start Modbus Slave
        /// </summary>
        /// <param name="port">Listening Port</param>
        /// <returns>true if success</returns>
        public bool Start(UInt16 port)
        {
            
            try
            {
                TcpListener newlistener = new TcpListener(System.Net.IPAddress.Any, port);
                newlistener.Start();
                
                new Thread(()=> DoListening(newlistener, port)).Start();
                if (_event != null) _event.ConnectStatus(true, "Modbus Listener Started at port "+port);
                return true;
            }
            catch (Exception e)
            {
                if (_event!=null) _event.ConnectStatus(false, "Modbus Listener failed to start at port "+port+", Message : "+ e.Message);
            }
            return false;
        }

        /// <summary>
        /// Stop Modbus Slave
        /// </summary>
        public void Stop()
        {
            running = false;
            if (listener != null)
            {
                try
                {
                    
                    listener.Stop();
                    if (_event != null)
                    {
                        _event.ConnectStatus(true, "ModbusSlave: Stop Success");
                    }
                } catch (Exception e)
                {
                    if (_event != null)
                    {
                        _event.ConnectStatus(false, "ModbusSlave: Stop failed, exception: " + e.Message);
                    }
                }
                listener = null;
            }
            foreach (ModbusClientRecord client in clientmap.Values)
            {
                CloseConnection(client.Client);
                if (_event != null)
                {
                    _event.CloseConnection(client);
                }
            }
            
        }

        private void CloseConnection(TcpClient client)
        {
            if (client != null)
            {
                client.Close();
            }
        }

        /// <summary>
        /// Get Transaction ID from Modbus Command<br/>
        /// TransactionID is 2 bytes at index [0,1]
        /// 
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>TransactionID or 0 if failed</returns>
        private UInt16 GetTransactionID(byte[] cmd)
        {
            if (cmd.Length >= 2)
            {
                return (UInt16)((cmd[0] << 8) + cmd[1]);

            }
            return 0;
        }

        /// <summary>
        /// Get Protocol ID from Modbus Command<br/>
        /// ProtocolID is 2 bytes at index [2,3]<br/>
        /// For TCP Modbus, ProtocolID is always 0
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>ProtocolID</returns>
        private UInt16 GetProtocolID(byte[] cmd)
        {
            if (cmd.Length >= 4)
            {
                return (UInt16)((cmd[2] << 8) + cmd[3]);

            }
            return 0;
        }

        /// <summary>
        /// Get Length from Modbus Command<br/>
        /// Length is 2 bytes at index [4,5]<br/>
        /// Length is Modbus Payload Length + 2 (Unit ID and Function Code)
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>Length or 0 if failed</returns>
        private UInt16 GetLength(byte[] cmd)
        {
            if (cmd.Length >= 6)
            {
                return (UInt16)((cmd[4] << 8) + cmd[5]);

            }
            return 0;
        }

        /// <summary>
        /// Get Unit ID from Modbus Command<br/>
        /// UnitID is 1 byte at index [6]
        /// For Modbus Slave, UnitID is always 1
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>Unit ID or 0 if failed</returns>
        private Byte GetUnitID(byte[] cmd)
        {
            if (cmd.Length >= 7)
            {
                return cmd[6];

            }
            return 0;
        }

        /// <summary>
        /// Get Function Code from Modbus Command<br/>
        /// Function Code is 1 byte at index [7]
        /// 
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>Function Code or 0 if failed</returns>
        private Byte GetFunctionCode(byte[] cmd)
        {
            if (cmd.Length >= 8)
            {
                return cmd[7];

            }
            return 0;
        }

        /// <summary>
        /// Get Modbus Payload from Modbus Command<br/>
        /// Modbus Payload begins at index 8
        /// </summary>
        /// <param name="cmd">Modbus Command</param>
        /// <returns>Modbus Payload or empty array if failed</returns>
        private byte[] GetModbusPayload(byte[] cmd)
        {
            if (cmd.Length > 8)
            {
                byte[] data = new byte[cmd.Length - 8];
                Array.Copy(cmd,8, data, 0, data.Length);
                return data;
            }
            return Array.Empty<byte>();
        }

        async void DoListening(TcpListener listener, int port)
        {
            running = true;
            if (_event != null) _event.Log("ModbusSlave: Start Accepting Connections on port " + port);
            while (running)
            {
                TcpClient client = null;
                String key = null;
                try
                {
                    client = await listener.AcceptTcpClientAsync();
                    key = client.Client.RemoteEndPoint?.ToString();
                    if (key == null)
                    {
                        CloseConnection(client);
                        client = null;
                    }
                    

                }
                catch (Exception e)
                {
                    if (_event != null) _event.Log("ModbusSlave: Exception " + e.Message);
                }

                if (key != null && client != null)
                {
                    ModbusClientRecord prev = null;
                    if (_event!=null) _event.Log("ModbusSlave: New Connection from " + key);
                    if (clientmap.ContainsKey(key))
                    {
                        prev = clientmap[key];
                    }
                    
                    
                    if (prev != null)
                    {
                        CloseConnection(prev.Client);
                        if (_event != null)
                        {
                            _event.CloseConnection(prev);
                        }
                    }

                    ModbusClientRecord newclient = new ModbusClientRecord(client);
                    clientmap[key] = newclient;
                    new Thread(() => DoCommunication(newclient)).Start();

                }

            }
            if (_event!=null) _event.Log("ModbusSlave: Stop Accepting Connections on port " + port);
        }

        async void DoCommunication(ModbusClientRecord client)
        {
            // komunikasi di sini
            String key = client.remoteEP ?? "";
            if (_event != null)
            {
                _event.NewConnection(client);
                _event.Log("ModbusSlave: Start Communication with " + key);
            }
            NetworkStream stream = client.GetStream();
            while (running && stream != null)
            {
                // spare 12 bytes untuk protocol modbus Read Register :
                // 2 bytes Transaction ID, index [0,1]
                // 2 bytes Protocol ID (always 0) , index [2,3]
                // 2 bytes Length , index [4,5]
                // 1 byte Unit ID, index [6]
                // 1 byte Function Code, index [7]
                // (3 = Read Holding Register)
                // 2 byte Register Address, index [8,9]
                // 2 byte Register Count, index [10,11]
                byte[] cmd = new byte[12];
                try {
                    int readcount = await stream.ReadAsync(cmd, 0, 12);

                    if (readcount >= 12)
                    {
                        Debug.WriteLine($"Read from {key}, size={readcount}");
                        String cmdString = BitConverter.ToString(cmd);
                        Debug.WriteLine($"cmd={cmdString}");

                        client.RXBytes += (uint)readcount;
                        UInt16 transactionID = GetTransactionID(cmd);
                        UInt16 protocolID = GetProtocolID(cmd);
                        UInt16 length = GetLength(cmd);
                        Byte unitID = GetUnitID(cmd);
                        Byte functionCode = GetFunctionCode(cmd);
                        byte[] payload = GetModbusPayload(cmd);
                        Debug.WriteLine($"TransactionID={transactionID}, ProtocolID={protocolID}, Length={length}, UnitID={unitID}, FunctionCode={functionCode}, Payload={payload.Length}");
                        String payloadstring = BitConverter.ToString(payload);
                        Debug.WriteLine($"Payload={payloadstring}");
                        if (protocolID == 0 && length == payload.Length + 2 && unitID == 1)
                        {
                            if (functionCode == 3)
                            {
                                //Implement Read Register
                                UInt16 registerAddress = (UInt16)((payload[0] << 8) + payload[1]);
                                UInt16 registerCount = (UInt16)((payload[2] << 8) + payload[3]);
                                Boolean valid = (registerAddress + registerCount <= Registers.Length);
                                byte[] response;
                                if (valid)
                                {
                                    response = new byte[9 + registerCount * 2];
                                }
                                else
                                {
                                    // Register Address out of range
                                    response = new byte[9];
                                }

                                // 2 bytes Transaction ID, index [0,1]
                                response[0] = (byte)(transactionID >> 8);
                                response[1] = (byte)(transactionID & 0xFF);
                                // 2 bytes Protocol ID (always 0) , index [2,3]
                                response[2] = 0;
                                response[3] = 0;
                                // 2 bytes Length , index [4,5]
                                if (valid)
                                {
                                    UInt16 _length = (UInt16)(3 + registerCount * 2);
                                    response[4] = (byte)(_length >> 8);
                                    response[5] = (byte)(_length & 0xFF);
                                }
                                else
                                {
                                    response[4] = 0;
                                    response[5] = 3;
                                }
                                // Unit ID, index [6]
                                response[6] = unitID;
                                // Function Code, index [7]
                                response[7] = valid ? functionCode : (byte)(functionCode + 0x80);

                                // Number of Register or Exception Code, index [8]
                                if (valid)
                                {
                                    response[8] = (byte)(registerCount * 2);
                                    // Register values, 2 bytes each , start from index [9]
                                    for (int i = 0; i < registerCount; i++)
                                    {
                                        UInt16 value = GetRegister((UInt16)(registerAddress + i));
                                        response[9 + i * 2] = (byte)(value >> 8);
                                        response[10 + i * 2] = (byte)(value & 0xFF);
                                    }
                                    client.RXValidRequest++;
                                }
                                else
                                {
                                    // Exception Code 2 : Illegal Data Address
                                    response[8] = 2;
                                    client.RXInvalidRequest++;
                                }
                                // Send Response
                                await stream.WriteAsync(response, 0, response.Length);
                                client.TXBytes += (uint)response.Length;
                                client.TXResponse++;
                            }
                            else
                            {
                                client.RXInvalidRequest++;
                                // exception, invalid function code
                                if (_event != null) _event.Log("ModbusSlave: Invalid Function Code " + functionCode);
                                byte[] response = new byte[9];
                                // 2 bytes Transaction ID, index [0,1]
                                response[0] = (byte)(transactionID >> 8);
                                response[1] = (byte)(transactionID & 0xFF);
                                // 2 bytes Protocol ID (always 0) , index [2,3]
                                response[2] = (byte)(protocolID >> 8);
                                response[3] = (byte)(protocolID & 0xFF);
                                // 2 bytes Length , index [4,5]
                                response[4] = 0;
                                response[5] = 3;
                                // Unit ID, index [6]
                                response[6] = unitID;
                                // Function Code, index [7]
                                response[7] = (byte)(functionCode + 0x80);
                                // Exception Code 1 : Illegal Function
                                response[8] = 1;
                                await stream.WriteAsync(response, 0, response.Length);
                                client.TXBytes += (uint)response.Length;
                                client.TXResponse++;
                            }
                        }
                        else
                        {
                            client.RXInvalidRequest++;
                            // exception, invalid protocol ID, length, or unit ID
                            if (_event != null) _event.Log("ModbusSlave: Invalid Protocol ID, Length, or Unit ID");
                            byte[] response = new byte[9];
                            // 2 bytes Transaction ID, index [0,1]
                            response[0] = (byte)(transactionID >> 8);
                            response[1] = (byte)(transactionID & 0xFF);
                            // 2 bytes Protocol ID (always 0) , index [2,3]
                            response[2] = (byte)(protocolID >> 8);
                            response[3] = (byte)(protocolID & 0xFF);
                            // 2 bytes Length , index [4,5]
                            response[4] = 0;
                            response[5] = 3;
                            // Unit ID, index [6]
                            response[6] = unitID;
                            // Function Code, index [7]
                            response[7] = (byte)(functionCode + 0x80);
                            // Exception Code 3 : Illegal Query
                            response[8] = 3;
                            await stream.WriteAsync(response, 0, response.Length);
                            client.TXBytes += (uint)response.Length;
                            client.TXResponse++;
                        }
                    }

                    else if (readcount == 0) break;     // connection closed
                }
                catch (Exception e)
                {
                    if (_event!=null) _event.Log("ModbusSlave: Exception " + e.Message);
                    break;
                }
                
            }
            if (_event != null) _event.Log("ModbusSlave: Stop Communication with " + key);
            CloseConnection(client.Client);
            clientmap.Remove(key);
            if (_event != null) _event.CloseConnection(client);

        }
    }

    

    class ModbusClientRecord
    {
        public TcpClient Client;
        public UInt32 TXBytes, RXBytes;
        public UInt32 RXValidRequest, RXInvalidRequest, TXResponse;
        public String remoteEP;
        public ModbusClientRecord(TcpClient client)
        {
            Client = client;
            remoteEP = "";
            TXBytes = 0;
            RXBytes = 0;
            RXValidRequest = 0;
            RXInvalidRequest = 0;
            TXResponse = 0;
            if (Client != null)
            {
                if (Client.Connected)
                {
                    if (Client.Client.RemoteEndPoint != null)
                    {
                        remoteEP = Client.Client.RemoteEndPoint.ToString();
                    }
                }
            } 
        }

        public NetworkStream GetStream()
        {
            return Client?.GetStream() ?? null;
        }
    }

        interface ModbusSlaveEvent
    {
         void Log(String msg);
        void NewConnection(ModbusClientRecord client);
        void CloseConnection(ModbusClientRecord client);
         void ConnectStatus(bool success, string message);
         void TXRXStatusUpdate(ModbusClientRecord client);
    }
}