#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;

Adafruit_MPR121 touchSensorKeys = Adafruit_MPR121();
Adafruit_MPR121 touchSensorUtil = 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

  ledStrip.begin();

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

  if (!touchSensorUtil.begin(UtilI2CAddr, &MainI2CBus)) {
    displayError("Roller/Util touch error");
    errorWait();
  }

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

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

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

/*
  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) {
  int out = 0;
  int32_t val = knobs[n].read();
  if (val > 4) {
    out = val / 4;
  } else if (val < -4) {
    out = val / 4;
  }

  if (out != 0) {
    knobs[n].write(0);
  }
  return out;
}

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

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

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

int readPressure() {
  return breathFilter.input(pressureSensorMain.readPressure()) * PRESSURE_SENS_MULTIPLIER;
}

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