// Implementations of the Arduino/Teensy stand-ins declared in shim/.
#include "SimHardware.h"
#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <RF24.h>
#include <SPI.h>
#include <EEPROM.h>
#include <HexapodProtocol.h>
#include <stdarg.h>
#include <deque>
#include <string>
#include <vector>

SimSerial Serial;
TwoWire Wire;
SPIClass SPI;
SimEEPROM EEPROM;

// =====================================================================
// Clock
// =====================================================================
static uint64_t g_now_us = 0;

namespace simhw {
void advanceMicros(uint32_t us) { g_now_us += us; }
uint64_t nowMicros() { return g_now_us; }
}  // namespace simhw

uint32_t micros() { return (uint32_t)g_now_us; }
uint32_t millis() { return (uint32_t)(g_now_us / 1000); }
// The robot loop never blocks; if it ever does, time just moves on.
void delay(uint32_t ms) { g_now_us += (uint64_t)ms * 1000; }
void delayMicroseconds(uint32_t us) { g_now_us += us; }

// =====================================================================
// Serial
// =====================================================================
static std::deque<char> g_serial_in;
static std::string g_serial_out;

namespace simhw {
void serialFeed(const char* bytes, int len) {
  for (int i = 0; i < len; i++) g_serial_in.push_back(bytes[i]);
}
int serialDrain(char* out, int cap) {
  int n = (int)g_serial_out.size() < cap ? (int)g_serial_out.size() : cap;
  memcpy(out, g_serial_out.data(), n);
  g_serial_out.erase(0, n);
  return n;
}
}  // namespace simhw

int SimSerial::available() { return (int)g_serial_in.size(); }
int SimSerial::read() {
  if (g_serial_in.empty()) return -1;
  char c = g_serial_in.front();
  g_serial_in.pop_front();
  return (uint8_t)c;
}
size_t SimSerial::write(uint8_t c) {
  if (g_serial_out.size() < 65536) g_serial_out.push_back((char)c);   // cap if nobody drains
  return 1;
}
size_t SimSerial::print(const char* s) {
  size_t n = 0;
  while (*s) n += write((uint8_t)*s++);
  return n;
}
size_t SimSerial::printf(const char* fmt, ...) {
  char buf[256];
  va_list ap;
  va_start(ap, fmt);
  vsnprintf(buf, sizeof(buf), fmt, ap);
  va_end(ap);
  return print(buf);
}

// =====================================================================
// Servos
// =====================================================================
static simhw::ServoPin g_pins[simhw::MAX_PINS];

namespace simhw {
const ServoPin& servoPin(int pin) {
  static const ServoPin none;
  return (pin >= 0 && pin < MAX_PINS) ? g_pins[pin] : none;
}
}  // namespace simhw

uint8_t Servo::attach(int pin) {
  if (pin < 0 || pin >= simhw::MAX_PINS) return 0;
  pin_ = pin;
  if (!g_pins[pin].attached) g_pins[pin].us = 1500;
  g_pins[pin].attached = true;
  return 1;
}
void Servo::detach() {
  if (pin_ >= 0) g_pins[pin_].attached = false;
  pin_ = -1;
}
void Servo::writeMicroseconds(int us) {
  if (pin_ < 0) return;
  g_pins[pin_].us = us;
  g_pins[pin_].writes++;
}

// ---- Hardware PWM (how ServoDriver drives the servos on the Teensy 3.6) ----
static uint32_t g_pwm_bits = 8;                 // Teensyduino default
static float g_pwm_hz[simhw::MAX_PINS];         // 0 = not set (Teensy default ~488 Hz)

void pinMode(uint8_t pin, uint8_t mode) {
  if (pin < simhw::MAX_PINS && mode == INPUT) g_pins[pin].attached = false;
}
void digitalWrite(uint8_t, uint8_t) {}
uint32_t analogWriteResolution(uint32_t bits) {
  uint32_t old = g_pwm_bits;
  g_pwm_bits = bits;
  return old;
}
void analogWriteFrequency(uint8_t pin, float hz) {
  if (pin < simhw::MAX_PINS) g_pwm_hz[pin] = hz;
}
// A duty of 0 means no pulses (a relaxed servo); otherwise the pulse width
// follows from duty, resolution and frequency, exactly as on the hardware.
void analogWrite(uint8_t pin, int value) {
  if (pin >= simhw::MAX_PINS) return;
  if (value <= 0) {
    g_pins[pin].attached = false;
    return;
  }
  float hz = g_pwm_hz[pin] > 0 ? g_pwm_hz[pin] : 488.28f;
  g_pins[pin].us = (int)lroundf((float)value / (1u << g_pwm_bits) * (1e6f / hz));
  g_pins[pin].attached = true;
  g_pins[pin].writes++;
}

// =====================================================================
// I2C + MPU6050 (register-level, address 0x68)
// =====================================================================
static constexpr int MPU_ADDR = 0x68;
static bool g_imu_present = true;
static uint8_t g_mpu_regs[128];
static uint8_t g_mpu_ptr = 0;
static int g_tx_addr = -1;
static std::vector<uint8_t> g_tx_bytes;
static std::deque<uint8_t> g_rx;

namespace simhw {
void imuSetPresent(bool present) { g_imu_present = present; }

static void putBE(uint8_t reg, float value, float scale) {
  float raw = value * scale;
  if (raw > 32767) raw = 32767;
  if (raw < -32768) raw = -32768;
  int16_t v = (int16_t)lroundf(raw);
  g_mpu_regs[reg]     = (uint8_t)((uint16_t)v >> 8);
  g_mpu_regs[reg + 1] = (uint8_t)(v & 0xFF);
}

void imuSet(float ax, float ay, float az, float gx, float gy, float gz) {
  // Default +/-2 g and +/-250 deg/s ranges, same scale factors RobotIMU uses.
  putBE(0x3B, ax, 16384.0f);
  putBE(0x3D, ay, 16384.0f);
  putBE(0x3F, az, 16384.0f);
  putBE(0x41, 0, 1);   // temperature, unused
  putBE(0x43, gx, 131.0f);
  putBE(0x45, gy, 131.0f);
  putBE(0x47, gz, 131.0f);
}
}  // namespace simhw

void TwoWire::beginTransmission(int address) {
  g_tx_addr = address;
  g_tx_bytes.clear();
}
size_t TwoWire::write(uint8_t b) {
  g_tx_bytes.push_back(b);
  return 1;
}
uint8_t TwoWire::endTransmission(bool) {
  if (g_tx_addr != MPU_ADDR || !g_imu_present) return 2;   // address NACK
  if (!g_tx_bytes.empty()) {
    g_mpu_ptr = g_tx_bytes[0] & 0x7F;                       // first byte = register pointer
    for (size_t i = 1; i < g_tx_bytes.size(); i++) g_mpu_regs[(g_mpu_ptr++) & 0x7F] = g_tx_bytes[i];
    if (g_tx_bytes.size() > 1) g_mpu_ptr = g_tx_bytes[0] & 0x7F;
  }
  return 0;
}
uint8_t TwoWire::requestFrom(int address, int quantity) {
  g_rx.clear();
  if (address != MPU_ADDR || !g_imu_present) return 0;
  for (int i = 0; i < quantity; i++) g_rx.push_back(g_mpu_regs[(g_mpu_ptr + i) & 0x7F]);
  return (uint8_t)quantity;
}
int TwoWire::available() { return (int)g_rx.size(); }
int TwoWire::read() {
  if (g_rx.empty()) return -1;
  uint8_t b = g_rx.front();
  g_rx.pop_front();
  return b;
}

// =====================================================================
// nRF24L01 "air"
// =====================================================================
struct AirPacket {
  uint8_t channel;
  rf24_datarate_e rate;
  uint8_t address[5];
  std::vector<uint8_t> payload;
};
static bool g_radio_present = true;
static std::deque<AirPacket> g_air;   // packets waiting at the receiver
static constexpr size_t RX_FIFO_DEPTH = 3;

namespace simhw {
void radioSetPresent(bool present) { g_radio_present = present; }
void radioSend(const uint8_t* bytes, int len) {
  if (len < 0 || len > 32) return;
  if (g_air.size() >= RX_FIFO_DEPTH) return;   // FIFO full: the real chip drops it too
  AirPacket p;
  p.channel = HXP_RADIO_CHANNEL;
  p.rate = (rf24_datarate_e)HXP_RADIO_DATA_RATE;
  memcpy(p.address, HXP_RADIO_ADDRESS, 5);
  p.payload.assign(bytes, bytes + len);
  g_air.push_back(p);
}
}  // namespace simhw

bool RF24::begin() { return g_radio_present; }
bool RF24::isChipConnected() { return g_radio_present; }
void RF24::openReadingPipe(uint8_t, const uint8_t* address) {
  memcpy(address_, address, 5);
  pipe_open_ = true;
}
bool RF24::available() {
  if (!g_radio_present || !listening_ || !pipe_open_) return false;
  // Drop anything this radio's configuration wouldn't have decoded.
  while (!g_air.empty()) {
    const AirPacket& p = g_air.front();
    bool match = p.channel == channel_ && p.rate == data_rate_ &&
                 memcmp(p.address, address_, 5) == 0 && p.payload.size() == payload_size_;
    if (match) return true;
    g_air.pop_front();
  }
  return false;
}
void RF24::read(void* buf, uint8_t len) {
  if (g_air.empty()) return;
  const AirPacket& p = g_air.front();
  memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size());
  g_air.pop_front();
}