/*
== TODOS ==
Calibration Mode
 [x] (irrelevant) autocalibrate: Zero trip, run trip check, enter expected distance, confirm
 
Button check
 [x] general refactoring
 [x] decouple check and action
 [ ] add remote control

Misc
 [ ] switch Pilot screen mode (Reg/total/inter, ...)?
 [ ] TEST OUT I2C_LCD lib instead of LCD_I2C
*/

#include <EEPROM.h>
#include <Wire.h>

#include "TripData.h"

#include "Button.h"

// PINS
#define SIGNAL_IN 2
#define LED_REVERSE 6
#define LED_FREEZE 12
#define SW_CALIB A0

// LCD DISPLAY
#include "LcdDisplay.h"

#include "PilotScreen.h"
#include "TripMainScreen.h"
#include "CalibMainScreen.h"

#define LCD_ADDR 0x27
LcdDisplay  *lcd_main    = nullptr;
MainScreen  *mainScreen  = nullptr;

#define LCD_PILOT_ADDR 0x28
LcdDisplay  *lcd_pilot   = nullptr;
PilotScreen *pilotScreen = nullptr;


TripData trip;

#define OUTPUT_INTERVAL 100 //millis
unsigned long lastOutputTime;
/*
int speedAdjIndicator = 0;
bool speedAdjIndicatorFwd = true;
*/

// BUTTONS
//Button btnMod(A3);
//Button btnZeroTotal(4);
Button btnZeroInter(5, true);
Button btnReverse(11);
Button btnFreeze(13);
Button btnPlus(7, true);
Button btnMinus(8, true);
//Button btnUp(10);
//Button btnDown(9);

// MENU MODES
bool menuChanged;
enum menuMode {TRIP, CALIB} menu;

bool tripFreezeChange;
bool tripReverseChange;

long calibAdjustAmount = 1;




void setup() {
  Serial.begin(115200);
  initCalibrationFromEEPROM();

  pinMode(SIGNAL_IN, INPUT_PULLUP);
  pinMode(SW_CALIB, INPUT_PULLUP);
  pinMode(LED_FREEZE, OUTPUT);
  pinMode(LED_REVERSE, OUTPUT);
  
  btnZeroInter.onPress = zeroTripInter;
  btnZeroInter.onRelease = restartTrip;
  
  btnZeroInter.onPressMod = zeroTripInter;
  btnZeroInter.onLongPressMod = zeroTripTotal;
  btnZeroInter.onReleaseMod = restartTrip;

  btnFreeze.onPress = toggleFreeze;
  btnReverse.onPress = toggleReverse;
  
  
  lcd_main = new LcdDisplay(20,4,LCD_ADDR);
  lcd_main->begin();
  menu = TRIP;
  menuChanged = true;


  // Check if pilot display is connected
  Wire.beginTransmission(LCD_PILOT_ADDR);
  if (Wire.endTransmission() == 0) {
    lcd_pilot = new LcdDisplay(16,2,LCD_PILOT_ADDR);
    lcd_pilot->begin();
    pilotScreen = new PilotScreen(lcd_pilot);
    pilotScreen->init(&trip);
  }
  
  cli();
  attachInterrupt(0,isr_signal,RISING);
  sei();
}

void mainTripButtonInit() {
  // TRIP MODE
  btnPlus.onPress  = totalDistancePlus;
  btnPlus.onHold   = totalDistancePlus;
  btnPlus.onPressMod   = calibFastUp;

  btnMinus.onPress = totalDistanceMinus;
  btnMinus.onHold  = totalDistanceMinus;
  btnMinus.onPressMod = calibFastDown;
}

void mainCalibButtonInit() {
  // CALIB MODE
  btnPlus.onPress   = calibValueUp;
  btnPlus.onHold    = calibValueUp;
  btnPlus.onPressMod  = calibCursorLeft;

  btnMinus.onPress = calibValueDown;
  btnMinus.onHold  = calibValueDown;
  btnMinus.onPressMod = calibCursorRight;
}


void loop() {
  // debug: Why so slow updates, when buttons are pressed?
  for (Button *btn : Button::allButtons)
    if (btn)
      btn->check();

  checkSwitchCalib();
  
  if (menuChanged) {
    delete mainScreen;

    switch (menu) {
      case TRIP:
      mainScreen = new TripMainScreen(lcd_main);
      mainTripButtonInit();
      break;

      case CALIB:
      trip.calibCursor = 0;
      mainScreen = new CalibMainScreen(lcd_main);
      mainCalibButtonInit();
    }

    mainScreen->init(&trip);
    menuChanged = false;
  }

  bool triggerOutput = (millis() - lastOutputTime > OUTPUT_INTERVAL);
  if(triggerOutput){
    //trip.calculateSpeed();
    mainScreen->update();
    if (pilotScreen) pilotScreen->update();

    char debugBuffer[48];


    lastOutputTime = millis();
  }
}

void isr_signal() {  
  unsigned long signalMicros = micros();
  trip.addSignal(signalMicros);
}

void initCalibrationFromEEPROM() {
  TripData::Calibration calib;

  EEPROM.get(0,calib);
  // unwritten EEPROM has all 0xFF values
  // good initial value for a fresh system:
  // 100.00 mm/c => value: 100 - prescale: 1
  
  trip.calibration.prescale = (calib.prescale >> 3) ? 3 : calib.prescale;
  trip.calibration.value = (calib.value >> 19) ? 193387 : calib.value;
  trip.updateCalibration();
}

void updateCalibrationToEEPROM() {
  EEPROM.put(0,trip.calibration);
}

void checkSwitchCalib() {
  bool in = digitalRead(SW_CALIB);
  if (menu == TRIP && in == LOW) {
    menu = CALIB;
    menuChanged = true;
  } else if (menu == CALIB && in == HIGH) {
    menu = TRIP;
    menuChanged = true;
    updateCalibrationToEEPROM();
  }
}

/*
 === BUTTON CALLBACK FUNCTIONS ===
*/
void zeroTripTotal() {
  trip.stop[0] = true;
  trip.zero(0);
}

void zeroTripInter() {
  trip.stop[1] = true;
  trip.zero(1);
}

void restartTrip() {
  trip.stop[0] = false;
  trip.stop[1] = false;
}

void toggleFreeze() {
  trip.freeze = !trip.freeze;
  digitalWrite(LED_FREEZE,trip.freeze);
}

void toggleReverse() {
  trip.reverse = !trip.reverse;
  digitalWrite(LED_REVERSE,trip.reverse);
}

void totalDistancePlus() {
  trip.changeDistance(0,10);
}
void totalDistanceMinus() {
  trip.changeDistance(0,-10);
}

void calibFastUp() {
  trip.calibration.value *= 1.001f;
  trip.updateCalibration();
}

void calibFastDown(){
  trip.calibration.value *= 0.999f;
  trip.updateCalibration();
}

void calibValueUp() {
  if (calibAdjustAmount == 0)
  {
    trip.calibration.prescale++;

    if (trip.calibration.prescale == 4)
      trip.calibration.prescale = 0;
    
  }
  else
  {
    trip.calibration.value += calibAdjustAmount;
    if (trip.calibration.value >= 1000000)
      trip.calibration.value -= calibAdjustAmount;
  }

  trip.updateCalibration();
}


void calibValueDown() {
  if (calibAdjustAmount == 0) 
  {
    if (trip.calibration.prescale == 0)
      trip.calibration.prescale = 4;

    trip.calibration.prescale--;
  }
  else 
  {
    trip.calibration.value -= calibAdjustAmount;
    if (trip.calibration.value < 1)
      trip.calibration.value += calibAdjustAmount;
  }

  trip.updateCalibration();
}


void calibCursorLeft() {
  trip.calibCursor++;

  if (trip.calibCursor == TripData::calibCursorMax)
    trip.calibCursor = 0;

  updateCalibAdjustAmount(trip.calibCursor);
}

void calibCursorRight() {
  if (trip.calibCursor == 0)
    trip.calibCursor = TripData::calibCursorMax;
  
  trip.calibCursor--;
  
  updateCalibAdjustAmount(trip.calibCursor);
}

void updateCalibAdjustAmount(int pow) {
  if (pow == TripData::calibCursorMax - 1) {
    calibAdjustAmount = 0;
    return;
  }

  calibAdjustAmount = 1;
  while (pow) {
    calibAdjustAmount *= 10;
    pow--;
  }
}