// HWT905-RS232 (WitMotion) IMU reader — Phase 1: pure observation, no control loop.
// Reads the sensor's active-push protocol (0x55-prefixed packets) over a plain
// serial port (RS232-over-USB, e.g. /dev/ttyUSB0) and prints accel/gyro/angle to
// stdout, optionally logging to CSV. Not Modbus — the RS485 variant uses Modbus,
// this RS232 variant does not.

#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <fcntl.h>
#include <fstream>
#include <iostream>
#include <string>
#include <termios.h>
#include <unistd.h>

namespace {

constexpr uint8_t kFrameHeader = 0x55;
constexpr uint8_t kTypeAccel = 0x51;
constexpr uint8_t kTypeGyro = 0x52;
constexpr uint8_t kTypeAngle = 0x53;
constexpr uint8_t kTypeMag = 0x54;

struct ImuState {
  double accel[3] = {0, 0, 0};  // g
  double gyro[3] = {0, 0, 0};   // deg/s
  double angle[3] = {0, 0, 0};  // deg (roll, pitch, yaw)
  double mag[3] = {0, 0, 0};    // raw counts
  bool has_accel = false, has_gyro = false, has_angle = false, has_mag = false;
};

int16_t toInt16(uint8_t lo, uint8_t hi) {
  return static_cast<int16_t>(static_cast<uint16_t>(lo) | (static_cast<uint16_t>(hi) << 8));
}

speed_t baudToSpeed(int baud) {
  switch (baud) {
    case 4800: return B4800;
    case 9600: return B9600;
    case 19200: return B19200;
    case 38400: return B38400;
    case 57600: return B57600;
    case 115200: return B115200;
    case 230400: return B230400;
    default:
      std::cerr << "[경고] 지원하지 않는 baud " << baud << ", 115200으로 대체\n";
      return B115200;
  }
}

int openSerialPort(const std::string &port, int baud) {
  int fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd < 0) {
    std::cerr << "[오류] 포트 열기 실패: " << port << " (" << std::strerror(errno) << ")\n";
    return -1;
  }
  fcntl(fd, F_SETFL, 0);  // switch back to blocking reads

  struct termios options;
  if (tcgetattr(fd, &options) != 0) {
    std::cerr << "[오류] tcgetattr 실패\n";
    close(fd);
    return -1;
  }

  speed_t speed = baudToSpeed(baud);
  cfsetispeed(&options, speed);
  cfsetospeed(&options, speed);

  options.c_cflag |= (CLOCAL | CREAD);
  options.c_cflag &= ~PARENB;
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;
  options.c_cflag &= ~CRTSCTS;

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

  options.c_cc[VMIN] = 1;
  options.c_cc[VTIME] = 5;  // 0.5s inter-byte timeout

  tcflush(fd, TCIFLUSH);
  if (tcsetattr(fd, TCSANOW, &options) != 0) {
    std::cerr << "[오류] tcsetattr 실패\n";
    close(fd);
    return -1;
  }
  return fd;
}

// Parses one validated 11-byte WitMotion frame (frame[0]==0x55, checksum ok)
// into the running ImuState. Returns true if this frame completed an
// accel+gyro+angle group worth printing (i.e. it was an angle frame).
bool applyFrame(const uint8_t *frame, ImuState &state) {
  const uint8_t type = frame[1];
  switch (type) {
    case kTypeAccel:
      for (int i = 0; i < 3; ++i) {
        int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]);
        state.accel[i] = raw / 32768.0 * 16.0;  // g
      }
      state.has_accel = true;
      return false;
    case kTypeGyro:
      for (int i = 0; i < 3; ++i) {
        int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]);
        state.gyro[i] = raw / 32768.0 * 2000.0;  // deg/s
      }
      state.has_gyro = true;
      return false;
    case kTypeAngle:
      for (int i = 0; i < 3; ++i) {
        int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]);
        state.angle[i] = raw / 32768.0 * 180.0;  // deg
      }
      state.has_angle = true;
      return true;
    case kTypeMag:
      for (int i = 0; i < 3; ++i) {
        int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]);
        state.mag[i] = raw;
      }
      state.has_mag = true;
      return false;
    default:
      return false;  // time/quaternion/GPS frames etc. — ignored in Phase 1
  }
}

}  // namespace

int main(int argc, char **argv) {
  std::setvbuf(stdout, nullptr, _IOLBF, 4096);  // line-buffer even when piped

  std::string port = "/dev/ttyUSB0";
  int baud = 9600;  // HWT905-232 factory default (confirmed against this unit)
  std::string log_path;

  for (int i = 1; i < argc; ++i) {
    std::string arg = argv[i];
    if (arg == "--port" && i + 1 < argc) {
      port = argv[++i];
    } else if (arg == "--baud" && i + 1 < argc) {
      baud = std::stoi(argv[++i]);
    } else if (arg == "--log" && i + 1 < argc) {
      log_path = argv[++i];
    } else if (arg == "--help") {
      std::cout << "사용법: " << argv[0]
                << " [--port /dev/ttyUSB0] [--baud 115200] [--log out.csv]\n";
      return 0;
    }
  }

  int fd = openSerialPort(port, baud);
  if (fd < 0) return 1;

  std::ofstream log_file;
  if (!log_path.empty()) {
    log_file.open(log_path);
    if (!log_file) {
      std::cerr << "[오류] 로그 파일 열기 실패: " << log_path << "\n";
      return 1;
    }
    log_file << "t_s,ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps,roll_deg,pitch_deg,yaw_deg\n";
  }

  std::cout << "포트 " << port << " @ " << baud << "bps 에서 IMU 데이터 수신 시작 "
            << "(Ctrl+C로 종료)\n";
  std::cout.flush();

  const auto t_start = std::chrono::steady_clock::now();
  ImuState state;
  uint8_t buf[11];
  size_t buf_len = 0;

  while (true) {
    uint8_t byte;
    ssize_t n = read(fd, &byte, 1);
    if (n <= 0) {
      if (n < 0 && errno != EAGAIN && errno != EINTR) {
        std::cerr << "[오류] 시리얼 읽기 실패: " << std::strerror(errno) << "\n";
        break;
      }
      continue;
    }

    if (buf_len == 0) {
      if (byte != kFrameHeader) continue;  // resync: wait for header
      buf[buf_len++] = byte;
      continue;
    }

    buf[buf_len++] = byte;
    if (buf_len < 11) continue;

    // Full 11-byte candidate frame collected — validate checksum.
    uint8_t sum = 0;
    for (int i = 0; i < 10; ++i) sum += buf[i];
    if (sum != buf[10]) {
      // Checksum mismatch: resync by sliding one byte and rescanning for 0x55.
      std::cerr << "[경고] 체크섬 불일치, 프레임 폐기\n";
      buf_len = 0;
      continue;
    }

    bool print_now = applyFrame(buf, state);
    buf_len = 0;

    if (print_now && state.has_accel && state.has_gyro && state.has_angle) {
      double t_s = std::chrono::duration<double>(std::chrono::steady_clock::now() - t_start).count();
      std::printf(
          "t=%7.3fs  accel[g]=(%+.3f,%+.3f,%+.3f)  gyro[dps]=(%+7.2f,%+7.2f,%+7.2f)  "
          "angle[deg]=(roll=%+7.2f,pitch=%+7.2f,yaw=%+7.2f)\n",
          t_s, state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1],
          state.gyro[2], state.angle[0], state.angle[1], state.angle[2]);

      if (log_file) {
        log_file << t_s << ',' << state.accel[0] << ',' << state.accel[1] << ','
                  << state.accel[2] << ',' << state.gyro[0] << ',' << state.gyro[1] << ','
                  << state.gyro[2] << ',' << state.angle[0] << ',' << state.angle[1] << ','
                  << state.angle[2] << '\n';
        log_file.flush();
      }
    }
  }

  close(fd);
  return 0;
}