#include "app/tools/obb_dual_gimbal.h"

#include <Arduino.h>
#include <Wire.h>

namespace obb::tools::gimbal {

namespace {
ControllerState s_state;
controller_callback_t s_callback = nullptr;
unsigned long s_last_poll_ms = 0;
constexpr uint8_t kI2CAddr = 0x28;  // RP2040 helper (tiny USB HID bridge)

bool read_stick_from_i2c(uint8_t channel, StickState &stick) {
  Wire.beginTransmission(kI2CAddr);
  Wire.write(channel);
  if (Wire.endTransmission(false) != 0) {
    return false;
  }
  if (Wire.requestFrom(kI2CAddr, static_cast<uint8_t>(5)) != 5) {
    return false;
  }
  stick.x = ([Wire.read](<http://Wire.read>)() << 8) | [Wire.read](<http://Wire.read>)();
  stick.y = ([Wire.read](<http://Wire.read>)() << 8) | [Wire.read](<http://Wire.read>)();
  [stick.press](<http://stick.press>) = [Wire.read](<http://Wire.read>)();
  return true;
}

bool read_keypad_mask(uint32_t &mask) {
  Wire.beginTransmission(kI2CAddr);
  Wire.write(0x80);  // keypad register
  if (Wire.endTransmission(false) != 0) {
    return false;
  }
  if (Wire.requestFrom(kI2CAddr, static_cast<uint8_t>(3)) != 3) {
    return false;
  }
  mask = [Wire.read](<http://Wire.read>)();
  mask |= [Wire.read](<http://Wire.read>)() << 8;
  mask |= [Wire.read](<http://Wire.read>)() << 16;
  return true;
}

void notify() {
  if (s_callback) {
    s_callback(s_state);
  }
}

}  // namespace

void init() {
  Wire.begin();
  s_state = ControllerState{};
  s_last_poll_ms = 0;
}

void poll() {
  const auto now = millis();
  if (now - s_last_poll_ms < 15) {
    return;  // ~60 Hz max
  }
  s_last_poll_ms = now;

  StickState left;
  StickState right;
  uint32_t keypad = 0;

  const bool left_ok = read_stick_from_i2c(0x00, left);
  const bool right_ok = read_stick_from_i2c(0x10, right);
  const bool keypad_ok = read_keypad_mask(keypad);

  const bool online = left_ok || right_ok || keypad_ok;
  s_state.rp2040_online = online;

  if (left_ok) {
    s_state.left = left;
  }
  if (right_ok) {
    s_state.right = right;
  }
  if (keypad_ok) {
    s_state.keypad_mask = keypad;
  }

  notify();
}

void set_callback(controller_callback_t cb) {
  s_callback = std::move(cb);
}

const ControllerState &last_state() {
  return s_state;
}

}  // namespace obb::tools::gimbal