/*
== TODOS ==
Calibration Mode
 [x] total/trip/speed calculation has overflow issues
 [X] persist calibration value
 [x] manually edit calibration value
 [x] edit calibration value prescaler
 [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
 [x] switch Pilot screen mode (Reg/total/inter, ...)

== IDEAS ==
Regularity Features
 [ ] separate device => connection? (-> distance, <- ±ideal time);
 [ ] enter/edit stage time, length, avg speed (numpad?)
 [ ] checkpoint/avg correction (enter/preselect next km, confirm on click)
 [ ] dedicated screen (See LCDPilotOutputUpdate() )
 [ ] save/load stage data from/to SD-Card (several csv files)

*/

#include <EEPROM.h>
#include <Wire.h>
#include <LCD_I2C.h>
#include "TripData.h"
#include "MainLCD.h"
#include "Button.h"

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

// LCD DISPLAY
#define LCD_ADDR 0x27
#define LCD_PILOT_ADDR 0x28

LCD_I2C lcd(LCD_ADDR,20,4);
LCD_I2C lcd_pilot(LCD_PILOT_ADDR, 16, 2);

bool lcdPilotConnected = false;
int lcdPilotMode = 1; // 1 = Inter, 0 = Total 

MainTripLCD mainTripLCD(&lcd);
MainCalibLCD mainCalibLCD(&lcd);
PilotTripLCD pilotTripLCD(&lcd_pilot);

TripData trip;

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

// BUTTONS
Button btnMod(A3);
Button btnZeroInter(5, &btnMod);
Button btnReverse(11);
Button btnFreeze(13, &btnMod);
Button btnPlus(7, &btnMod);
Button btnMinus(8, &btnMod);

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

bool tripFreezeChange;
bool tripReverseChange;

long calibAdjustAmount = 1;

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

  initCalibrationFromEEPROM();

  pinMode(SIGNAL_IN, INPUT_PULLUP);
  cli();
  attachInterrupt(0,isr_signal,RISING);
  sei();

  pinMode(SW_CALIB, INPUT_PULLUP);

  pinMode(LED_FREEZE, OUTPUT);
  pinMode(LED_REVERSE, OUTPUT);
  
  btnZeroInter.onPushed = resetTripInter;
  btnZeroInter.onPushedMod = resetTripTotal;
  btnFreeze.onPress = toggleFreeze;
  btnFreeze.onPressMod = togglePilotMode;


  // Check if pilot display is connected
  Wire.beginTransmission(LCD_PILOT_ADDR);
  lcdPilotConnected = (Wire.endTransmission() == 0);
  
  lcd.begin();
  lcd.backlight();
  
  if (lcdPilotConnected) {
    lcd_pilot.begin();
    lcd_pilot.backlight();
    pilotTripLCD.init();
  }
  menu = TRIP;
}

void mainTripButtonInit() {
  // TRIP MODE
  btnReverse.onPress = toggleReverse;

  btnPlus.onPress  = totalDistancePlus;
  btnPlus.onHold   = totalDistancePlus;
  btnMinus.onPress = totalDistanceMinus;
  btnMinus.onHold  = totalDistanceMinus;

  btnPlus.onPressMod  = calibFastUp;
  btnMinus.onPressMod = calibFastDown;

  //btnFreeze.onHoldMod = nullptr;
}

void mainCalibButtonInit() {
  // CALIB MODE
  btnReverse.onPress = changePrescale;

  btnPlus.onPress   = calibValueUp;
  btnPlus.onHold    = calibValueUp;
  btnMinus.onPress = calibValueDown;
  btnMinus.onHold  = calibValueDown;

  btnPlus.onPressMod  = calibCursorLeft;
  btnMinus.onPressMod = calibCursorRight;

  //btnFreeze.onHoldMod = showFirmwareVersion;
}


void loop() {
  for (Button* btn : Button::allButtons) if (btn) btn->check();

  checkSwitchCalib();
  bool triggerOutput = (millis() - lastOutputTime > OUTPUT_INTERVAL);
  
  if(triggerOutput){
    trip.calculate();
    if (lcdPilotConnected) pilotTripLCD.update(trip.distance[lcdPilotMode], trip.speed);
    lastOutputTime = millis();
  }

  switch (menu) {
    case TRIP:
      if (menuChanged) {
        mainTripLCD.init();
        mainTripButtonInit();
        menuChanged = false;
      }

      mainTripLCD.flagFreeze(trip.freeze);
      mainTripLCD.flagReverse(trip.reverse);
      
      if(triggerOutput){
        //serialOutput();   
        mainTripLCD.update(trip.distance[0], trip.distance[1], trip.speed);
        
        /*
        if (speedAdjIndicatorFwd) {
          speedAdjIndicator++;
        } else {
          speedAdjIndicator--;
        } 
        if (speedAdjIndicator > 7 || speedAdjIndicator < -7) speedAdjIndicatorFwd ^= true;
        */
      }
      break;
    case CALIB:
      if (menuChanged) {
        mainCalibLCD.init();
        mainCalibButtonInit();
        menuChanged = false;
      }

      mainCalibLCD.flagFreeze(trip.freeze);
      
      if(triggerOutput) {
        mainCalibLCD.update(trip.calibrationValue, trip.calibrationPrescale, trip.distance[1]);
      }
      break;
    default:
      break;
  }

}

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

void initCalibrationFromEEPROM() {
  long calibPrescale;
  long calibValue;

  EEPROM.get(0,calibPrescale);
  EEPROM.get(4,calibValue);

  trip.calibrationPrescale = calibPrescale == -1 ? 10000 : calibPrescale;
  trip.calibrationValue = calibValue == -1 ? 193400 : calibValue;
}

void updateCalibrationToEEPROM() {
  EEPROM.put(0,trip.calibrationPrescale);
  EEPROM.put(4,trip.calibrationValue);
}

void serialOutput() {
  Serial.print("T: ");
  Serial.print(trip.distance[0]);
  Serial.print(" - I: ");
  Serial.print(trip.distance[1]);
  Serial.print(" - V: ");
  Serial.print(trip.speed,1);
  Serial.print(" (");
  Serial.print(trip.speed,3);
  Serial.print(")");
  Serial.println();
} 

void checkSwitchCalib() {
  bool in = digitalRead(SW_CALIB);
  if (menu == TRIP && in == LOW) {
    Serial.println("Chg menu to CALIB");
    menu = CALIB;
    menuChanged = true;
  } else if (menu == CALIB && in == HIGH) {
    Serial.println("Chg menu to TRIP");
    menu = TRIP;
    menuChanged = true;
    updateCalibrationToEEPROM();
  }
}

/*
 === BUTTON CALLBACK FUNCTIONS ===
*/
void resetTripTotal() {
  trip.reset(0);
}

void resetTripInter() {
  trip.reset(1);
}

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.calibrationValue *= 1.001f;
}

void calibFastDown(){
  trip.calibrationValue *= 0.999f;
}

void changePrescale() {
  trip.calibrationPrescale *= 10;
  if (trip.calibrationPrescale > 10000) trip.calibrationPrescale =1;
}

void calibValueUp() {
  trip.calibrationValue += calibAdjustAmount;
  if (trip.calibrationValue >= 1000000) {
    Serial.print("Calibration+ overflow: ");
    Serial.println(trip.calibrationValue);
    trip.calibrationValue -= calibAdjustAmount;
  };
}
void calibValueDown() {
  trip.calibrationValue -= calibAdjustAmount;
  if (trip.calibrationValue < 1) {
    Serial.print("Calibration- underflow: ");
    Serial.println(trip.calibrationValue);
    trip.calibrationValue += calibAdjustAmount;
  };
}
void calibCursorLeft() {
  int power = mainCalibLCD.moveCursorLeft();
  updateCalibAdjustAmount(power);
}
void calibCursorRight() {
  int power = mainCalibLCD.moveCursorRight();
  updateCalibAdjustAmount(power);
}

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

void togglePilotMode() {
  ++lcdPilotMode %= 2;
}

void showFirmwareVersion() {
  
}