#define ETH_CLK_MODE ETH_CLOCK_GPIO17_OUT
#define ETH_PHY_ADDR      1
#define ETH_PHY_MDC       23
#define ETH_PHY_MDIO      18
#define ETH_PHY_POWER     2
#define ETH_PHY_TYPE      ETH_PHY_LAN8720

#define RXD2 16
#define TXD2 14

#include <ETH.h>
#include <WiFi.h>
#include <Ticker.h>

static bool eth_connected = false;

IPAddress local_IP(192, 168, 1, 25);
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);
IPAddress primaryDNS(8, 8, 8, 8); //optional
IPAddress secondaryDNS(4, 4, 4, 4); //optional

boolean debug = false;

int CC300ms = 0;
int CC10s = 0;
int CC60s = 0;

uint8_t sbuf[2048];

#define MAX_COILS 8
#define COILS_ADDR 1000

#define MAX_INPUTS 8
#define INPUTS_ADDR 2000

uint8_t COILS_[MAX_COILS];              // OUTPUTS
uint8_t INPUTS[MAX_INPUTS];             // INPUTS

uint32_t data_cnt = 0;

Ticker tickerSystemUptime;

#define time_out 600  // 60sec TCP time out without poll
#define MAX_SRV_CLIENTS 5

WiFiServer server(502);
WiFiClient serverClients[MAX_SRV_CLIENTS];

uint32_t client_time_out[MAX_SRV_CLIENTS];

// Increment time counters
void ticker_system_uptime_inc(int i) {
  if (eth_connected == false) {
    CC60s++;
  }
  else {
    CC300ms++;
    CC10s++;
    for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
      client_time_out[i]++;
    }
  }
}

void WiFiEvent(WiFiEvent_t event)
{
  switch (event) {
    case SYSTEM_EVENT_ETH_START:
      if (debug) Serial.println("ETH Started");
      ETH.setHostname("esp32-ethernet");
      break;
    case SYSTEM_EVENT_ETH_CONNECTED:
      if (debug) Serial.println("ETH Connected");
      break;
    case SYSTEM_EVENT_ETH_GOT_IP:
      if (eth_connected == true) break;
      if (debug) Serial.print("ETH MAC: ");
      if (debug) Serial.print(ETH.macAddress());
      if (debug) Serial.print(", IPv4: ");
      if (debug) Serial.print(ETH.localIP());
      if (ETH.fullDuplex()) {
        if (debug) Serial.print(", FULL_DUPLEX");
      }
      if (debug) Serial.print(", ");
      if (debug) Serial.print(ETH.linkSpeed());
      if (debug) Serial.println("Mbps");
      eth_connected = true;
      CC60s = 0;
      break;
    case SYSTEM_EVENT_ETH_DISCONNECTED:
      if (debug) Serial.println("ETH Disconnected");
      eth_connected = false;
      CC60s = 0;
      break;
    case SYSTEM_EVENT_ETH_STOP:
      if (debug) Serial.println("ETH Stopped");
      eth_connected = false;
      CC60s = 0;
      break;
    default:
      if (debug) Serial.println("Unknown event");
      break;
  }

  if (debug) Serial.flush();

}

// CRC16 calculation
// ----------------
unsigned int CRC16(unsigned int crc, unsigned int data_) {
  const unsigned int Poly16 = 0xA001;
  unsigned int LSB, i;
  crc = ((crc ^ data_) | 0xFF00) & (crc | 0x00FF);
  for (i = 0; i < 8; i++) {
    LSB = (crc & 0x0001);
    crc = crc/2;
    if (LSB)
    crc = crc^Poly16;
  } 
  return(crc);
}

void send_serial2() {

  digitalWrite(12, HIGH); // LED

  while (Serial2.available() > 0) char t = Serial2.read(); // empty reveive buffer
  
  sbuf[0] = (byte) data_cnt;
  sbuf[1] = (byte) (data_cnt >> 8);
  sbuf[2] = (byte) (data_cnt >> 16);
  sbuf[3] = (byte) (data_cnt >> 24);
  
  data_cnt++;

  int j = 4;
  
  for (int i = 0; i < 8; i++) {
    sbuf[j] = COILS_[i];
    j++;  
  }

  unsigned int Crc = 0xFFFF;
  
  for (int i = 0; i < j; i++) {
    Crc = CRC16 (Crc, sbuf[i] );
  }
  
  sbuf[j] = (byte) Crc;
  j++;
  sbuf[j] = (byte) (Crc >> 8);
  j++;

  for (int i = 0; i < j; i++) {
    Serial2.write(sbuf[i]);
  }
  Serial2.flush(); 
}

void process_serial2() {
  if (Serial2.available()) {

    delay(5);

    size_t slen = Serial2.available();

    if (slen > 64) slen = 64;

    Serial2.readBytes(sbuf, slen);

    while (Serial2.available() > 0) char t = Serial2.read();

    if (slen == 22) {
      
      unsigned int Crc = 0xFFFF;
      unsigned int Crc1 = 0;
      
      for (int i = 0; i < 20; i++) {
        Crc = CRC16 (Crc, sbuf[i] );
      }

      Crc1 = (unsigned int)(sbuf[21] << 8) | sbuf[20];

      if (Crc == Crc1) {
        
        int data_cnt_buf;
        
        data_cnt_buf = (unsigned long)(sbuf[3] << 24) | (unsigned long)(sbuf[2] << 16) | (unsigned long)(sbuf[1] << 8) | sbuf[0];
        
        if (data_cnt == data_cnt_buf) {

          digitalWrite(12, LOW); // LED

          int j = 12;
  
          for (int i = 0; i < 8; i++) {
            INPUTS[i] = sbuf[j];
            j++;  
          }
  
        }
        
      }
      
      if (debug) Serial.flush();
    }
    
  }
  
}

void setup() {
  pinMode(2, OUTPUT);
  digitalWrite(2, LOW);
  delay(200);

  Serial.begin(115200);
  
  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);

  WiFi.onEvent(WiFiEvent);

  ETH.begin();

  ETH.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS);

  pinMode(12, OUTPUT);
  digitalWrite(12, LOW);// LED
  
  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);// 24V POWER, HIGH to ENABLE

  delay(100);

  digitalWrite(4, HIGH);// 24V POWER ENABLE

  for (int i = 0; i < MAX_COILS; i++) {
    COILS_[i] = 0;  
  }
  for (int i = 0; i < MAX_INPUTS; i++) {
    INPUTS[i] = 0;
  }  

  for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
    client_time_out[i] = 0;
  }
  
  //start server
  server.begin();
  server.setNoDelay(true);

  tickerSystemUptime.attach_ms(100, ticker_system_uptime_inc, 0);
}

void check_clients_timeout() {
  if (CC10s > 100) {
    CC10s = 0;
    
    for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
      if (!serverClients[i].connected()) {
        client_time_out[i] = 0;
        serverClients[i].flush();
        serverClients[i].stop();
      }
      else {
        if (client_time_out[i] >= time_out) {
          client_time_out[i] = 0;
          serverClients[i].flush();
          serverClients[i].stop();  
        }
      }
    }
  }
}

int Generate_Error(int error_core) {
  sbuf[7] += 0x80;
  sbuf[8] = error_core;
  return 9;
}

void process_modbus() {
  //check if there are any new clients
  if (server.hasClient()) {
    //find free/disconnected spot
    int i;
    for (i = 0; i < MAX_SRV_CLIENTS; i++)
      if (!serverClients[i]) { // equivalent to !serverClients[i].connected()
        serverClients[i] = server.available();
        break;
      }
      
    //no free/disconnected spot so reject
    if (i == MAX_SRV_CLIENTS) server.available();
      // hints: server.available() is a WiFiClient with short-term scope
      // when out of scope, a WiFiClient will
      // - flush() - all data will be sent
      // - stop() - automatically too
  }

  //check TCP clients for data
  for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
    int j = 0;
    int k;
    int t;
    uint8_t b;
    int m;
    int addr;
    int len;
    int error_code;
    
    while (serverClients[i].available()) {
      // working char by char is not very efficient
      sbuf[j] = serverClients[i].read(); 
      if (j++ > 256) j = 256;
    }
    
    if (j != 0) {

      client_time_out[i] = 0;
      
      error_code = 0x01;

      if (j >= 270) {
        serverClients[i].flush();
        serverClients[i].stop();  
      }
      else if (sbuf[7] == 0x01) {
        addr = sbuf[8] << 8;
        addr += sbuf[9];
        error_code = 0x02;
        if (addr < COILS_ADDR) goto error_;
        if (addr > (COILS_ADDR + MAX_COILS)) goto error_;
        len = sbuf[11];
        error_code = 0x03;
        if (len == 0) goto error_;
        if (len > MAX_COILS) goto error_;
        if ((addr + len) > (COILS_ADDR + MAX_COILS)) goto error_; 
        t = 0;
        k = 1;
        m = 0;
        b = 1;
        sbuf[8 + k] = 0;
        while (t < len) {
          if (m == 8) {
            m = 0;
            b = 1;
            k++;
            sbuf[8 + k] = 0;
          }
          if (COILS_[addr-COILS_ADDR] != 0) {
            sbuf[8 + k] = (sbuf[8 + k] | b);
          }
          b = b << 1;
          m++;
          addr++;
          t++;
        }
        sbuf[4] = 0;
        sbuf[5] = k + 3;
        sbuf[8] = k;
        serverClients[i].write(sbuf, k + 9);        
      }
      else if (sbuf[7] == 0x02) {
        addr = sbuf[8] << 8;
        addr += sbuf[9];
        error_code = 0x02;
        if (addr < INPUTS_ADDR) goto error_;
        if (addr > (INPUTS_ADDR + MAX_INPUTS)) goto error_;
        len = sbuf[11];
        error_code = 0x03;
        if (len == 0) goto error_;
        if (len > MAX_INPUTS) goto error_;
        if ((addr + len) > (INPUTS_ADDR + MAX_INPUTS)) goto error_; 
        t = 0;
        k = 1;
        m = 0;
        b = 1;
        sbuf[8 + k] = 0;
        while (t < len) {
          if (m == 8) {
            m = 0;
            b = 1;
            k++;
            sbuf[8 + k] = 0;
          }
          if (INPUTS[addr-INPUTS_ADDR] != 0) {
            sbuf[8 + k] = (sbuf[8 + k] | b);
          }
          b = b << 1;
          m++;
          addr++;
          t++;
        }
        sbuf[4] = 0;
        sbuf[5] = k + 3;
        sbuf[8] = k;
        serverClients[i].write(sbuf, k + 9);        
      }
      else if (sbuf[7] == 0x05) {
        addr = sbuf[8] << 8;
        addr += sbuf[9];
        error_code = 0x02;
        if (addr < COILS_ADDR) goto error_;
        if (addr > (COILS_ADDR + MAX_COILS)) goto error_;
        if (sbuf[10] != 0) {
          COILS_[addr-COILS_ADDR] = 1;  
        }
        else {
          COILS_[addr-COILS_ADDR] = 0;  
        }
        sbuf[4] = 0;
        sbuf[5] = 6;
        serverClients[i].write(sbuf, 12);        
      }
      else if (sbuf[7] == 0x0F) {
        addr = sbuf[8] << 8;
        addr += sbuf[9];
        error_code = 0x02;
        if (addr < COILS_ADDR) goto error_;
        if (addr > (COILS_ADDR + MAX_COILS)) goto error_;
        len = sbuf[11];
        error_code = 0x03;
        if (len == 0) goto error_;
        if (len > MAX_COILS) goto error_;
        if ((addr + len) > (COILS_ADDR + MAX_COILS)) goto error_; 
        t = 0;
        k = 13;
        m = 0;
        b = 1;
        while (t < len) {
          if (m == 8) {
            m = 0;
            b = 1;
            k++;
          }
          if ((sbuf[k] & b) != 0) {
            COILS_[addr-COILS_ADDR] = 1;  
          }
          else {
            COILS_[addr-COILS_ADDR] = 0;   
          }
          b = b << 1;
          m++;
          addr++;
          t++;
        }
        sbuf[4] = 0;
        sbuf[5] = 6;
        serverClients[i].write(sbuf, 12);        
      }
      else {
      error_:
        serverClients[i].write(sbuf, Generate_Error(error_code));  
      } 
    }  
  }
}

void loop() {
  process_serial2();

  process_modbus();

  if (CC300ms > 2) {

    CC300ms = 0;

    send_serial2();

  }

  check_clients_timeout();
  
  if (CC60s > 599) {
    CC60s = 0;
    ESP.restart();
  }

}