#include "fori_serial_bridge_cpp/modbus_serial.hpp"

#include <chrono>
#include <cstring>
#include <fcntl.h>
#include <termios.h>
#include <thread>
#include <unistd.h>

namespace fori_serial_bridge_cpp {

uint16_t calcCRC16(const uint8_t *data, size_t len) {
  uint16_t crc = 0xFFFF;
  for (size_t i = 0; i < len; ++i) {
    crc ^= data[i];
    for (int j = 0; j < 8; ++j) {
      if (crc & 0x0001) {
        crc >>= 1;
        crc ^= 0xA001;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

SerialPort::~SerialPort() { closePort(); }

bool SerialPort::openPort(const std::string &port_name, int baudrate) {
  fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd_ < 0) {
    return false;
  }

  fcntl(fd_, F_SETFL, 0);

  struct termios options;
  tcgetattr(fd_, &options);

  speed_t speed = B115200;
  switch (baudrate) {
  case 9600:
    speed = B9600;
    break;
  case 19200:
    speed = B19200;
    break;
  case 38400:
    speed = B38400;
    break;
  case 57600:
    speed = B57600;
    break;
  default:
    speed = B115200;
    break;
  }

  cfsetispeed(&options, speed);
  cfsetospeed(&options, speed);

  options.c_cflag &= ~PARENB; // No parity
  options.c_cflag &= ~CSTOPB; // 1 stop bit
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;      // 8 data bits
  options.c_cflag &= ~CRTSCTS; // No hardware flow control
  options.c_cflag |= CREAD | CLOCAL;

  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
                        ISTRIP | INLCR | IGNCR | ICRNL);
  options.c_oflag &= ~OPOST;

  options.c_cc[VMIN] = 0;
  options.c_cc[VTIME] = 1; // 0.1s timeout

  tcsetattr(fd_, TCSANOW, &options);
  tcflush(fd_, TCIOFLUSH);
  return true;
}

void SerialPort::closePort() {
  if (fd_ >= 0) {
    close(fd_);
    fd_ = -1;
  }
}

bool SerialPort::writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
  uint8_t pkt[8];
  pkt[0] = slave;
  pkt[1] = 0x06;
  pkt[2] = (reg >> 8) & 0xFF;
  pkt[3] = reg & 0xFF;
  pkt[4] = (val >> 8) & 0xFF;
  pkt[5] = val & 0xFF;

  uint16_t crc = calcCRC16(pkt, 6);
  pkt[6] = crc & 0xFF;
  pkt[7] = (crc >> 8) & 0xFF;

  tcflush(fd_, TCIOFLUSH);
  ssize_t written = write(fd_, pkt, 8);
  if (written != 8)
    return false;

  if (slave == 0)
    return true; // Broadcast: driver sends no response.

  uint8_t rx_buf[8];
  ssize_t rx_bytes = 0;
  auto start = std::chrono::steady_clock::now();
  while (rx_bytes < 8) {
    ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
    if (res > 0)
      rx_bytes += res;
    auto now = std::chrono::steady_clock::now();
    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50)
      break;
    std::this_thread::sleep_for(std::chrono::microseconds(500));
  }
  if (rx_bytes != 8)
    return false;

  uint16_t resp_crc = calcCRC16(rx_buf, 6);
  uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(rx_buf[7]) << 8);
  return resp_crc == recv_crc && rx_buf[0] == slave;
}

bool SerialPort::writeRegs(uint8_t slave, uint16_t reg, const std::vector<uint16_t> &vals) {
  size_t count = vals.size();
  size_t pkt_len = 7 + count * 2 + 2;
  std::vector<uint8_t> pkt(pkt_len);

  pkt[0] = slave;
  pkt[1] = 0x10;
  pkt[2] = (reg >> 8) & 0xFF;
  pkt[3] = reg & 0xFF;
  pkt[4] = (count >> 8) & 0xFF;
  pkt[5] = count & 0xFF;
  pkt[6] = static_cast<uint8_t>(count * 2);

  for (size_t i = 0; i < count; ++i) {
    pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
    pkt[8 + i * 2] = vals[i] & 0xFF;
  }

  uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
  pkt[pkt_len - 2] = crc & 0xFF;
  pkt[pkt_len - 1] = (crc >> 8) & 0xFF;

  tcflush(fd_, TCIOFLUSH);
  ssize_t written = write(fd_, pkt.data(), pkt_len);
  if (written != static_cast<ssize_t>(pkt_len))
    return false;

  if (slave == 0)
    return true;

  uint8_t rx_buf[8];
  ssize_t rx_bytes = 0;
  auto start = std::chrono::steady_clock::now();
  while (rx_bytes < 8) {
    ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
    if (res > 0)
      rx_bytes += res;
    auto now = std::chrono::steady_clock::now();
    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50)
      break;
    std::this_thread::sleep_for(std::chrono::microseconds(500));
  }
  if (rx_bytes != 8)
    return false;

  uint16_t resp_crc = calcCRC16(rx_buf, 6);
  uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(rx_buf[7]) << 8);
  return resp_crc == recv_crc && rx_buf[0] == slave;
}

bool SerialPort::readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t> &out_vals) {
  uint8_t pkt[8];
  pkt[0] = slave;
  pkt[1] = 0x03;
  pkt[2] = (reg >> 8) & 0xFF;
  pkt[3] = reg & 0xFF;
  pkt[4] = (count >> 8) & 0xFF;
  pkt[5] = count & 0xFF;

  uint16_t crc = calcCRC16(pkt, 6);
  pkt[6] = crc & 0xFF;
  pkt[7] = (crc >> 8) & 0xFF;

  tcflush(fd_, TCIOFLUSH);
  if (write(fd_, pkt, 8) != 8)
    return false;

  size_t expected_bytes = 5 + count * 2;
  std::vector<uint8_t> rx_buf(expected_bytes);
  size_t rx_bytes = 0;

  auto start = std::chrono::steady_clock::now();
  while (rx_bytes < expected_bytes) {
    ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
    if (res > 0)
      rx_bytes += res;
    auto now = std::chrono::steady_clock::now();
    // 100Hz(10ms) 루프 안에서도 응답을 안정적으로 받도록 25ms까지 허용.
    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 25)
      break;
    std::this_thread::sleep_for(std::chrono::microseconds(100));
  }

  if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
    uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2);
    uint16_t recv_crc = static_cast<uint16_t>(rx_buf[expected_bytes - 2]) |
                         (static_cast<uint16_t>(rx_buf[expected_bytes - 1]) << 8);
    if (resp_crc != recv_crc)
      return false;

    out_vals.resize(count);
    for (uint16_t i = 0; i < count; ++i) {
      out_vals[i] = (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
    }
    return true;
  }
  return false;
}

} // namespace fori_serial_bridge_cpp
