#include "cpp_lowcmd_bridge/motor_crc_hg.hpp"

#include <algorithm>
#include <cstddef>
#include <cstring>

namespace {

static_assert(sizeof(MotorCmd) == 28, "unexpected H2 MotorCmd CRC layout");
static_assert(sizeof(LowCmd) == 1004, "unexpected H2 LowCmd CRC layout");

struct ImuStateCrcLayout {
  std::array<float, 4> quaternion;
  std::array<float, 3> gyroscope;
  std::array<float, 3> accelerometer;
  std::array<float, 3> rpy;
  int16_t temperature;
};

struct MotorStateCrcLayout {
  uint8_t mode;
  float q;
  float dq;
  float ddq;
  float tau_est;
  std::array<int16_t, 2> temperature;
  float vol;
  std::array<uint32_t, 2> sensor;
  uint32_t motorstate;
  std::array<uint32_t, 4> reserve;
};

struct LowStateCrcLayout {
  std::array<uint32_t, 2> version;
  uint8_t mode_pr;
  uint8_t mode_machine;
  uint32_t tick;
  ImuStateCrcLayout imu_state;
  std::array<MotorStateCrcLayout, 35> motor_state;
  std::array<uint8_t, 40> wireless_remote;
  std::array<uint32_t, 4> reserve;
  uint32_t crc;
};

static_assert(sizeof(ImuStateCrcLayout) == 56, "unexpected H2 IMU CRC layout");
static_assert(sizeof(MotorStateCrcLayout) == 56, "unexpected H2 motor-state CRC layout");
static_assert(sizeof(LowStateCrcLayout) == 2092, "unexpected H2 LowState CRC layout");
static_assert(offsetof(LowStateCrcLayout, crc) == 2088, "unexpected H2 LowState CRC offset");

}  // namespace

void get_crc(unitree_hg::msg::LowCmd &msg) {
  LowCmd raw{};

  raw.modePr = msg.mode_pr;
  raw.modeMachine = msg.mode_machine;

  for (int i = 0; i < 35; i++) {
    raw.motorCmd[i].mode = msg.motor_cmd[i].mode;
    raw.motorCmd[i].q = msg.motor_cmd[i].q;
    raw.motorCmd[i].dq = msg.motor_cmd[i].dq;
    raw.motorCmd[i].tau = msg.motor_cmd[i].tau;
    raw.motorCmd[i].Kp = msg.motor_cmd[i].kp;
    raw.motorCmd[i].Kd = msg.motor_cmd[i].kd;

    raw.motorCmd[i].reserve = msg.motor_cmd[i].reserve;
  }

  std::copy(msg.reserve.begin(), msg.reserve.end(), raw.reserve.begin());

  std::array<uint32_t, sizeof(LowCmd) / sizeof(uint32_t)> words{};
  std::memcpy(words.data(), &raw, sizeof(raw));
  raw.crc = crc32_core(words.data(), static_cast<uint32_t>(words.size() - 1));
  msg.crc = raw.crc;
}

bool low_state_crc_valid(const unitree_hg::msg::LowState &msg) {
  LowStateCrcLayout raw{};
  raw.version = msg.version;
  raw.mode_pr = msg.mode_pr;
  raw.mode_machine = msg.mode_machine;
  raw.tick = msg.tick;
  raw.imu_state.quaternion = msg.imu_state.quaternion;
  raw.imu_state.gyroscope = msg.imu_state.gyroscope;
  raw.imu_state.accelerometer = msg.imu_state.accelerometer;
  raw.imu_state.rpy = msg.imu_state.rpy;
  raw.imu_state.temperature = msg.imu_state.temperature;
  for (size_t index = 0; index < raw.motor_state.size(); ++index) {
    const auto &source = msg.motor_state[index];
    auto &target = raw.motor_state[index];
    target.mode = source.mode;
    target.q = source.q;
    target.dq = source.dq;
    target.ddq = source.ddq;
    target.tau_est = source.tau_est;
    target.temperature = source.temperature;
    target.vol = source.vol;
    target.sensor = source.sensor;
    target.motorstate = source.motorstate;
    target.reserve = source.reserve;
  }
  raw.wireless_remote = msg.wireless_remote;
  raw.reserve = msg.reserve;
  raw.crc = msg.crc;
  std::array<uint32_t, sizeof(LowStateCrcLayout) / sizeof(uint32_t)> words{};
  std::memcpy(words.data(), &raw, sizeof(raw));
  const uint32_t expected = crc32_core(
      words.data(), static_cast<uint32_t>(words.size() - 1));
  return msg.crc == expected;
}

uint32_t crc32_core(const uint32_t *ptr, uint32_t len) {
  uint32_t xbit = 0;
  uint32_t data = 0;
  uint32_t CRC32 = 0xFFFFFFFF;
  const uint32_t dwPolynomial = 0x04c11db7;
  for (uint32_t i = 0; i < len; i++) {
    xbit = 1 << 31;
    data = ptr[i];
    for (uint32_t bits = 0; bits < 32; bits++) {
      if (CRC32 & 0x80000000) {
        CRC32 <<= 1;
        CRC32 ^= dwPolynomial;
      } else
        CRC32 <<= 1;
      if (data & xbit) CRC32 ^= dwPolynomial;

      xbit >>= 1;
    }
  }
  return CRC32;
}