#include "WS_RS485.h"
#include <algorithm>
#include <cstring>
#include <endian.h>

HardwareSerial lidarSerial(1); // Using serial port 1
uint8_t data[][8] = {
    // ESP32-S3-POE-ETH-8DI-8RO Control Command (RS485 receiving data)
    {0x06, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA2, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH1 Toggle
    {0x06, 0x05, 0x00, 0x02, 0x55, 0x00, 0x52, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH2 Toggle
    {0x06, 0x05, 0x00, 0x03, 0x55, 0x00, 0x03, 0x2D}, // ESP32-S3-POE-ETH-8DI-8RO CH3 Toggle
    {0x06, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB2, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH4 Toggle
    {0x06, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE3, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH5 Toggle
    {0x06, 0x05, 0x00, 0x06, 0x55, 0x00, 0x13, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH6 Toggle
    {0x06, 0x05, 0x00, 0x07, 0x55, 0x00, 0x42, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH7 Toggle
    {0x06, 0x05, 0x00, 0x08, 0x55, 0x00, 0x72, 0xEF}, // ESP32-S3-POE-ETH-8DI-8RO CH8 Toggle
    {0x06, 0x05, 0x00, 0xFF, 0xFF, 0x00, 0xBD, 0xBD}, // ESP32-S3-POE-ETH-8DI-8RO ALL ON
    {0x06, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFC, 0x4D}, // ESP32-S3-POE-ETH-8DI-8RO ALL OFF
};
uint8_t Send_Data[][8] = {
    // Modbus RTU Relay Control Command (RS485 send data)
    {0x01, 0x05, 0x00, 0x00, 0x55, 0x00, 0xF2, 0x9A}, // Modbus RTU Relay CH1 Toggle
    {0x01, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA3, 0x5A}, // Modbus RTU Relay CH2 Toggle
    {0x01, 0x05, 0x00, 0x02, 0x55, 0x00, 0x53, 0x5A}, // Modbus RTU Relay CH3 Toggle
    {0x01, 0x05, 0x00, 0x03, 0x55, 0x00, 0x02, 0x9A}, // Modbus RTU Relay CH4 Toggle
    {0x01, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB3, 0x5B}, // Modbus RTU Relay CH5 Toggle
    {0x01, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE2, 0x9B}, // Modbus RTU Relay CH6 Toggle
    {0x01, 0x05, 0x00, 0x06, 0x55, 0x00, 0x12, 0x9B}, // Modbus RTU Relay CH7 Toggle
    {0x01, 0x05, 0x00, 0x07, 0x55, 0x00, 0x43, 0x5B}, // Modbus RTU Relay CH8 Toggle
    {0x01, 0x05, 0x00, 0xFF, 0xFF, 0xFF, 0xFC, 0x4A}, // Modbus RTU Relay ALL ON
    {0x01, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFD, 0xFA}, // Modbus RTU Relay ALL OFF
};
uint8_t buf[20] = {0}; // Data storage area
int numRows = sizeof(data) / sizeof(data[0]);

void SetData(uint8_t *data, size_t length)
{
  lidarSerial.write(data, length); // Send data from the RS485
}
void ReadData(uint8_t *buf, uint8_t length)
{
  uint8_t Receive_Flag = 0;
  Receive_Flag = lidarSerial.available();
  if (Receive_Flag >= length)
  {
    lidarSerial.readBytes(buf, length);
    char printBuf[length * 3 + 1];
    sprintf(printBuf, "Received data: ");
    for (int i = 0; i < length; i++)
    {
      sprintf(printBuf + strlen(printBuf), "%02X ", buf[i]);
    }
    printf(printBuf);
    /*************************
    Add a receiving data handler
    *************************/
    Receive_Flag = 0;
    memset(buf, 0, sizeof(buf));
  }
}
void RS485_Analysis(uint8_t *buf)
{
  switch (buf[1])
  {
  case Extension_CH1:
    SetData(Send_Data[0], sizeof(Send_Data[0]));
    printf("|***  Toggle expansion channel 1 ***|\r\n");
    break;
  case Extension_CH2:
    SetData(Send_Data[1], sizeof(Send_Data[1]));
    printf("|***  Toggle expansion channel 2 ***|\r\n");
    break;
  case Extension_CH3:
    SetData(Send_Data[2], sizeof(Send_Data[2]));
    printf("|***  Toggle expansion channel 3 ***|\r\n");
    break;
  case Extension_CH4:
    SetData(Send_Data[3], sizeof(Send_Data[3]));
    printf("|***  Toggle expansion channel 4 ***|\r\n");
    break;
  case Extension_CH5:
    SetData(Send_Data[4], sizeof(Send_Data[4]));
    printf("|***  Toggle expansion channel 5 ***|\r\n");
    break;
  case Extension_CH6:
    SetData(Send_Data[5], sizeof(Send_Data[5]));
    printf("|***  Toggle expansion channel 6 ***|\r\n");
    break;
  case Extension_CH7:
    SetData(Send_Data[6], sizeof(Send_Data[6]));
    printf("|***  Toggle expansion channel 7 ***|\r\n");
    break;
  case Extension_CH8:
    SetData(Send_Data[7], sizeof(Send_Data[7]));
    printf("|***  Toggle expansion channel 8 ***|\r\n");
    break;
  case Extension_ALL_ON:
    SetData(Send_Data[8], sizeof(Send_Data[8]));
    printf("|***  Enable all extension channels ***|\r\n");
    break;
  case Extension_ALL_OFF:
    SetData(Send_Data[9], sizeof(Send_Data[9]));
    printf("|***  Close all expansion channels ***|\r\n");
    break;
  default:
    printf("Note : Non-control external device instructions !\r\n");
  }
}
uint32_t Baudrate = 0;
double transmission_time = 0;
double RS485_cmd_Time = 0;
void RS485_Init() // Initializing serial port
{
  Baudrate = 9600;                                     // Set the baud rate of the serial port
  lidarSerial.begin(Baudrate, SERIAL_8N1, RXD1, TXD1); // Initializing serial port
  transmission_time = 10.0 / Baudrate * 1000;
  RS485_cmd_Time = transmission_time * 8; // 8:data length
  xTaskCreatePinnedToCore(
      RS485Task,
      "RS485Task",
      4096,
      NULL,
      3,
      NULL,
      0);
}

void RS485Task(void *parameter)
{
  while (1)
  {
    RS485_Loop();
    vTaskDelay(pdMS_TO_TICKS(50));
  }
  vTaskDelete(NULL);
}

void RS485_Loop()
{
  uint8_t Receive_Flag = 0; // Receiving mark
  Receive_Flag = lidarSerial.available();

  if (Receive_Flag > 0)
  {
    if (RS485_cmd_Time > 1) // Time greater than 1 millisecond
      delay((uint16_t)RS485_cmd_Time);
    else // Time is less than 1 millisecond
      delay(1);
    Receive_Flag = lidarSerial.available();
    lidarSerial.readBytes(buf, Receive_Flag); // The Receive_Flag length is read
    if (Receive_Flag == 8)
    {
      uint8_t i = 0;
      for (i = 0; i < numRows; i++)
      {
        bool result = std::equal(std::begin(buf), std::begin(buf) + 8, std::begin(data[i])); // Compare two arrays
        if (result)
        {
          if (i < numRows - 1)
            buf[0] = i + 1 + 48;
          else if (i == numRows - 1)
            buf[0] = 48;
          Relay_Analysis(buf, RS485_Mode);
          break;
        }
      }
      if (i > numRows - 1)
        printf("Note : Non-instruction data was received - RS485 !\r\n");
    }
    else
    {
      printf("Note : Non-instruction data was received .Number of bytes: %d - RS485 !\r\n", Receive_Flag);
    }
    Receive_Flag = 0;
    memset(buf, 0, sizeof(buf));
  }
}

namespace drivers
{

  ////////////////////////////////
  //////////// RS485  ////////////
  ////////////////////////////////

  RS485::RS485(const uint32_t baud, const SerialConfig conf)
  {
    log_i("Init serial port 1");
    m_serial = std::make_unique<HardwareSerial>(PORT); // RS485 is hardwired to serial port 1
    m_serial->begin(baud, conf);
    m_serial->setMode(UART_MODE_RS485_HALF_DUPLEX);
  }

  const bool RS485::write(const std::vector<uint8_t> data)
  {
    return data.size() == m_serial->write(data.data(), data.size());
  }

  const bool RS485::readAll(std::vector<uint8_t> &data)
  {
    const uint32_t avail(m_serial->available());
    data.resize(avail);
    return data.size() == m_serial->readBytes(data.data(), avail);
  }

  const bool RS485::readN(const uint16_t nBytes, std::vector<uint8_t> &data)
  {
    data.resize(nBytes);
    return data.size() == m_serial->readBytes(data.data(), nBytes);
  }

  const bool RS485::readUntil(const uint8_t ch, std::vector<uint8_t> &data)
  {
    const uint32_t avail(m_serial->available());
    data.resize(avail);
    m_serial->readBytesUntil(ch, data.data(), avail);
    data.shrink_to_fit();
  }

  ////////////////////////////////
  //////////// MODBUS ////////////
  ////////////////////////////////

  MODBUS::MODBUS(const uint32_t baud, const SerialConfig conf) : RS485::RS485(baud, conf)
  {
    log_i("Init MODBUS Master Mode");
  }

  // Func 0x01
  const bool MODBUS::readCoils(const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<bool> &coils)
  {
    constexpr uint8_t func = 0x01;
    readBinary(func, device, reg, num, coils);
  }

  // Func 0x02
  const bool MODBUS::readInputs(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<bool> &inputs)
  {
    constexpr uint8_t func = 0x01;
    readBinary(func, device, reg, num, inputs);
  }

  const bool MODBUS::readBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, std::vector<bool> &out)
  {
    if (!write(buildRequest(device, func, reg, bits)))
    {
      log_e("Failed send readCoils command");
      return false;
    }
    const uint16_t nRespDataBytes = 1 + (bits % 8);                                       // 1 bit for every coil, if not 8 mutiple padded with zeroes
    const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      log_e("Failed receive readCoils response");
      return false;
    }

    // element 2 of response has the response data bytes expected
    if (response.at(2) != nRespDataBytes)
    {
      log_e("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
      return false;
    }

    // compute crc of current message
    if (!verifyCrc(response))
      return false;

    // extract coils data from data portion of response
    out.clear();
    out.reserve(bits);
    uint16_t bitNum(0);

    // get response data bytes excluding header and crc
    const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
    for (auto it = respData.begin(); it < respData.end(); it++)
    {
      const auto v = *it;
      for (uint8_t j(0); j < 8 && bitNum < bits; j++)
      {
        const auto cv((0x01 << j) && v);
        out.push_back(cv);
        bitNum++;
      }
    }
  }

  // Func 0x03
  const bool MODBUS::readHoldingRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
  {
  }

  // Func 0x04
  const bool MODBUS::readInputRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
  {
  }

  const bool MODBUS::readInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out)
  {
    if (!write(buildRequest(device, func, reg, num)))
    {
      log_e("Failed send readCoils command");
      return false;
    }
    const uint16_t nRespDataBytes = num * sizeof(uint16_t);
    const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      log_e("Failed receive readCoils response");
      return false;
    }

    // element 2 of response has the response data bytes expected
    if (response.at(2) != nRespDataBytes)
    {
      log_e("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
      return false;
    }

    // compute crc of current message
    if (!verifyCrc(response))
      return false;

    // extract coils data from data portion of response
    out.clear();
    out.reserve(nRespDataBytes);
    // get response data bytes excluding header and crc
    const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
    for (auto i(0); i < nRespDataBytes; i++)
    {
      reg_bytes_t val;
      val.hi = respData.at(i * sizeof(uint16_t));
      val.lo = respData.at(1 + i * sizeof(uint16_t));
      out.push_back(be16toh(val.reg16));
    }
  }

  // Func 0x05
  const bool MODBUS::writeCoil(const uint8_t device, const uint16_t coil, const bool value)
  {
  }

  // Func 0x06
  const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
  {
  }

  // Func 0x0F
  const bool MODBUS::writeCoils(const uint8_t device, const uint16_t reg, const std::vector<bool> &coils)
  {
  }

  // Func 0x10
  const bool MODBUS::writeRegisters(const uint8_t device, const uint16_t reg, const std::vector<uint16_t> &values)
  {
  }

  const std::vector<uint8_t> MODBUS::buildRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty)
  {
    req_t msg;
    msg.device = device;
    msg.func = func;
    msg.reg.reg16 = htobe16(reg);
    msg.qty.reg16 = htobe16(qty);

    m_crc.reset();
    m_crc.add((uint8_t *)&msg, sizeof(req_t));
    msg.crc = m_crc.getCRC();

    std::vector<uint8_t> data(sizeof(req_t), 0);
    std::memcpy(data.data(), &msg, sizeof(req_t));
    return data;
  }

  const bool MODBUS::verifyCrc(const std::vector<uint8_t> &data)
  {
    // compute crc of current message
    m_crc.reset();
    m_crc.add(data.data(), data.size());
    const uint16_t computedCrc(m_crc.getCRC());
    // extract crc from response
    const uint8_t crcHi(data.back());
    const uint8_t crcLo(data.back() - 1);

    // verify crc code
    if (highByte(computedCrc) != crcHi || lowByte(computedCrc) != crcLo)
    {
      log_e("Failed verify CRC code: comp[%04x], rec[%04x]", computedCrc, 0xFFFF && ((crcHi << 8) || crcLo));
      return false;
    }
    return true;
  }

}