#include <Wire.h>
#include "hardware.h"

#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#include "menu.h"
#include "config.h"
#include "FilterOnePole.h" // for the breath signal low-pass filtering, from https://github.com/JonHub/Filters
FilterOnePole breathFilter;
FilterOnePole breathAltFilter;
FilterOnePole spikeFilter;
FilterOnePole tiltFilter;
FilterOnePole rollFilter;
FilterOnePole sliderFilterExtra;
FilterOnePole sliderFilterPB;
FilterOnePole sliderFilterLever;
float sliderMAExtra = 0.0;
float sliderMAPB = 0.0;
float sliderMALever = 0.0;

Adafruit_MPR121 touchSensorKeys = Adafruit_MPR121();
Adafruit_MPR121 touchSensorRoller = Adafruit_MPR121();
Adafruit_MPRLS pressureSensorMain = Adafruit_MPRLS();
Adafruit_MPRLS pressureSensorAlt = Adafruit_MPRLS();

byte drawingMemory[numLeds*3];         //  3 bytes per LED
DMAMEM byte displayMemory[numLeds*12]; // 12 bytes per LED
WS2812Serial ledStrip(numLeds, displayMemory, drawingMemory, ledStripPin, WS2812_GRB);
Adafruit_ICM20948 icmSensor;
Adafruit_Sensor *accelSensor;
Encoder knobs[] = {
  Encoder(e1aPin, e1bPin),
  Encoder(e2aPin, e2bPin),
  Encoder(e3aPin, e3bPin),
  Encoder(e4aPin, e4bPin),
};

void errorWait();


void initHardware() {
  MainI2CBus.setClock(1000000);
  AuxI2CBus.setClock(1000000);
  pinMode(statusLedPin,OUTPUT);    // Teensy onboard LED

  // Buttons
  pinMode(b1Pin,INPUT_PULLUP);
  pinMode(b2Pin,INPUT_PULLUP);
  pinMode(b3Pin,INPUT_PULLUP);
  pinMode(b4Pin,INPUT_PULLUP);

  breathFilter.setFilter(LOWPASS, FILTER_FREQ, 0.0); // create a one pole (RC) lowpass filter
  breathAltFilter.setFilter(LOWPASS, FILTER_FREQ, 0.0); // create a one pole (RC) lowpass filter
  spikeFilter.setFilter(HIGHPASS, SPIKE_FILTER_FREQ, 0.0); // create a one pole (RC) lowpass filter
  tiltFilter.setFilter(LOWPASS, ICM_FILTER_FREQ, 0.0); // create a one pole (RC) lowpass filter
  rollFilter.setFilter(LOWPASS, ICM_FILTER_FREQ, 0.0); // create a one pole (RC) lowpass filter
  icmSensor.begin_I2C(ICM20948_I2CADDR_DEFAULT, &MainI2CBus);

  ledStrip.begin();

  if (!touchSensorKeys.begin(KeysI2CAddr, &MainI2CBus)) {
    displayError("Keys touch error");
    errorWait();
  }

  if (!touchSensorRoller.begin(RollerI2CAddr, &MainI2CBus)) {
    displayError("Roller touch error");
    errorWait();
  }

  if (!pressureSensorMain.begin(MPRLS_DEFAULT_ADDR, &AuxI2CBus)) {
    displayError("Main pressure sensor error");
    errorWait();
  }

  if (!pressureSensorAlt.begin(MPRLS_DEFAULT_ADDR, &MainI2CBus)) {
    displayError("Alt pressure sensor error");
    errorWait();
  }


  if (!icmSensor.begin_I2C(ICM20948_I2CADDR_DEFAULT, &MainI2CBus)) {
    displayError("ICM sensor error");
    errorWait();
  }
}

void updateFilters(preset_t &preset) {
  breathFilter.setFrequency(preset.breathFilterFreq);
  breathAltFilter.setFrequency(preset.breathFilterFreq);
  spikeFilter.setFrequency(preset.spikeFilterFreq);
}

/*
  Return true if the given button bitmask is pressed
*/
bool checkButtonState(uint8_t mask) {
  return buttonState() & mask;
}

/**
 * Read the state of the switches (note that they are active low, so we invert the values)
*/
uint8_t buttonState() {
  return 0x0f ^(digitalRead(b1Pin)
    | (digitalRead(b2Pin) << 1)
    | (digitalRead(b3Pin) << 2)
    | (digitalRead(b4Pin) << 3));
}


void errorWait() {
  while (1) {
      if (!digitalRead(b1Pin)) {
        _reboot_Teensyduino_(); // reboot to program mode if b1 pressed
      }

      if (!digitalRead(b4Pin)) {
        break; // Continue if b4 pressed
      }

      delay(10);
  }
}

/*
 * Read the knob value accounting for 4x precision - NB this might not work on other kinds of encoder
*/
int readKnob(uint8_t n) {
  // Make it temporarily more sensitive when switching directions, the hardware loses a tick frequently
  static int lastOut = 0;
  static unsigned long lastOutTime = 0;
  int out = 0;
  int32_t val = knobs[n].read();
  if (val > ((lastOut < 0) ? 3 : 4)) {
    out = val / 4;
  } else if ((val < (lastOut > 0) ? -3 : -4)) {
    out = val / 4;
  }

  if (out != 0) {
    unsigned long time = millis();

    knobs[n].write(val - (out * 4));
    lastOut = out;

    if (time - lastOutTime < KNOB_CURVE_THRESHOLD) {
      out *= KNOB_CURVE_MULTIPLIER;
    }
    lastOutTime = time;
  }
  return out;
}

int readTouchKey(uint8_t n) {
  return CAP_SENS_ABSOLUTE_MAX - touchSensorKeys.filteredData(n);
}

int readTouchRoller(uint8_t n) {
  return CAP_SENS_ABSOLUTE_MAX - touchSensorRoller.filteredData(n);
}

uint16_t keysTouched() {
  return touchSensorKeys.touched();
}

int readSpikePressure() {
  return spikeFilter.output();
}

int readPressure() {
  float p = pressureSensorMain.readPressure();
  int r = breathFilter.input(p) * PRESSURE_SENS_MULTIPLIER;
  spikeFilter.input(r);
  return r;
}

int readAltPressure() {
  return breathAltFilter.input(pressureSensorAlt.readPressure()) * PRESSURE_SENS_MULTIPLIER;
}

int16_t readAnalog(Control id, int thr) {
  switch (id) {
    default:
      return 0;
      break;
  }
}

icm_result_t readICM() {
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t mag;
  sensors_event_t temp;
  icmSensor.getEvent(&accel, &gyro, &temp, &mag);

  return {
    tiltFilter.input(mag.magnetic.y),
    rollFilter.input(mag.magnetic.x),
  };
}

int readRawControl(Control id) {
  int a;
  switch (id) {
    case CONTROL_PB:
      a = analogRead(pbXPin);
      break;
    case CONTROL_PB_Y:
      a = analogRead(pbYPin);
      break;
    case CONTROL_EXTRA:
      a = readTouchRoller(extraPin);
      break;
    case CONTROL_BITE:
      a = analogRead(bitePin);
      break;
    case CONTROL_LEVER:
      a = analogRead(leverPin);
      break;
    case CONTROL_STICK_X:
      a = analogRead(stickXPin);
      break;
    case CONTROL_STICK_Y:
      a = analogRead(stickYPin);
      break;
    default:
      return 0.0;
      break;
  }

  return a;
}