#include "Gear.h"

static const int8_t GEAR_MAP[3][3] = {
  { 5, 3, 1 },
  { -2, 0, -2 },
  { -1, 4, 2 },
};

GearSelector::GearSelector(uint8_t xPin, uint8_t yPin, int threshLow, int threshHigh)
  : _xPin(xPin), _yPin(yPin),
    _threshLow(threshLow), _threshHigh(threshHigh),
    _currentGear(0) {}

bool GearSelector::update() {
  AxisPos x = _quantize(analogRead(_xPin));
  AxisPos y = _quantize(analogRead(_yPin));

  int newGear = _mapToGear(x, y);

  if (newGear == -2) return false;

  if (newGear != _currentGear) {
    delay(100);
    _currentGear = newGear;
    return true;
  }
  return false;
}

int GearSelector::getGear() const {
  return _currentGear;
}

char GearSelector::getGearChar() const {
  if (_currentGear == -1) return 'R';
  return '0' + _currentGear;
}

AxisPos GearSelector::_quantize(int val) const {
  if (val < _threshLow) return POS_LOW;
  if (val > _threshHigh) return POS_HIGH;
  return POS_CENTRE;
}

int GearSelector::_mapToGear(AxisPos x, AxisPos y) const {
  int xi = (int)x + 1;
  int yi = (int)y + 1;
  return GEAR_MAP[xi][yi];
}