#ifndef INSPIRE_H
#define INSPIRE_H

#include <eigen3/Eigen/Dense>
#include <vector>

#include "SerialPort.h"

namespace inspire
{

class InspireHand
{
public:
  InspireHand(SerialPort::SharedPtr serial = nullptr, id_t id = 0)
  : serial_(serial), id(id)
  {
    if(!serial)
      serial_ = std::make_shared<SerialPort>("/dev/ttyUSB0", B115200);

  }

  void ChangeID(uint8_t before, uint8_t now)
  {
    std::vector<uint8_t> cmd = {0xEB, 0x90, before, 0x04, 0x12, 0xE8, 0x03, now, 0x00};
    cmd.back() = CheckSum(cmd.data(), cmd.size());
    serial_->send(cmd.data(), cmd.size());

    usleep(5000);
    serial_->recv(recvBuff, 9);
  }


  /**
   * @brief Set the finger position
   * -1: no change
   * [0, 1] 0: close
   * ID: pinky, ring, middle, index, thumb_bend, thumb_rotate
   */
  int16_t SetPosition(const Eigen::Matrix<double, 6, 1> & q)
  {
    Eigen::Matrix<int16_t, 6, 1> q_int16;
    for (int i = 0; i < 6; i++)
    {
      // 0xFFFF = "no change" (per the docstring above): the hand IGNORES this joint and keeps
      // driving it to its previous target. It does NOT release the motor — an earlier comment
      // here claimed it did, and a hand-pose recording mode was built on that before the
      // operator found the finger still locked. There is no release command on the RH56, and
      // the drive is a non-backdrivable worm gear, so no force setting frees it either.
      if (q(i) < 0.0) q_int16(i) = -1;
      else { double v = q(i) * 1000.0; q_int16(i) = (int16_t)(v < 0 ? 0 : (v > 1000 ? 1000 : v)); }
    }

    uint8_t cmd[20];
    cmd[0] = 0xEB;    // head
    cmd[1] = 0x90;
    cmd[2] = id;      // ID
    cmd[3] = 0x0F;    // data length
    cmd[4] = 0x12;    // write
    cmd[5] = 0xCE;    // address
    cmd[6] = 0x05;

    cmd[7] = q_int16(0) & 0xFF;
    cmd[8] = (q_int16(0) >> 8) & 0xFF;
    cmd[9] = q_int16(1) & 0xFF;
    cmd[10] = (q_int16(1) >> 8) & 0xFF;
    cmd[11] = q_int16(2) & 0xFF;
    cmd[12] = (q_int16(2) >> 8) & 0xFF;
    cmd[13] = q_int16(3) & 0xFF;
    cmd[14] = (q_int16(3) >> 8) & 0xFF;
    cmd[15] = q_int16(4) & 0xFF;
    cmd[16] = (q_int16(4) >> 8) & 0xFF;
    cmd[17] = q_int16(5) & 0xFF;
    cmd[18] = (q_int16(5) >> 8) & 0xFF;

    cmd[19] = CheckSum(cmd, 20);

    serial_->send(cmd, 20);
    usleep(1500);   // was 5000 - recv() polls via select(), and reads flush() first
    serial_->recv(recvBuff, 9);
    return 0;
  }

  /**
   * @brief Get the finger position
   * 
   * ID: pinky, ring, middle, index, thumb_bend, thumb_rotate
   */
  int16_t GetPosition(Eigen::Matrix<double, 6, 1> & q)
  {
    const uint8_t *b = Query12(0x0A);   // angleAct
    if (!b) return 1;
    for (int i = 0; i < 6; i++)
      q(i) = (b[7 + 2 * i] | (b[8 + 2 * i] << 8)) / 1000.;
    return 0;
  }

  /**
   * @brief Set the finger velocity
   */
  void SetVelocity(int16_t v0, int16_t v1, int16_t v2, int16_t v3, int16_t v4, int16_t v5)
  {
    uint8_t cmd[20];
    cmd[0] = 0xEB;    // head
    cmd[1] = 0x90;
    cmd[2] = id;      // ID
    cmd[3] = 0x0F;    // data length
    cmd[4] = 0x12;    // write
    cmd[5] = 0xF2;    // address
    cmd[6] = 0x05;

    cmd[7] = v0 & 0xFF;
    cmd[8] = (v0 >> 8) & 0xFF;
    cmd[9] = v1 & 0xFF;
    cmd[10] = (v1 >> 8) & 0xFF;
    cmd[11] = v2 & 0xFF;
    cmd[12] = (v2 >> 8) & 0xFF;
    cmd[13] = v3 & 0xFF;
    cmd[14] = (v3 >> 8) & 0xFF;
    cmd[15] = v4 & 0xFF;
    cmd[16] = (v4 >> 8) & 0xFF;
    cmd[17] = v5 & 0xFF;
    cmd[18] = (v5 >> 8) & 0xFF;

    cmd[19] = CheckSum(cmd, 20);

    serial_->send(cmd, 20);
    usleep(1500);   // was 5000 - recv() polls via select(), and reads flush() first
    serial_->recv(recvBuff, 9);
  }

  /**
   * @brief Get the force control threshold
   * 
   * [0, 1000] Unit: g
   */
  void SetForce(uint16_t f0, uint16_t f1, uint16_t f2, uint16_t f3, uint16_t f4, uint16_t f5)
  {
    uint8_t cmd[20];
    cmd[0] = 0xEB;    // head
    cmd[1] = 0x90;
    cmd[2] = id;      // ID
    cmd[3] = 0x0F;    // data length
    cmd[4] = 0x12;    // write
    cmd[5] = 0xDA;    // address
    cmd[6] = 0x05;

    cmd[7] = f0 & 0xFF;
    cmd[8] = (f0 >> 8) & 0xFF;
    cmd[9] = f1 & 0xFF;
    cmd[10] = (f1 >> 8) & 0xFF;
    cmd[11] = f2 & 0xFF;
    cmd[12] = (f2 >> 8) & 0xFF;
    cmd[13] = f3 & 0xFF;
    cmd[14] = (f3 >> 8) & 0xFF;
    cmd[15] = f4 & 0xFF;
    cmd[16] = (f4 >> 8) & 0xFF;
    cmd[17] = f5 & 0xFF;
    cmd[18] = (f5 >> 8) & 0xFF;

    cmd[19] = CheckSum(cmd, 20);

    serial_->send(cmd, 20);
    usleep(1500);   // was 5000 - recv() polls via select(), and reads flush() first
    serial_->recv(recvBuff, 9);
  }

  /**
   * @brief Get the force of each finger
   * 
   * The force is in the unit of g, [0 - 1000], convert to N
   */
  int16_t GetForce(Eigen::Matrix<double, 6, 1> & f)
  {
    // Force sensor register 0x2E. Output = grams, SIGNED. Used for push-to-teach: a hand
    // push shows up as a force deviation even though the finger can't back-drive.
    const uint8_t *b = Query12(0x2E);
    if (!b) return 1;
    for (int i = 0; i < 6; i++)
      f(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
    return 0;
  }

  /**
   * @brief Read the per-finger ERROR register (1606).
   *
   * Bits: 0x01 locked rotor, 0x02 over-temperature, 0x04 over-current,
   *       0x08 abnormal operation, 0x10 communication error.
   *
   * The RH56 LATCHES these: once a finger stalls it refuses every subsequent motion
   * command — any target, any force limit, and a release (q<0) does not reset it — until
   * CLEAR_ERROR is sent. It keeps reporting a low force and a valid angle throughout, so
   * from the outside it just looks like a finger that stopped listening.
   */
  int16_t GetError(uint8_t out[6])
  {
    const uint8_t *b = QueryReg(1606, 6);
    if (!b) return 1;
    for (int i = 0; i < 6; i++) out[i] = b[7 + i];
    return 0;
  }

  /**
   * @brief Per-finger STATUS (1612). 0 unclenching, 1 grasping, 2 reached position,
   *        3 reached force, 5 current protection, 6 locked rotor, 7 fault.
   */
  int16_t GetStatus(uint8_t out[6])
  {
    const uint8_t *b = QueryReg(1612, 6);
    if (!b) return 1;
    for (int i = 0; i < 6; i++) out[i] = b[7 + i];
    return 0;
  }

  /// Per-finger motor temperature in degrees C (1618).
  int16_t GetTemp(uint8_t out[6])
  {
    const uint8_t *b = QueryReg(1618, 6);
    if (!b) return 1;
    for (int i = 0; i < 6; i++) out[i] = b[7 + i];
    return 0;
  }

  /**
   * @brief Per-finger motor CURRENT in mA (1594, 12 bytes / 6 int16).
   *
   * The single number that separates "dead" from "force-gated": a finger commanded to move
   * that draws 0 mA is not being driven at all, whereas one drawing current against a
   * mechanical block is trying and failing. Reading only angle and force cannot tell those
   * apart, which is how healthy fingers were once written off as dead actuators.
   */
  int16_t GetCurrent(Eigen::Matrix<double, 6, 1> &c)
  {
    const uint8_t *b = QueryReg(1594, 12);
    if (!b) return 1;
    for (int i = 0; i < 6; i++)
      c(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
    return 0;
  }

  /// Per-finger grip force LIMIT currently in the hand (FORCE_SET 1498). Shown next to the
  /// measured force so "force >= limit -> driver refuses to move" is visible, not deduced.
  int16_t GetForceSet(Eigen::Matrix<double, 6, 1> &f)
  {
    const uint8_t *b = QueryReg(1498, 12);
    if (!b) return 1;
    for (int i = 0; i < 6; i++)
      f(i) = (double)(int16_t)(b[7 + 2 * i] | (b[8 + 2 * i] << 8));
    return 0;
  }

  /**
   * @brief Clear error
   *
   * When the Inspire Hand has a fault such as a stall, overcurrent, or abnormality,
   * the fault can be cleared by the clear fault command.
   */
  void ClearError()
  {
    uint8_t cmd[9];
    cmd[0] = 0xEB;    // head
    cmd[1] = 0x90;
    cmd[2] = id;      // ID
    cmd[3] = 0x04;    // data length
    cmd[4] = 0x12;    // write
    cmd[5] = 0xEC;    // address
    cmd[6] = 0x03;
    cmd[7] = 0x01;
    cmd[8] = CheckSum(cmd, 9);
    
    serial_->send(cmd, 9);
    usleep(5000);
    serial_->recv(recvBuff, 9);
  }

  /**
   * @brief Force sensor calibration
   * 
   * @attention The Inspire Hand must be in an unloaded state during calibration
   */
  void Calibration()
  {
    std::vector<uint8_t> cmd = {0xEB, 0x90, id, 0x04, 0x12, 0x2F, 0x06, 0x01, 0x00};
    cmd.back() = CheckSum(cmd.data(), cmd.size());
    serial_->send(cmd.data(), cmd.size());

    usleep(5000);
    serial_->recv(recvBuff, 9); // First frame
    sleep(10); // The calibration process takes about 6 seconds
    serial_->recv(recvBuff, 9); // Second frame
  }

  uint8_t id = 1;
private:
  /**
   * @brief Send a 12-byte read query for `addr` and wait for its reply, POLLING the port
   *        rather than sleeping a fixed interval.
   *
   * The previous version slept a flat 8ms per attempt whether or not the reply had already
   * landed. With four of these per control cycle (2 hands x angle+force) that alone cost
   * ~32ms, which is why the 100Hz RecurrentThread actually published at a measured 28Hz.
   * select() inside recv() already returns the instant bytes arrive, so we accumulate until
   * a complete, checksum-valid frame is present or the budget expires — typically ~3ms.
   *
   * Still re-queries on failure: if the hand is briefly busy after a write it ignores a
   * single query and a one-shot read returns all-zeros. We only accept a reply whose cmd
   * (0x11) and address match, so a stray write-ack or echo can never be mis-parsed as 0.
   *
   * @return pointer to the 20-byte frame inside recvBuff, or nullptr if it never arrived.
   */
  const uint8_t* QueryReg(uint16_t addr, uint8_t nbytes, int budget_us = 12000)
  {
    uint8_t query[9] = {0xEB, 0x90, id, 0x04, 0x11,
                        (uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), nbytes, 0x00};
    query[8] = CheckSum(query, 9);
    const size_t frame = 7u + nbytes + 1u;   // header+addr + data + checksum

    for (int attempt = 0; attempt < 4; attempt++)
    {
      serial_->flush();
      serial_->send(query, 9);

      size_t n = 0;
      auto t0 = std::chrono::steady_clock::now();
      while (n + 1 < sizeof(recvBuff))
      {
        ssize_t got = serial_->recv(recvBuff + n, sizeof(recvBuff) - n);
        if (got > 0)
        {
          n += (size_t)got;
          for (size_t off = 0; off + frame <= n; off++)
          {
            if (!((recvBuff[off] == 0x90 && recvBuff[off + 1] == 0xEB) ||
                  (recvBuff[off] == 0xEB && recvBuff[off + 1] == 0x90))) continue;
            if (recvBuff[off + 4] != 0x11) continue;
            uint16_t got_addr = recvBuff[off + 5] | (recvBuff[off + 6] << 8);
            if (got_addr != addr) continue;
            if (recvBuff[off + frame - 1] != CheckSum(recvBuff + off, frame)) continue;
            return recvBuff + off;
          }
        }
        auto el = std::chrono::duration_cast<std::chrono::microseconds>(
                    std::chrono::steady_clock::now() - t0).count();
        if (el >= budget_us) break;
      }
    }
    return nullptr;
  }

  /// 12-byte reads (angleAct 0x060A, forceAct 0x062E) keep their short spelling.
  const uint8_t* Query12(uint8_t addrLo, int budget_us = 12000)
  {
    return QueryReg((uint16_t)(0x0600 | addrLo), 12, budget_us);
  }

  uint8_t CheckSum(const uint8_t* data, uint8_t len)
  {
    uint8_t sum = 0;
    for (int i = 2; i < len - 1; i++)
    {
      sum += data[i];
    }
    return sum;
  }

  SerialPort::SharedPtr serial_;
  uint8_t recvBuff[1024];
};

} // namespace inspire
#endif // INSPIRE_H