calibration.cpp

#include "calibration.h"

#include <cstdlib>

namespace {

int roundToInt(float value) {
  return value >= 0.0f ? static_cast<int>(value + 0.5f)
                       : static_cast<int>(value - 0.5f);
}

int clamp(int value, int lo, int hi) {
  if (value < lo) return lo;
  if (value > hi) return hi;
  return value;
}

// Returns the option after/before `current` in `options`, wrapping
// around at either end. Falls back to the first entry if `current` isn't
// itself one of the options (e.g. a value loaded from NVS that predates
// an option-list change) — same convention as power_management.cpp's
// nextInList().
int nextInList(int current, const int *options, size_t count) {
  for (size_t i = 0; i < count; ++i) {
    if (options[i] == current) {
      return options[(i + 1) % count];
    }
  }
  return options[0];
}

int prevInList(int current, const int *options, size_t count) {
  for (size_t i = 0; i < count; ++i) {
    if (options[i] == current) {
      return options[(i + count - 1) % count];
    }
  }
  return options[0];
}

}  // namespace

int nextDeadzonePercent(int currentPercent) {
  return nextInList(currentPercent, StickDeadzoneOptions::kPercents,
                     StickDeadzoneOptions::kCount);
}

int prevDeadzonePercent(int currentPercent) {
  return prevInList(currentPercent, StickDeadzoneOptions::kPercents,
                    StickDeadzoneOptions::kCount);
}

int AnalogCalibration::calibrateTrigger(int raw, const CalibrationData &data) {
  if (data.triggerMax == data.triggerMin) {
    return 0;  // degenerate range
  }
  const float percent = static_cast<float>(raw - data.triggerMin) /
                        (data.triggerMax - data.triggerMin) * 100.0f;
  return clamp(roundToInt(percent), 0, 100);
}

int AnalogCalibration::calibrateStickAxis(int raw, int min, int center,
                                           int max, int deadzonePercent) {
  if (max <= center || center <= min) {
    return 0;  // uncalibrated/degenerate range
  }

  const float deadzoneHalfWidth =
      static_cast<float>(max - min) * deadzonePercent / 100.0f;
  if (std::abs(raw - center) <= deadzoneHalfWidth) {
    return 0;
  }

  if (raw > center) {
    const float percent =
        static_cast<float>(raw - center) / (max - center) * 100.0f;
    return clamp(roundToInt(percent), 0, 100);
  }

  const float percent =
      static_cast<float>(raw - center) / (center - min) * 100.0f;
  return clamp(roundToInt(percent), -100, 0);
}

void TriggerCalibrationFlow::confirmStep(int rawAdc) {
  switch (step_) {
    case Step::kAwaitingRelease:
      min_ = rawAdc;
      step_ = Step::kAwaitingFullPull;
      break;
    case Step::kAwaitingFullPull:
      max_ = rawAdc;
      step_ = Step::kDone;
      break;
    case Step::kDone:
      break;
  }
}

void StickCalibrationFlow::confirmCenter(int rawX, int rawY) {
  if (step_ != Step::kAwaitingCenter) {
    return;
  }
  centerX_ = rawX;
  centerY_ = rawY;
  minX_ = maxX_ = rawX;
  minY_ = maxY_ = rawY;
  step_ = Step::kRolling;
}

void StickCalibrationFlow::sample(int rawX, int rawY) {
  if (step_ != Step::kRolling) {
    return;
  }
  if (rawX < minX_) minX_ = rawX;
  if (rawX > maxX_) maxX_ = rawX;
  if (rawY < minY_) minY_ = rawY;
  if (rawY > maxY_) maxY_ = rawY;
}

int StickCalibrationFlow::sidesWithEnoughRange() const {
  return ((maxX_ - centerX_) >= kMinRangeCounts ? 1 : 0) +
         ((centerX_ - minX_) >= kMinRangeCounts ? 1 : 0) +
         ((maxY_ - centerY_) >= kMinRangeCounts ? 1 : 0) +
         ((centerY_ - minY_) >= kMinRangeCounts ? 1 : 0);
}

bool StickCalibrationFlow::hasEnoughRange() const {
  return sidesWithEnoughRange() == 4;
}

void StickCalibrationFlow::confirmDone() {
  if (step_ != Step::kRolling || !hasEnoughRange()) {
    return;
  }
  step_ = Step::kDone;
}