#include "RS485_driver.h"
#include <algorithm>
#include <cstring>
#include <machine/endian.h>
#ifdef ESP32
#include <endian.h>
#else
#define be16toh(x) __bswap16(x)
#define htobe16(x) __bswap16(x)
#define htole16(x) x
#endif

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
};

namespace drivers
{

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

#ifdef ESP32
  RS485::RS485(const uint32_t baud, const SerialConfig conf)
  {
    LOG_INFO("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);
  }
#else

  RS485::RS485(const uint32_t baud)
  {
    LOG_INFO("Init serial port 1");
    m_serial = std::make_unique<HardwareSerial>(PORT); // RS485 is hardwired to serial port 1
    m_serial->begin(baud);
  }
#endif
  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();
    return true;
  }

  ////////////////////////////////
  //////////// MODBUS ////////////
  ////////////////////////////////
#ifdef ESP32
  MODBUS::MODBUS(const uint32_t baud, const SerialConfig conf) : RS485::RS485(baud, conf)
  {
    LOG_INFO("Init MODBUS Master Mode");
  }
#else
  MODBUS::MODBUS(const uint32_t baud) : RS485::RS485(baud)
  {
    LOG_INFO("Init MODBUS Master Mode");
  }
#endif
  // 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;
    LOG_DEBUG("Read coils: dev[%02x], reg[%04x], num[%d]", device, reg, num);
    return 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;
    LOG_DEBUG("Read multi inputs: dev[%02x], reg[%04x], num[%d]", device, reg, num);
    return readBinary(func, device, reg, num, inputs);
  }

  // Func 0x03
  const bool MODBUS::readHoldingRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
  {
    constexpr uint8_t func = 0x03;
    LOG_DEBUG("Read multi holding registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
    return readInteger(func, device, reg, num, values);
  }

  // Func 0x04
  const bool MODBUS::readInputRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
  {
    constexpr uint8_t func = 0x04;
    LOG_DEBUG("Read multi input registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
    return readInteger(func, device, reg, num, values);
  }

  // Func 0x05
  const bool MODBUS::writeCoil(const uint8_t device, const uint16_t coil, const bool value)
  {
    constexpr uint8_t func = 0x05;
    LOG_DEBUG("Write single coil: dev[%02x], reg[%04x], val[...]", device, coil);
    return writeBinary(device, func, coil, 1, {value});
  }

  // Func 0x06
  const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
  {
    constexpr uint8_t func = 0x06;
    LOG_DEBUG("Write single register: dev[%02x], reg[%04x], val[%04x]", device, reg, value);
    return writeInteger(device, func, reg, 1, {value});
  }

  // Func 0x0F
  const bool MODBUS::writeCoils(const uint8_t device, const uint16_t coils, const std::vector<bool> &values)
  {
    constexpr uint8_t func = 0x0F;
    LOG_DEBUG("Write multi coils: dev[%02x], reg[%04x], val[...]", device, coils);
    return writeBinary(device, func, coils, values.size(), values);
  }

  // Func 0x10
  const bool MODBUS::writeRegisters(const uint8_t device, const uint16_t reg, const std::vector<uint16_t> &values)
  {
    constexpr uint8_t func = 0x10;
    LOG_DEBUG("Write multi registers: dev[%02x], reg[%04x], val[...]", device, reg);
    return writeInteger(device, func, reg, values.size(), values);
  }

  /////////////////////////////////////////////////////////////////
  /////////////////////// Utility Functions ///////////////////////
  /////////////////////////////////////////////////////////////////

  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(singleRequest(device, func, reg, bits)))
    {
      LOG_ERROR("Failed send readBinary command");
      return false;
    }
    const uint16_t nRespDataBytes = 1 + (uint16_t)(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_ERROR("Failed receive readBinary response");
      return false;
    }

    // element 2 of response has the response data bytes expected
    if (response.at(2) != nRespDataBytes)
    {
      LOG_ERROR("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++;
      }
    }
    return true;
  }

  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(singleRequest(device, func, reg, num)))
    {
      LOG_ERROR("Failed send readInteger 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_ERROR("Failed receive readInteger response");
      return false;
    }

    // element 2 of response has the response data bytes expected
    if (response.at(2) != nRespDataBytes)
    {
      LOG_ERROR("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++)
    {
      const uint8_t hi(respData.at(i * sizeof(uint16_t)));
      const uint8_t lo(respData.at(1 + i * sizeof(uint16_t)));
      const uint16_t val(0xFFFF & ((hi << 8) | lo));
      out.push_back(be16toh(val));
    }
    return true;
  }

  const bool MODBUS::writeBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in)
  {
    std::vector<uint16_t> bitsOut;
    if (bits <= 1) // if single coil value must be 0x00FF[00] for on[off]
    {
      bitsOut.push_back(htobe16(in.front() ? 0x00FF : 0x0000));
    }
    else // if multiple coils value is 0x01 shifted for the number of coil intended
    {
      const uint16_t numRegisters((uint16_t)(bits / 16) + 1);
      bitsOut.resize(numRegisters, 0);
      for (uint16_t i(0); i < in.size(); i++)
      {
        if (!in[i]) // if value is false skip
          continue;
        const uint16_t curReg(i / 16);
        bitsOut[curReg] |= 0x01 << i % 16;
      }
    }

    if (!write(multiRequest(device, func, reg, bits, bitsOut)))
    {
      LOG_ERROR("Failed send writeBinary command");
      return false;
    }

    const uint16_t expectedRespLen(sizeof(resp_t));
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      LOG_ERROR("Failed receive writeBinary response");
      return false;
    }

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

    return true;
  }

  const bool MODBUS::writeInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, const std::vector<uint16_t> &in)
  {
    if (!write(multiRequest(device, func, reg, num, in)))
    {
      LOG_ERROR("Failed send writeInteger command");
      return false;
    }

    const uint16_t expectedRespLen(sizeof(resp_t));
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      LOG_ERROR("Failed receive writeInteger response");
      return false;
    }

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

    return true;
  }

  const std::vector<uint8_t> MODBUS::singleRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t data)
  {
    req_t header;
    header.device = device;
    header.func = func;
    header.reg = htobe16(reg);
    header.data = htobe16(data);

    const uint8_t headerBytes(sizeof(req_t));
    const uint8_t crcBytes(sizeof(crc_t));

    // compute crc for header + data
    m_crc.reset();
    m_crc.add((uint8_t *)&header, headerBytes); // exclude last two bytes of crc
    const uint16_t crc(htole16(m_crc.calc()));

    std::vector<uint8_t> dataOut(headerBytes + crcBytes, 0);
    std::memcpy(dataOut.data(), &header, headerBytes);
    std::memcpy(dataOut.data() + headerBytes, &crc, crcBytes);
    return dataOut;
  }

  const std::vector<uint8_t> MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint16_t> &data)
  {
    req_multi_t header;
    header.device = device;
    header.func = func;
    header.reg = htobe16(reg);
    header.qty = htobe16(qty);
    header.bytes = (uint8_t)(data.size() * sizeof(uint16_t)); // 8 bit value

    // convert uint16_t values from host endianness to big endian
    std::vector<uint16_t> dataBe;
    dataBe.reserve(data.size());
    std::for_each(data.begin(), data.end(), [&dataBe](auto v)
                  { dataBe.push_back(htobe16(v)); });

    const uint8_t headerBytes(sizeof(req_multi_t));
    const uint8_t dataBytes(sizeof(uint16_t) * dataBe.size());
    const uint8_t crcBytes(sizeof(crc_t));

    // compute crc for header + data
    m_crc.reset();
    m_crc.add((uint8_t *)&header, headerBytes); // add the request excluding the CRC code
    m_crc.add((uint8_t *)dataBe.data(), dataBytes);
    const uint16_t crc(htole16(m_crc.calc()));

    std::vector<uint8_t> dataOut;
    dataOut.resize(headerBytes + dataBytes + crcBytes);                    // header message + data values + crc code
    std::memcpy(dataOut.data(), &header, headerBytes);                     // copy message
    std::memcpy(dataOut.data() + headerBytes, dataBe.data(), dataBytes);   // copy data
    std::memcpy(dataOut.data() + headerBytes + dataBytes, &crc, crcBytes); // copy crc
    return dataOut;
  }

  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.calc());
    // extract crc from response
    const uint16_t size(data.size());
    const uint8_t crcLo(data.at(size - 2));
    const uint8_t crcHi(data.at(size - 1));

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

}