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

//#define DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
#include "utils.h"

namespace drivers
{

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

  RS485::RS485(const uint32_t baud, const SerialConfig conf) : m_serial(Serial1)
  {
    LOG_INFO("Init serial port 1");
    // RS485 is hardwired to serial port 1
    m_serial.begin(baud, conf, 18, 17);
    m_serial.flush();
  }

  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());
    if (avail == 0)
      return true;
    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)
  {
    std::vector<uint8_t> buf;
    buf.resize(nBytes);
    if (m_serial.readBytes(buf.data(), nBytes) == nBytes)
    {
      data = std::move(buf);
      return true;
    }
    return false;
  }

  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 ////////////
  ////////////////////////////////
  MODBUS::MODBUS(const uint32_t baud, const SerialConfig conf) : RS485::RS485(baud, conf)
  {
    std::vector<uint8_t> garbage;
    readAll(garbage);
    LOG_INFO("Init MODBUS Master Mode");
    m_crc.reset(CRC16_MODBUS_POLYNOME, CRC16_MODBUS_INITIAL, CRC16_MODBUS_XOR_OUT, CRC16_MODBUS_REV_IN, CRC16_MAXIM_REV_OUT);
  }

  // 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[", device, "], reg[", reg, "], num[", num, "]");
    return readBinary(device, func, 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 = 0x02;
    LOG_DEBUG("Read multi inputs: dev[", device, "], reg[", reg, "], num[", num, "]");
    return readBinary(device, func, 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[", device, "], reg[", reg, "], num[", num, "]");
    return readInteger(device, func, 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[", device, "], reg[", reg, "], num[", num, "]");
    return readInteger(device, func, 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[", device, "], coil[", coil, "], value[", value ? "true" : "false", "]");
    return writeBinary(device, func, coil, {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[", device, "], reg[", reg, "], value[", value, "]");
    return writeInteger(device, func, reg, {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[", device, "], start[", coils, "], num[", values.size(), "]");
    return writeBinary(device, func, coils, 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[", device, "], start[", reg, "], num[", values.size(), "]");
    return writeInteger(device, func, reg, values);
  }

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

  const bool MODBUS::readBinary(const uint8_t device, const uint8_t func, 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 = (uint16_t)ceil(bits / 8.0f);                          // 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, expected[", expectedRespLen, "], received[", response.size(), "]");
      return false;
    }

    // element 2 of response has the response data bytes expected
    const uint8_t actualRespLen(response.at(2));
    if (actualRespLen != nRespDataBytes)
    {
      LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", nRespDataBytes, "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
      printBytes("readBinary Response", response);
#endif
      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() - sizeof(crc_t));
    for (auto it = respData.begin(); it < respData.end(); it++)
    {
      for (uint8_t j(0); j < 8 && bitNum < bits; j++)
      {
        const bool cv((0x01 << j) & *it);
        out.push_back(cv);
        bitNum++;
      }
    }
    return true;
  }

  const bool MODBUS::readInteger(const uint8_t device, const uint8_t func, 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 + sizeof(crc_t)) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      LOG_ERROR("Failed receive readInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
      printBytes("readInteger Response", response);
#endif
      return false;
    }

    // element 2 of response has the response data bytes expected
    const uint8_t actualRespLen(response.at(2));
    if (actualRespLen != nRespDataBytes)
    {
      LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", 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 / sizeof(uint16_t));
    // 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 uint8_t lo(*it++);
      const uint8_t hi(*it);
      const uint16_t val(0xFFFF & ((hi << 8) | lo));
      out.push_back(be16toh(val));
    }
    return true;
  }

  const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<bool> &in)
  {
    const uint16_t bits(in.size());
    std::vector<uint8_t> bitsOut;
    if (bits == 1) // if single coil value must be 0x00FF[00] for on[off]
    {
      if (!write(singleRequest(device, func, reg, in.front() ? 0xFF00 : 0x0000)))
      {
        LOG_ERROR("Failed send writeSingleBinary command");
        return false;
      }
    }
    else // if multiple coils value is 0x01 shifted for the number of coil intended
    {
      const uint16_t numBytes((uint16_t)ceil(bits / 8.0f));
      bitsOut.resize(numBytes, 0x00);
      for (uint16_t i(0); i < in.size(); i++)
      {
        if (!in[i]) // if value is false skip
          continue;
        bitsOut[i / 8] |= 0x01 << i % 8;
      }
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
      LOG_DEBUG("\nnumBytes", numBytes);
      printBool("bitsOut", in);
      printBytes("bitsOut", bitsOut);
#endif
      if (!write(multiRequest(device, func, reg, bits, bitsOut)))
      {
        LOG_ERROR("Failed send writeMultiBinary command");
        return false;
      }
    }

    const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      LOG_ERROR("Failed receive writeBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
      printBytes("writeBinary Response", response);
#endif
      return false;
    }

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

    return true;
  }

  const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<uint16_t> &in)
  {
    const uint16_t num(in.size());
    if (num == 1)
    {
      if (!write(singleRequest(device, func, reg, in[0])))
      {
        LOG_ERROR("Failed send writeSingleInteger command");
        return false;
      }
    }
    else
    {
      // build data vector for request, inverting bytes if necessary
      std::vector<uint8_t> requestData;
      requestData.resize(in.size() * sizeof(uint16_t));
      auto it=requestData.begin();
      std::for_each(in.begin(), in.end(), [requestData, &it](auto inV) {
        const uint16_t beV(htobe16(inV));
        *it=highByte(beV);
        *(++it)=lowByte(beV);
      });

      if (!write(multiRequest(device, func, reg, num, requestData)))
      {
        LOG_ERROR("Failed send writeMultiInteger command");
        return false;
      }
    }

    const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
    std::vector<uint8_t> response;
    if (!readN(expectedRespLen, response))
    {
      LOG_ERROR("Failed receive writeInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
      printBytes("writeInteger Response", response);
#endif
      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.restart();
    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);

#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
    printBytes("singleRequest", dataOut);
#endif
    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<uint8_t> &data)
  {
    req_multi_t header;
    header.device = device;
    header.func = func;
    header.reg = htobe16(reg);
    header.qty = htobe16(qty);
    header.bytes = data.size(); // 8 bit value

    //const uint8_t headerBytes(sizeof(req_multi_t)); // sizeof not working because of memory padding
    const uint8_t headerBytes(7);
    const uint8_t dataBytes(data.size());
    const uint8_t crcBytes(sizeof(crc_t));

    // compute crc for header + data
    m_crc.restart();
    m_crc.add((uint8_t *)&header, headerBytes); // add the request excluding the CRC code
    m_crc.add((uint8_t *)data.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, data.data(), dataBytes);   // copy data
    std::memcpy(dataOut.data() + headerBytes + dataBytes, &crc, crcBytes); // copy crc

#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
    printBytes("multiRequest", dataOut);
#endif
    return dataOut;
  }

  const bool MODBUS::verifyCrc(const std::vector<uint8_t> &data)
  {
    // compute crc of current message
    m_crc.restart();
    m_crc.add(data.data(), data.size() - sizeof(crc_t));
    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));
    const uint16_t receivedCrc(0xFFFF & ((crcHi << 8) | crcLo));

    // verify crc code
    if (highByte(computedCrc) != crcHi || lowByte(computedCrc) != crcLo)
    {
      LOG_ERROR("Failed verify CRC code: comp[", computedCrc, "], rec[", receivedCrc, "]");
      return false;
    }
    return true;
  }

}