#include "config.h"

#ifdef MUDBUS

#include <ArduinoLog.h>

#include "Mudbus.h"
#include "enums.h"

EthernetServer MbServer(MB_PORT);
#define MbDebug
#define MAX_CLIENTS 8
EthernetClient clients[MAX_CLIENTS];



Mudbus::Mudbus()
{
  server = &MbServer;
}

void printBuffer(byte *buffer, int count)
{
  Serial.print("Buffer: ");
  for (int i = 0; i < count; i++)
  {
    if (buffer[i] < 16)
      Serial.print("0");
    Serial.print(buffer[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}

void Mudbus::Run()
{
  Runs = 1 + Runs * (Runs < 999);
  EthernetClient client = MbServer.available();
  
  if (client)
  {
    for (byte i = 0; i < MAX_CLIENTS; i++)
    {
      if (!clients[i])
      {
        Serial.print("We have a new client #");
        Serial.println(i);
        clients[i] = client;
        break;
      }
    }
  }
  for (byte i = 0; i < MAX_CLIENTS; i++)
  {
    if (clients[i] && clients[i].available() > 0)
    {
      int count = clients[i].read(ByteArray, 260);
      if (count)
      {
        Log.verbose(" client data : L=%d | \n", count);
        printBuffer(ByteArray, count);
        // Log.verbose(" client data : %d \n", count);
        //  Log.verbose(F("* FC : %d" CR), buffer[7]);
        SetFC(ByteArray[7]); // Byte 7 of request is FC

/*
        //  read bytes from a client
        if (FC == MB_FC_READ_COILS)
        {
          int Start, WordDataLength, ByteDataLength, CoilDataLength, MessageLength;
          Start = word(ByteArray[8], ByteArray[9]);
          CoilDataLength = word(ByteArray[10], ByteArray[11]);
          ByteDataLength = CoilDataLength / 8;
          if (ByteDataLength * 8 < CoilDataLength)
            ByteDataLength++;
          CoilDataLength = ByteDataLength * 8;
          ByteArray[5] = ByteDataLength + 3; // Number of bytes after this one.
          ByteArray[8] = ByteDataLength;     // Number of bytes after this one (or number of bytes of data).
          for (int i = 0; i < ByteDataLength; i++)
          {
            for (int j = 0; j < 8; j++)
            {
              bitWrite(ByteArray[9 + i], j, C[Start + i * 8 + j]);
            }
          }
          MessageLength = ByteDataLength + 9;
          // memcpy(ResponseBuffer, ByteArray + 1, MessageLength -1);
          Log.verbose(" WRITE CLIENT : Start:  %d : Length:%d | Coil Data : %d : -", Start, MessageLength, CoilDataLength);
          printBuffer(ByteArray, MessageLength);
          client.write(ByteArray, MessageLength);
          Writes = 1 + Writes * (Writes < 999);
          FC = MB_FC_NONE;
          return;
        }*/

        // memcpy(ByteArray, buffer, count);
      }
      else
      {
        Serial.println("client : no data");
      }

      /*
      while (client.available())
      {
        ByteArray[i] = client.read();
        Serial.print("Byte #");
        i++;
      }
      */
      // SetFC(ByteArray[7]); // Byte 7 of request is FC
      //  read bytes from a client
    }
  }

  /*
    if (client.available())
    {
      Serial.println("client!");
      Reads = 1 + Reads * (Reads < 999);
      int i = 0;
      while (client.available())
      {
        ByteArray[i] = client.read();
        i++;
      }
      SetFC(ByteArray[7]); // Byte 7 of request is FC
      if (!Active)
      {
        Active = true;
        PreviousActivityTime = millis();
  #ifdef MbDebug
        Serial.println("Mb active");
  #endif
      }
    }
    */

  if (!client)
  {
    //Serial.println("no client");
    //return;
  }

  if (millis() > (PreviousActivityTime + 60000))
  {
    if (Active)
    {
      Active = false;
#ifdef MbDebug
      Serial.println("Mb not active");
#endif
    }
  }

  int Start, WordDataLength, ByteDataLength, CoilDataLength, MessageLength;

  //****************** Read Coils **********************
  if (FC == MB_FC_READ_COILS)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    CoilDataLength = word(ByteArray[10], ByteArray[11]);
    ByteDataLength = CoilDataLength / 8;
    if (ByteDataLength * 8 < CoilDataLength)
      ByteDataLength++;
    CoilDataLength = ByteDataLength * 8;
#ifdef MbDebug
/*
    I = CA : 0 : 0 : 0 : 6 : 1 : 1 : 0 : 0 : 0 : 2
    R = CA : 0 : 0 : 0 : 4 : 1 : 1 : 1 : 0
    RR = 01 01 00 00 00 02 BD CB

  000000FC  19 74 00 00 00 06 01 01  00 00 00 02               
  000000D2  19 74 00 00 00 04 01 01  01 00
  
  00000408  3d 0f 00 00 00 06 01 01  00 00 00 01               =....... ....
  00000408  3d 0f 00 00 00 06 01 01  00 00 00 01               =....... ....

  33 : 00000000  3e a3 00 00 00 06 01 01  00 00 00 01               >....... ....
  177  00000000  3e a3 00 00 00 06 01 01  00 00 00 01               >....... ....

            3E A3 00 00 00 06 01 01 00 00 00 01 
            3E A3 00 00 00 04 01 01 01 00 


            3F 46 00 00 00 06 01 01 00 00 00 02
            3F 46 00 00 00 04 01 01 01 00

            3F 6D 00 00 00 06 01 01 00 00 00 03
            3F 6D 00 00 00 04 01 01 01 00
*/
/*
    Serial.print(" MB_FC_READ_COILS S=");
    Serial.print(Start);
    Serial.print(" L=");
    Serial.println(CoilDataLength);*/
#endif
    ByteArray[5] = ByteDataLength + 3; // Number of bytes after this one.
    ByteArray[8] = ByteDataLength;     // Number of bytes after this one (or number of bytes of data).
    for (int i = 0; i < ByteDataLength; i++)
    {
      for (int j = 0; j < 8; j++)
      {
        bitWrite(ByteArray[9 + i], j, C[Start + i * 8 + j]);
      }
    }
    MessageLength = ByteDataLength + 9;
    // memcpy(ResponseBuffer, ByteArray + 1, MessageLength -1);
    Log.verbose(" WRITE CLIENT : Start:  %d : Length:%d | Coil Data : %d : -\n", Start, MessageLength, CoilDataLength);
    printBuffer(ByteArray, MessageLength);
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  //****************** Read Registers ******************
  if (FC == MB_FC_READ_REGISTERS)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    WordDataLength = word(ByteArray[10], ByteArray[11]);
    ByteDataLength = WordDataLength * 2;
#ifdef MbDebug
    Serial.print(" MB_FC_READ_REGISTERS S=");
    Serial.print(Start);
    Serial.print(" L=");
    Serial.println(WordDataLength);
#endif
    ByteArray[5] = ByteDataLength + 3; // Number of bytes after this one.
    ByteArray[8] = ByteDataLength;     // Number of bytes after this one (or number of bytes of data).
    for (int i = 0; i < WordDataLength; i++)
    {
      ByteArray[9 + i * 2] = highByte(R[Start + i]);
      ByteArray[10 + i * 2] = lowByte(R[Start + i]);
    }
    MessageLength = ByteDataLength + 9;
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  //****************** Write Coil **********************
  if (FC == MB_FC_WRITE_COIL)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    C[Start] = word(ByteArray[10], ByteArray[11]) > 0;
#ifdef MbDebug
    Serial.print(" MB_FC_WRITE_COIL C");
    Serial.print(Start);
    Serial.print("=");
    Serial.println(C[Start]);
#endif
    ByteArray[5] = 2; // Number of bytes after this one.
    MessageLength = 8;
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  //****************** Write Register ******************
  if (FC == MB_FC_WRITE_REGISTER)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    R[Start] = word(ByteArray[10], ByteArray[11]);
#ifdef MbDebug
    Serial.print(" MB_FC_WRITE_REGISTER R");
    Serial.print(Start);
    Serial.print("=");
    Serial.println(R[Start]);
#endif
    ByteArray[5] = 6; // Number of bytes after this one.
    MessageLength = 12;
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  //****************** Write Multiple Coils **********************
  // Function codes 15 & 16 by Martin Pettersson http://siamect.com
  if (FC == MB_FC_WRITE_MULTIPLE_COILS)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    CoilDataLength = word(ByteArray[10], ByteArray[11]);
    ByteDataLength = CoilDataLength / 8;
    if (ByteDataLength * 8 < CoilDataLength)
      ByteDataLength++;
    CoilDataLength = ByteDataLength * 8;
#ifdef MbDebug
    Serial.print(" MB_FC_WRITE_MULTIPLE_COILS S=");
    Serial.print(Start);
    Serial.print(" L=");
    Serial.println(CoilDataLength);
#endif
    ByteArray[5] = ByteDataLength + 5; // Number of bytes after this one.
    for (int i = 0; i < ByteDataLength; i++)
    {
      for (int j = 0; j < 8; j++)
      {
        C[Start + i * 8 + j] = bitRead(ByteArray[13 + i], j);
      }
    }
    MessageLength = 12;
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  //****************** Write Multiple Registers ******************
  // Function codes 15 & 16 by Martin Pettersson http://siamect.com
  if (FC == MB_FC_WRITE_MULTIPLE_REGISTERS)
  {
    Start = word(ByteArray[8], ByteArray[9]);
    WordDataLength = word(ByteArray[10], ByteArray[11]);
    ByteDataLength = WordDataLength * 2;
#ifdef MbDebug
    Serial.print(" MB_FC_READ_REGISTERS S=");
    Serial.print(Start);
    Serial.print(" L=");
    Serial.println(WordDataLength);
#endif
    ByteArray[5] = ByteDataLength + 3; // Number of bytes after this one.
    for (int i = 0; i < WordDataLength; i++)
    {
      R[Start + i] = word(ByteArray[13 + i * 2], ByteArray[14 + i * 2]);
    }
    MessageLength = 12;
    client.write(ByteArray, MessageLength);
    Writes = 1 + Writes * (Writes < 999);
    FC = MB_FC_NONE;
  }

  // stop any clients which disconnect
  for (byte i = 0; i < 8; i++)
  {
    if (clients[i] && !clients[i].connected())
    {
      Serial.print("disconnect client #");
      Serial.println(i);
      clients[i].stop();
    }
  }

#ifdef MbDebug
/*
  Serial.print("Mb runs: ");
  Serial.print(Runs);
  Serial.print("  reads: ");
  Serial.print(Reads);
  Serial.print("  writes: ");
  Serial.print(Writes);
  Serial.println();
  */
#endif
}

void Mudbus::SetFC(int fc)
{
  if (fc == 1)
    FC = MB_FC_READ_COILS;
  if (fc == 3)
    FC = MB_FC_READ_REGISTERS;
  if (fc == 5)
    FC = MB_FC_WRITE_COIL;
  if (fc == 6)
    FC = MB_FC_WRITE_REGISTER;
  if (fc == 15)
    FC = MB_FC_WRITE_MULTIPLE_COILS;
  if (fc == 16)
    FC = MB_FC_WRITE_MULTIPLE_REGISTERS;
}
#endif