#include "Sticks.h"
#include <EEPROM.h>

// EEPROM layout: one record at address 0. Bump CAL_VERSION if AxisCal or
// the axis order ever changes so stale data is ignored instead of misread.
static constexpr uint32_t CAL_MAGIC   = 0x48585043;   // "HXPC"
static constexpr uint16_t CAL_VERSION = 1;
static constexpr int      CAL_EEPROM_ADDR = 0;

struct StoredCal {
  uint32_t magic;
  uint16_t version;
  AxisCal axes[NUM_AXES];
  uint16_t checksum;
};

static uint16_t checksumOf(const StoredCal& s) {
  const uint8_t* p = reinterpret_cast<const uint8_t*>(&s);
  uint16_t sum = 0;
  for (size_t i = 0; i < offsetof(StoredCal, checksum); i++) sum = (sum << 1 | sum >> 15) ^ p[i];
  return sum;
}

bool StickFrame::allCentered(int16_t tolerance) const {
  for (int i = 0; i < NUM_AXES; i++)
    if (abs(v[i]) > tolerance) return false;
  return true;
}

bool Sticks::begin() {
  analogReadResolution(ADC_BITS);
  analogReadAveraging(ADC_AVERAGING);

  if (!loadFromEeprom()) applyDefaults();

  bool all_ok = true;
  for (int i = 0; i < NUM_AXES; i++) {
    int16_t rest = readAveraged(i, 32);
    filtered_[i] = rest;
    if (abs(rest - cal_[i].center) <= CENTER_CAPTURE_TOLERANCE) {
      cal_[i].center = rest;
    } else {
      all_ok = false;   // someone's holding a stick, or the stored cal is off - keep stored center
    }
  }
  return all_ok;
}

int16_t Sticks::readAveraged(int axis, int samples) {
  int32_t sum = 0;
  for (int n = 0; n < samples; n++) sum += analogRead(STICK_PINS[axis]);
  return (int16_t)(sum / samples);
}

// Raw counts -> -1..1 (piecewise around center, so asymmetric pots still
// reach full scale both ways) -> invert -> deadzone with rescale -> expo.
int16_t Sticks::mapAxis(int axis, float raw) const {
  const AxisCal& c = cal_[axis];
  float n;
  if (raw >= c.center) n = (raw - c.center) / (float)max(1, c.max - c.center);
  else                 n = (raw - c.center) / (float)max(1, c.center - c.min);
  n = constrain(n, -1.0f, 1.0f);
  if (STICK_INVERT[axis]) n = -n;

  float dz = STICK_DEADZONE[axis];
  float mag = fabsf(n);
  if (mag <= dz) return 0;
  mag = (mag - dz) / (1.0f - dz);

  float e = STICK_EXPO[axis];
  mag = (1.0f - e) * mag + e * mag * mag * mag;

  return (int16_t)lroundf(copysignf(mag, n) * 1000.0f);
}

void Sticks::update() {
  for (int i = 0; i < NUM_AXES; i++) {
    float r = analogRead(STICK_PINS[i]);
    filtered_[i] += STICK_FILTER_ALPHA * (r - filtered_[i]);

    if (calibrating_) {
      int16_t v = (int16_t)filtered_[i];
      sweep_[i].min = min(sweep_[i].min, v);
      sweep_[i].max = max(sweep_[i].max, v);
      frame_.v[i] = 0;   // never command motion while the operator is sweeping sticks
    } else {
      frame_.v[i] = mapAxis(i, filtered_[i]);
    }
  }
}

void Sticks::startCalibration() {
  for (int i = 0; i < NUM_AXES; i++) {
    int16_t v = (int16_t)filtered_[i];
    sweep_[i] = { v, v, v };
  }
  calibrating_ = true;
}

bool Sticks::saveCalibration() {
  if (!calibrating_) return false;
  AxisCal next[NUM_AXES];
  for (int i = 0; i < NUM_AXES; i++) {
    // Sticks are released when SAVE is pressed, so the current reading is center.
    int16_t center = (int16_t)filtered_[i];
    if (center - sweep_[i].min < CAL_MIN_HALF_SPAN || sweep_[i].max - center < CAL_MIN_HALF_SPAN)
      return false;   // stay in calibration mode so the operator can keep sweeping
    // Pull the ends in slightly so full deflection reliably reaches 1000.
    int16_t inset_lo = (center - sweep_[i].min) / 50;
    int16_t inset_hi = (sweep_[i].max - center) / 50;
    next[i] = { (int16_t)(sweep_[i].min + inset_lo), center, (int16_t)(sweep_[i].max - inset_hi) };
  }
  memcpy(cal_, next, sizeof(cal_));
  saveToEeprom();
  calibrating_ = false;
  return true;
}

void Sticks::cancelCalibration() { calibrating_ = false; }

void Sticks::applyDefaults() {
  for (int i = 0; i < NUM_AXES; i++)
    cal_[i] = { STICK_DEFAULT_MIN, STICK_DEFAULT_CENTER, STICK_DEFAULT_MAX };
}

void Sticks::resetCalibrationToDefaults() {
  applyDefaults();
  StoredCal blank{};
  EEPROM.put(CAL_EEPROM_ADDR, blank);   // magic = 0 -> ignored on next boot
}

bool Sticks::loadFromEeprom() {
  StoredCal s;
  EEPROM.get(CAL_EEPROM_ADDR, s);
  if (s.magic != CAL_MAGIC || s.version != CAL_VERSION || s.checksum != checksumOf(s)) return false;
  memcpy(cal_, s.axes, sizeof(cal_));
  return true;
}

void Sticks::saveToEeprom() {
  StoredCal s{};
  s.magic = CAL_MAGIC;
  s.version = CAL_VERSION;
  memcpy(s.axes, cal_, sizeof(cal_));
  s.checksum = checksumOf(s);
  EEPROM.put(CAL_EEPROM_ADDR, s);
}

void Sticks::printCalibration(Stream& s) const {
  static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TWIST", "R_X", "R_Y", "R_TWIST" };
  s.println("// stick calibration {min, center, max}:");
  for (int i = 0; i < NUM_AXES; i++) {
    s.printf("//   %-8s { %4d, %4d, %4d }\n", names[i], cal_[i].min, cal_[i].center, cal_[i].max);
  }
}