using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;
using System.IO.Ports;

namespace ConsoleApplication1
{

    abstract class Modbus
    {
        protected byte address;

        protected abstract byte[] read(byte function, ushort register, ushort count);

        protected abstract void write(byte function, ushort register, byte[] data);

        protected byte[] makePacket(byte function, ushort register, ushort count)
        {
            return new byte[] {
                address,                //slave address
                function,               //function code
                (byte)(register >> 8),  //start register high
                (byte)register,         //start register low
                (byte)(count >> 8),     //# of registers high
                (byte)count             //# of registers low
            };
        }

        protected byte[] makePacket(byte function, ushort register, byte[] data)
        {
            ushort count = (ushort)(data.Count() / 2);
            byte[] header = new byte[] {
                address,                //slave address
                function,               //function code
                (byte)(register >> 8),  //start register high
                (byte)register,         //start register low
                (byte)(count >> 8),     //# of registers high
                (byte)count,            //# of registers low
                (byte)data.Count()      //# of bytes to follow
            };
            return header.Concat(data).ToArray();
        }

        /// <summary>
        /// Read data from register(s).
        /// </summary>
        /// <param name="register">First data register.</param>
        /// <param name="count">Number of registers to read.</param>
        /// <returns>The data in the holding registers.</returns>
        public ushort[] ReadInputReg(ushort register, ushort count)
        {
            ushort[] data;
            int dataI = 0;
            byte[] rPacket = read(4, register, count);
            data = new ushort[(rPacket.Length) / 2];
            for (int i = 0; i < rPacket.Length; i += 2)
            {
                data[dataI] = rPacket[i];
                data[dataI] <<= 8;
                data[dataI] += rPacket[i + 1];
                dataI++;
            }
            return data;
        }

        /// <summary>
        /// Read data from register(s).
        /// </summary>
        /// <param name="register">First data register.</param>
        /// <param name="count">Number of registers to read.</param>
        /// <returns>The data in the holding registers.</returns>
        public ushort[] ReadHolding(ushort register, ushort count)
        {
            ushort[] data;
            int dataI = 0;
            byte[] rPacket = read(3, register, count);
            data = new ushort[(rPacket.Length) / 2];
            for (int i = 0; i < rPacket.Length; i += 2)
            {
                data[dataI] = rPacket[i];
                data[dataI] <<= 8;
                data[dataI] += rPacket[i + 1];
                dataI++;
            }
            return data;
        }

        public float[] ReadHoldingFloat(UInt16 register, UInt16 count)
        {
            byte[] rVal = read(3, register, (ushort)(count * 2));
            byte[] tempVals = new byte[4];
            float[] values = new float[rVal.Length / 4];
            for (int i = 0; i < rVal.Length; i += 4)
            {
                // Note this assumes the F4T is set to use the word order Low High (default)
                values[i / 4] = BitConverter.ToSingle(new byte[] { rVal[i + 1], rVal[i], rVal[i + 3], rVal[i + 2] }, 0);
            }
            return values;
        }

        /// <summary>
        /// Write data to register(s).
        /// </summary>
        /// <param name="register">First Holding register.</param>
        /// <param name="count">Number of registers to write.</param>
        /// <param name="data">Data to write.</param>
        public void WriteHolding(ushort register, ushort[] data)
        {
            byte[] bdata = new byte[data.Count() * 2];
            int i = 0;
            foreach (ushort item in data)
            {
                bdata[i] = (byte)(item >> 8);
                bdata[i + 1] = (byte)item;
                i += 2;
            }
            write(16, register, bdata);
        }

        public void WriteHolding(ushort register, ushort data)
        {
            WriteHolding(register, new ushort[] { data });
        }

        public void WriteHoldingFloat(ushort register, float[] data)
        {
            byte[] bdata = new byte[data.Count() * 4];
            byte[] mydata;
            int i = 0;
            foreach (float item in data)
            {
                mydata = BitConverter.GetBytes(item);
                bdata[i + 1] = mydata[0];
                bdata[i] = mydata[1];
                bdata[i + 3] = mydata[2];
                bdata[i + 2] = mydata[3];
                i += 4;
            }
            write(16, register, bdata);
        }

        public void WriteHoldingFloat(ushort register, float data)
        {
            WriteHoldingFloat(register, new float[] { data });
        }

        public void WriteHoldingFloat(ushort register, double data)
        {
            WriteHoldingFloat(register, (float)data);
        }

    }

    class ModbusTCP : Modbus
    {
        private TcpClient client;
        private Socket sock;
        private ushort id;

        public ModbusTCP(string ipAddress, int port = 502, byte adr = 1)
        {
            client = new TcpClient();
            address = adr;
            id = 0;
            IAsyncResult result = client.BeginConnect(ipAddress, port, null, null);
            if (!result.AsyncWaitHandle.WaitOne(3000, true))
            {
                client.Close();
                throw new ApplicationException("Failed to connect to server");
            }
            else
            {
                sock = client.Client;
                sock.ReceiveTimeout = 3000;
                sock.SendTimeout = 3000;
            }
        }

        private byte[] makeMBAP(ushort count)
        {
            byte[] idBytes = BitConverter.GetBytes((short)id);
            return new byte[] {
                idBytes[0],         //message id high byte
                idBytes[1],         //message id low byte
                0,                  //protocol id high byte
                0,                  //protocol id low byte
                (byte)(count >> 8), //length high byte
                (byte)(count)       //length low byte
            };
        }

        private byte[] sendReceive(byte[] packet)
        {
            byte[] mbap = new byte[7];
            byte[] response;
            byte[] rtn;
            ushort count;
            sock.Send(packet);
            sock.Receive(mbap, 0, mbap.Length, SocketFlags.None);
            count = mbap[4];
            count <<= 8;
            count += mbap[5];
            response = new byte[count - 1];
            sock.Receive(response, 0, response.Count(), SocketFlags.None);

            if (response[0] > 128)
                throw new System.IO.IOException("ModBus TCP error(" + (response[1]) + ")");
            return response;
        }

        protected override byte[] read(byte function, ushort register, ushort count)
        {
            byte[] rtn;
            byte[] packet = makePacket(function, register, count);
            byte[] mbap = makeMBAP((ushort)packet.Count());
            byte[] response = sendReceive(mbap.Concat(packet).ToArray());

            rtn = new byte[response[1]];
            Array.Copy(response, 2, rtn, 0, rtn.Length);
            return rtn;
        }

        protected override void write(byte function, ushort register, byte[] data)
        {
            byte[] packet;
            if (function == 16)
                packet = makePacket(function, register, data);
            else
                throw new System.IO.IOException("Only function code 16 is implimented for writes");
            sendReceive(makeMBAP((ushort)packet.Count()).Concat(packet).ToArray());
        }
    }


    class ModbusRTU : Modbus
    {
        private SerialPort sport;
        private int modbusTimeout = 30;

        public ModbusRTU(string portName, int baud, byte address = 1)
        {
            this.address = address;
            sport = new SerialPort(portName, baud, Parity.None, 8, StopBits.One);
            sport.ReadTimeout = 500;
            sport.WriteTimeout = 500;
            sport.Open();
        }

        private ushort makeCRC(byte[] data, int count)
        {
            ushort crc = 0xFFFF;
            byte lsb;

            for (int i = 0; i < count; i++ )
            {
                crc = (ushort)(crc ^ (ushort)data[i]);
                for (int j = 0; j < 8; j++)
                {
                    lsb = (byte)(crc & 0x0001);
                    crc = (ushort)((crc >> 1) & 0x7fff);
                    if (lsb == 1)
                        crc = (ushort)(crc ^ 0xA001);
                }
            }
            return crc;
        }

        private byte[] sendReceive(byte[] packet)
        {
            byte[] buffer = new byte[1024];
            byte[] rtn;
            int timeoutTemp;
            sport.Write(packet, 0, packet.Count());
            System.Threading.Thread.Sleep(modbusTimeout); // packets are time delimitted, per watlow use 30ms it does not follow spec.
            sport.Read(buffer, 0, 1); //wait for some data to be available, we should use a larger timeout here.
            timeoutTemp = sport.ReadTimeout;
            sport.ReadTimeout = modbusTimeout; // use the watlow spec'ed timeout.
            sport.ReadTimeout = timeoutTemp;
            sport.Read(buffer, 1, 1023);
            if (buffer[1] > 128)
            {
                throw new System.IO.IOException("ModBus TCP error(" + (buffer[2]) + ")");
            }
            else if (buffer[1] < 5)
            {
                rtn = new byte[buffer[2]];
                Array.Copy(buffer, 3, rtn, 0, buffer[2]);
            }
            else {
                rtn = new byte[4];
                Array.Copy(buffer, 2, rtn, 0, 4);
            }
            return rtn;
        }

        protected override byte[] read(byte function, ushort register, ushort count)
        {
            byte[] packet = makePacket(function, register, count);
            ushort crc = makeCRC(packet, packet.Count());
            byte[] crcbytes = new byte[] { (byte)(crc & 0xFF), (byte)((crc >> 8) & 0xFF) };
            return sendReceive(packet.Concat(crcbytes).ToArray());
        }

        protected override void write(byte function, ushort register, byte[] data)
        {
            byte[] packet;
            if (function == 16)
                packet = makePacket(function, register, data);
            else
                throw new System.IO.IOException("Only function code 16 is implimented for writes");
            ushort crc = makeCRC(packet, packet.Count());
            byte[] crcbytes = new byte[] { (byte)(crc & 0xFF), (byte)((crc >> 8) & 0xFF) };
            sendReceive(packet.Concat(crcbytes).ToArray());
        }
    }



    class Example
    {
        static void Main(string[] args)
        {
            float[] rsp;
            ushort[] ursp;

            Modbus device = new ModbusTCP("10.30.100.68");
            //Modbus device = new ModbusRTU("COM11", 38400, 1);
            //device.ReadHolding(107, 3);
            rsp = device.ReadHoldingFloat(2782, 1);
            Console.WriteLine(String.Format("Temperature Set Point: {0}", rsp[0]));
            device.WriteHoldingFloat(2782, 20.0);
            device.WriteHolding(16594, 62); //Turn profile event 1 off.
            rsp = device.ReadHoldingFloat(2782, 1);
            Console.WriteLine(String.Format("Temperature Set Point: {0}", rsp[0]));

            Console.WriteLine("Start profile 1");
            ursp = device.ReadHolding(16568, 1);
            if (ursp[0] == 149 || ursp[0] == 146)
            {
                Console.WriteLine("A profile is already running, we must terminate it before we can start a new one.");
                device.WriteHolding(16566, 148);
                System.Threading.Thread.Sleep(2000); // give the watlow time to update.
            }
            //now we actually start the profile.
            Console.WriteLine("Starting profile 1, step 1");
            device.WriteHolding(16558, 1); // profile number 1
            device.WriteHolding(16560, 1); // profile step 1
            device.WriteHolding(16562, 1782); // start the profile

            Console.Write("Press Enter to quit.");
            Console.ReadLine();
        }
    }
}