//#define ST7789
//#define DEBUG_INFO
//#include <Arduino.h>
#include <Ewma.h>
#include <PulseSensorPlayground.h>     // Includes the PulseSensorPlayground Library.   
//#include <SPI.h>
//#include <Adafruit_ST7789.h>
#ifdef ST7789
  #include <Arduino_ST7789_Fast.h>  //https://github.com/cbm80amiga/Arduino_ST7789_Fast
#else
  #include <Adafruit_ILI9341.h>
#endif

#include <Adafruit_GFX.h>
#include <EEPROM.h>
#include <RREFont.h>
//#include <SPI.h>
//#include <Wire.h>
//#include "rre_arialb_16.h"
#include "rre_8x12.h"

#define USE_ARDUINO_INTERRUPTS true    // Set-up low-level interrupts for most acurate BPM math.

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

#define TFT_DC 9
#define TFT_CS 7
#define TFT_RST   8
#define TFT_LED   10

const int OUTPUT_TYPE = SERIAL_PLOTTER;

#define LEFT 3
#define CALB 4
#define RIGHT 5

#define NUM_BUTTONS 3
#define NUM_SENSORS 2

#define BAUD_RATE 230400

#define RELEASED 0x1
#define PRESSED  0x0

static const uint8_t numReadings=20;
static const uint8_t SCREEN_SIZE = 240;
static const uint8_t SCREEN_MAX = SCREEN_SIZE-1;
static const uint8_t NUM_CALIB=SCREEN_SIZE-2;
static const uint8_t MID_VAL=NUM_CALIB/2;
static const uint8_t MAX_GRAPH=MID_VAL-1;
static const uint8_t PZOOM_LIMIT[] = {MID_VAL/4, MID_VAL * 3/4, MAX_GRAPH};
uint8_t settingsOffset = 2;
static const uint8_t calibrationOffset = 50;
uint8_t Settings[3] = {4, 2, 60};

// display coordinates
static const uint8_t X_BASE = 5;
static const uint8_t X_DAMP = 152;
static const uint8_t X_ZOOM = 130;
static const uint8_t X_RPM = 92;
static const uint8_t X_VALUE = 92;
static const uint8_t X_CALIB = 170;
static const uint8_t X_CALIBER = 124;

static const uint8_t Y_ZOOM = 95;
static const uint8_t Y_RPM = 130;
static const uint8_t Y_DAMP = 165;
static const uint8_t Y_CALIB = 170;
static const uint8_t Y_BOT = 224;

static const uint8_t refresh_graph = 100;
static const uint16_t refresh_zoom1 = 500;
static const uint16_t refresh_zoom2 = 250;
static const uint16_t refresh_num = 500;

uint16_t CALIB_MIN = Settings[2] * 10;
uint16_t CRETE = CALIB_MIN + 256;

static const uint8_t versionUID = 31; //update when settings_t changes!

static const uint16_t fade_time = 800;
static const uint8_t fade_steps = 100;
static const uint8_t fade_delay = fade_time / fade_steps;

static const uint8_t SENSOR_PINS[NUM_SENSORS] = { A0, A1 }; //used as a means to access the sensor pins using a counter
static const uint8_t SWITCH_PINS [] = {3, 4, 5};

unsigned long last_Update_graph = 0;
unsigned long last_Update_num = 0;

uint16_t RPM = 0;                                       //stores the current RPM
uint16_t RPM_PREV = 0;                                       //stores the current RPM
uint16_t AVERAGE[NUM_SENSORS]; //used to share the current average for each sensor
int16_t Pdelta = 0;
//int Pdelta_prev = 0;
uint16_t MIN_VAL[NUM_SENSORS] = {CRETE,CRETE}; //used to share the current average for each sensor
float Pfactor = 1.0;
float Rfactor = 1.0;

int16_t Pzoomed = 0;
int16_t Pzoomed_prev = 0;
uint8_t Pzoom = 1;
uint8_t Pzoom_prev = 1;
static const uint8_t Pzoom_min = 1;
static const uint8_t Pzoom_max = 16.0;
uint16_t Pzoomed_abs = 0;
bool Pzoom_change[] = {false,false,false};
unsigned long last_Pzoom = 0;
unsigned long Pzoom_timer[] = {0,0,0};
int8_t SENSOR_CALIB[NUM_CALIB];
bool calib_lvl1_exit;
bool calib_lvl2_exit;
uint16_t readingSensor = 0;
uint16_t readingStandard = 0;

byte buttonState[NUM_BUTTONS] = { HIGH, HIGH, HIGH }; //array for recording the state of buttons
byte buttonCount[NUM_BUTTONS] = { 0, 0, 0 }; //array for recording the state of buttons
byte lastButtonState[NUM_BUTTONS] = { HIGH, HIGH, HIGH }; //array for recording the previous state of buttons
unsigned long debounceTimes[NUM_BUTTONS]; //array for recording when the button press was first seen
unsigned long lastEntry = 0;
static const uint8_t debounceDelay = 200; //allow 200ms for switches to settle before they register

//extern settings_t settings;

PulseSensorPlayground pulseSensor(2);
Ewma Pfilter(Pfactor);   // Less smoothing - faster to detect changes, but more prone to noise
Ewma Rfilter(Rfactor);  // More smoothing - less prone to noise, but slower to detect changes

#ifdef ST7789
#else
  #define BLACK   0x0000
  #define BLUE    0x001F
  #define RED     0xF800
  #define GREEN   0x07E0
  #define CYAN    0x07FF
  #define MAGENTA 0xF81F
  #define YELLOW  0xFFE0
  #define WHITE   0xFFFF
  #define GREY    0x9999
#endif

//Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);
#ifdef ST7789
  Arduino_ST7789 tft = Arduino_ST7789(TFT_DC, TFT_RST);
#else
  Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
#endif
//SSD1283A tft(/*CS=10*/ SS, /*DC=*/ TFT_DC, /*RST=*/ TFT_RST, /*LED=*/ TFT_LED); //hardware spi,cs,cd,reset,led

RREFont font;
// needed for RREFont library initialization, define your fillRect
void customRect(int x, int y, int w, int h, int c) {
  return tft.fillRect(x, y, w, h, c);}

void setup()
{

  #ifdef DEBUG_INFO
    Serial.begin(250000);
    Serial.println("DEBUG MODE ON");
    //Serial.println(PZOOM_LIMIT[1]);
    //Serial.println(PZOOM_LIMIT[2]);
    //Serial.println(PZOOM_LIMIT[3]);
    //Serial.println(MID_VAL);
  #endif

  actionReadSettings();
  Pfactor = 1 / Settings[0];
  Rfactor = 1 / Settings[1];
  Pfilter.alpha = Pfactor;
  Rfilter.alpha = Rfactor;
  
  readCalibrationFromEEPROM();

  for (uint8_t i = 0; i < NUM_SENSORS; i++)
  {
    pulseSensor.analogInput(SENSOR_PINS[i], i);
    pulseSensor.setThreshold(CRETE, i);
  }
//  pulseSensor.setSerial(Serial);
//  pulseSensor.setOutputType(OUTPUT_TYPE);

  if (!pulseSensor.begin()) {
    /*
       PulseSensor initialization failed,
       likely because our Arduino platform interrupts
       aren't supported yet.

       If your Sketch hangs here, try changing USE_ARDUINO_INTERRUPTS to false.
    */
    for (;;) {
      // Flash the led to show things didn't work.
      digitalWrite(LED_BUILTIN, LOW);
      delay(50);
      digitalWrite(LED_BUILTIN, HIGH);
      delay(50);
    }
  }

  sbi(ADCSRA, ADPS2);
  cbi(ADCSRA, ADPS1);
  cbi(ADCSRA, ADPS0);

  
  for (int i = 0; i < NUM_BUTTONS; i++) pinMode(i + LEFT, INPUT);
  for (int i = 0; i < NUM_SENSORS; i++) pinMode(i + A0, INPUT);

  #ifdef ST7789
    tft.init(SCREEN_SIZE, SCREEN_SIZE);
    tft.setRotation(0);
    tft.fillScreen(BLACK);
  #else
    tft.begin();
    tft.fillScreen(BLACK);
    tft.drawRect(0, 0, SCREEN_SIZE, SCREEN_SIZE, WHITE);
  #endif

  font.init(customRect, SCREEN_SIZE, SCREEN_SIZE);
  analogWrite(TFT_LED, 0);

  font.setFont(&rre_8x12);
  font.setScale(3);
  font.setSpacing(1);
  font.setColor(YELLOW, BLACK);
  font.printStr(20, 80,(char*)"BoxerBal");
  font.setScale(1);
  font.printStr(213, 98,(char*)"c");
  tft.drawCircle(215, 105, 7, YELLOW);
  delay(500);
  font.setColor(WHITE, BLACK);
  font.setScale(2);
  font.printStr(60, 150,(char*)"by Buzzz");

  analogWrite(TFT_LED, 255);
  delay(2000);
  analogWrite(TFT_LED, 0);
  delay(500);
  Screen_setup();

//  #ifdef DEBUG_INFO
//    Serial.println(font.strWidth("Speed: "));//92
//    Serial.println(font.strWidth("Zoom lvl: "));//130
//    Serial.println(font.strWidth("Value: "));//92
//    Serial.println(font.strWidth("Caliber: "));//124
//    Serial.println(font.strWidth("Calibrated: "));//170
//    Serial.println(font.strWidth("R damping: "));//152 
//  #endif

}

void loop()
{
//  Serial.println("Loop");
  switch (buttonPressed()) {          //test if a button was pressed
  case LEFT:
    //Serial.println("Pressure damping");
    Set_Damping(true);
    break;        //the menu is the only function that does not return asap

  case CALB:
    //Serial.println("Calibration");
    Calib_Init();
    break;

  case RIGHT:
    //Serial.println("RPM damping");
    Set_Damping(false);
    break;
  }
Sensor_loop();
}

void Sensor_loop()
{
  for (int SENSOR = 0; SENSOR < 2; SENSOR++)
  { //loop over all SENSORs
    MIN_VAL[SENSOR] = min (MIN_VAL[SENSOR], pulseSensor.getLatestSample(SENSOR));
    //Serial.print(MIN_VAL[0]);
    //Serial.print(",");   
    //Serial.println(MIN_VAL[1]);
    //pulseSensor.outputSample();

    if (pulseSensor.sawStartOfBeat(SENSOR)){
      if (SENSOR == 1)
      {
        AVERAGE[1] += SENSOR_CALIB[AVERAGE[1] - CALIB_MIN] ;
        RPM = (int) Rfilter.filter(pulseSensor.getBeatsPerMinute(1) << 1) / 10;
        RPM *= 10;
      }

      AVERAGE[SENSOR] = Pfilter.filter(MIN_VAL[SENSOR]);
      MIN_VAL[SENSOR]=1024;

      Pdelta = AVERAGE[1] - AVERAGE[0];
      PzoomedCalc(true, 0);
    }  
    #ifdef DEBUG_INFO
      //Serial.print(AVERAGE[0]);
      //Serial.print(",");
      //Serial.print(SENSOR_CALIB[AVERAGE[1] - CALIB_MIN]);
      //Serial.print(",");
      //Serial.print(AVERAGE[1]);
      //Serial.print(",");   
      //Serial.println(Pdelta);
      //Serial.print(",");
      //Serial.println(Pdelta_prev);
      //Serial.println(Pzoom);
    #endif
    delay(10);
  }
  if (millis() - last_Update_graph > refresh_graph && Pzoomed != Pzoomed_prev) Update_graph();

  Define_zoom();

  if (millis() - last_Update_num > refresh_num) Update_num();
}

void Screen_setup()
{
  tft.fillScreen(BLACK);
  tft.drawRect(0, 35, SCREEN_MAX, 40, WHITE);
  tft.writeFastVLine(MID_VAL , 25 , 60 , GREEN);
  #ifdef ST7789
  #else
    tft.writeFastHLine(0, SCREEN_MAX, SCREEN_SIZE, WHITE);
  #endif
  tft.writeFastHLine(0, 220, SCREEN_SIZE, YELLOW);
  font.setColor(YELLOW,BLACK);
  font.setScale(1);
  font.printStr(2, Y_BOT,(char*) "PRESS DAMP   |   CALIB   |   RPM DAMP");

  Pzoom_prev = MAX_GRAPH ;
  RPM_PREV = 4000 ;
  font.setColor(WHITE,BLACK);
  font.setScale(2);
  font.printStr(X_BASE, Y_ZOOM,(char*) "Zoom lvl: 1");
  font.printStr(X_BASE, Y_RPM, (char*)"Speed:    0RPM");

  analogWrite(TFT_LED, 255);
}

void Update_graph()
{
  uint8_t INDEX = Pzoomed + MID_VAL;

  // Clear the entire graph area
  tft.fillRect(1, 36, NUM_CALIB-1, 38, BLACK);
  // Draw the updated zoom level in white
  tft.fillRect(Pzoomed > 0 ? MID_VAL + 1 : INDEX, 36, abs(Pzoomed), 38, WHITE);
  tft.writeFastVLine(MID_VAL, 36, 38, GREEN); // Draw center line in green
  #ifdef DEBUG_INFO
//    Serial.println(Pzoomed);
  #endif

  Pzoomed_prev = Pzoomed;
  last_Update_graph = millis();
}

void Update_num()
{
  font.setColor(WHITE,BLACK);
  font.setScale(2);
  char buffer[20];
  if (RPM != RPM_PREV) {
    sprintf(buffer, "%4iRPM   ", RPM);
    font.printStr(X_BASE + X_RPM, Y_RPM, buffer);
//    sprintf(buffer, "Speed: %4dRPM  ", RPM);
//    font.printStr(X_BASE, Y_RPM, buffer);
    RPM_PREV = RPM;
  }
  if (Pzoom != Pzoom_prev) {
    sprintf(buffer, "%2i  ", Pzoom);
    font.printStr(X_BASE + X_ZOOM, Y_ZOOM, buffer);
//    sprintf(buffer, "Zoom lvl: %2d    ", Pzoom);
//    font.printStr(X_BASE , Y_ZOOM, buffer);
    Pzoom_prev = Pzoom;
  }
  last_Update_num = millis();
}

void Update_calib(int ABSCISSE, int LINESIZE, uint8_t QUALITY)
{
  unsigned int LINECOLOR [5]= {RED,RED,YELLOW,YELLOW,GREEN};
  if (LINESIZE < -49) LINESIZE = -49;
  else if (LINESIZE > 49) LINESIZE = 49;
  tft.writeFastVLine(ABSCISSE , 1 , 100 , BLACK);
  tft.writeFastVLine(ABSCISSE , min(51, 50 - LINESIZE) , abs(LINESIZE) , WHITE);
  tft.writeFastHLine(0 , 50 , SCREEN_MAX , GREEN);
  tft.writeFastVLine(ABSCISSE , 154 - QUALITY * 2, QUALITY * 2, LINECOLOR [map(QUALITY, 0, 20, 0, 4)]);
}

void PzoomedCalc(bool isPressure, int8_t CALIBRE)
{
  Pzoomed = isPressure ? Pdelta / (Pzoom_max / Pzoom) : readingStandard - readingSensor + CALIBRE;
  if (Pzoomed < -MAX_GRAPH) Pzoomed = -MAX_GRAPH;
  else if (Pzoomed > MAX_GRAPH) Pzoomed = MAX_GRAPH;
  Pzoomed_abs = abs(Pzoomed);
}

void Define_zoom()
{
    for (int i = 0; i < 3; ++i)
    {
        if (i == 0 && (Pzoomed_abs <= PZOOM_LIMIT[0]) && Pzoom < Pzoom_max)
        {
            handleZoomChange(0, refresh_zoom1, true, Pzoom << 1);
        }
        else if (i == 1 && (Pzoomed_abs > PZOOM_LIMIT[1]) && Pzoom > Pzoom_min)
        {
            handleZoomChange(1, refresh_zoom1, true, Pzoom >> 1);
        }
        else if (i == 2 && (Pzoomed_abs == PZOOM_LIMIT[2]) && Pzoom > Pzoom_min)
        {
            if (!Pzoom_change[2])
            {
                resetPzoomChange(2);
                Pzoom_timer[2] = millis();
            }
            else if (millis() - Pzoom_timer[2] > refresh_zoom2)
            {
                while ((Pzoomed_abs == PZOOM_LIMIT[2]) && Pzoom > 1)
                {
                    Pzoom >>= 1;
                    PzoomedCalc(true, 0);
                }
                Pzoom_change[2] = false;
            }
        }
    }
}

void handleZoomChange(uint8_t INDEX, uint16_t refreshTime, bool reset, uint8_t newZoomValue)
{
    if (!Pzoom_change[INDEX])
    {
        resetPzoomChange(INDEX);
        Pzoom_timer[INDEX] = millis();
    }
    else if (millis() - Pzoom_timer[INDEX] > refreshTime)
    {
        Pzoom = newZoomValue;
        Pzoom_change[INDEX] = false;
    }
}

void resetPzoomChange(int activeINDEX)
{
    for (int i = 0; i < 3; ++i)
    {
        Pzoom_change[i] = (i == activeINDEX);
    }
}

void Set_Damping(bool isPressure)
{
  uint8_t i = isPressure ? 0 : 1;
  font.setColor(YELLOW,BLACK);
  font.setScale(1);
  font.printStr(2, Y_BOT,(char*) "  -          |       SAVE       |         +   ");
//  font.setColor(WHITE,BLACK);

  char label[20];
  font.setScale(2);
  font.setColor(CYAN,BLACK);
  sprintf(label, "%s damping: %2i   ", isPressure ? "P" : "R",Settings[i]);
  font.printStr(X_BASE, Y_DAMP, label);
//  font.setColor(WHITE,BLACK);
  
  uint8_t damp_prev = Settings[i];
  while (true) {
    Sensor_loop();
    uint8_t button = buttonPressed();

    if (button == LEFT && Settings[i] > 1) (Settings[i]) /= 2;
    else if (button == RIGHT && Settings[i] < 1024) (Settings[i]) *= 2;
    else if (button == CALB) break;

    Pfactor = isPressure ? 1.0 / Settings[i] : Pfactor;
    Rfactor = isPressure ? Rfactor : 1.0 / Settings[i];
    Pfilter.alpha = Pfactor;
    Rfilter.alpha = Rfactor;

    if (damp_prev != Settings[i])
    {
      font.setColor(CYAN,BLACK);
      sprintf(label, "%i   ", Settings[i]);
      font.printStr(X_BASE + X_DAMP, Y_DAMP, label);
//      font.setColor(WHITE,BLACK);
      damp_prev = Settings[i];
    }
  }
  #ifdef DEBUG_INFO
    //Serial.print(AVERAGE[0]);
    //Serial.print(",");
    //Serial.print(SENSOR_CALIB[AVERAGE[1] - CALIB_MIN]);
    //Serial.print(",");
    //Serial.print(AVERAGE[1]);
    //Serial.print(",");   
    //Serial.println(Pdelta);
    //Serial.print(",");
    //Serial.println(Pdelta_prev);
    //Serial.println(Pzoom);
    //Serial.println("exit");
  #endif
  //tft.fillRect(0, Y_DAMP, SCREEN_MAX, 25, BLACK);
  actionSaveSettings();
  Screen_setup();
}

void Calib_Init()
{
  calib_lvl1_exit = false;
  Pzoomed_prev = 117;
  tft.fillRect(0, Y_ZOOM, SCREEN_SIZE, 60, BLACK);
  font.printStr(X_BASE , Y_ZOOM,(char*) "Value:");
  font.printStr(X_BASE , Y_RPM,(char*) "Caliber:");
  font.setColor(YELLOW,BLACK);
  font.setScale(1);
  font.printStr(2, Y_BOT,(char*) "CANCEL     |  CALIBRATE  |     CANCEL ");
  font.setColor(WHITE,BLACK);
  font.setScale(2);
  while(!calib_lvl1_exit)
  {
    switch (buttonPressed()) {
    case RIGHT:
    case LEFT:
      //Serial.println("RESET");
      calib_lvl1_exit = true;
      break;
    case CALB:
      //Serial.println("CANCEL");
      //Screen_setup(false);
      Calib();
      break;
    default:
      readingSensor = pulseSensor.getLatestSample(0);
      readingStandard = pulseSensor.getLatestSample(1);
      int8_t CALIBRE = 0;
      if (readingStandard >= CALIB_MIN && readingStandard < (CALIB_MIN + NUM_CALIB - 1))
      {
       CALIBRE = SENSOR_CALIB[readingStandard - CALIB_MIN];
//        Serial.println(CALIBRE);
      }
      PzoomedCalc(false, CALIBRE);
      if (millis() - last_Update_graph > refresh_graph)
      {
        Update_graph();
        char buffer[20];
        sprintf(buffer,"%4i  ", readingStandard);
        font.printStr(X_BASE + X_VALUE , Y_ZOOM, buffer);
        sprintf(buffer, "%3i ", CALIBRE);
        font.printStr(X_BASE + X_CALIBER, Y_RPM, buffer);
      }
    }
    delay(10);

  }
  Screen_setup();
}

void Calib()
{
  char prog[20];

  while (!calib_lvl1_exit)
  {
    calib_lvl2_exit = false;
    uint8_t TOTAL_QUAL = 0;
    uint8_t INDEX;
    uint8_t PROGRESS = 0;
    uint8_t PROGRESS_PREV = 200;
    uint8_t SENSOR_QUAL[NUM_CALIB] = {0};

    tft.fillScreen(BLACK);
    tft.drawRect(0, 0, SCREEN_SIZE, 102, WHITE);
    tft.writeFastHLine(0 , 50 , SCREEN_MAX , GREEN);
    tft.drawRect(0, 105, SCREEN_SIZE, 50, WHITE);
    font.printStr(X_BASE, Y_CALIB, (char*) "Calibrated:");
    tft.writeFastHLine(0, 220, SCREEN_MAX+1, YELLOW);
    font.setColor(YELLOW,BLACK);
    font.setScale(1);
    font.printStr(2, Y_BOT,(char*) "CANCEL       |   RESTART   |       SAVE ");
//    font.setColor(WHITE,BLACK);
    font.setScale(2);
    font.setColor(RED,BLACK);
    font.printStr(X_BASE + X_CALIB, Y_CALIB, (char*) "0%");

    while (!calib_lvl2_exit)
    {
      switch (buttonPressed()) {          //test if a button was pressed
        case LEFT:
          //Serial.println("CANCEL");
          calib_lvl1_exit = true;
          calib_lvl2_exit = true;
          readCalibrationFromEEPROM();
          break;
        case CALB:
          //Serial.println("RESET");
          calib_lvl2_exit = true;
          break;
        case RIGHT:
          //Serial.println("SAVE");
          calib_lvl1_exit = true;
          calib_lvl2_exit = true;
          writeCalibrationToEEPROM();
          break;
        default:
          unsigned int readingStandardPre = pulseSensor.getLatestSample(1);  //read master
          readingSensor = pulseSensor.getLatestSample(0);  //read master
          unsigned int readingStandardPost = pulseSensor.getLatestSample(1);  //read master again
          readingStandard = (readingStandardPre + readingStandardPost) >> 1; //average both to increase accuracy on slopes
//          Serial.print(readingStandard);
//          Serial.print(",");
//          Serial.println(readingSensor);
          if (readingStandard >= CALIB_MIN && readingStandard < (CALIB_MIN + NUM_CALIB - 1))
          {
            INDEX = readingStandard - CALIB_MIN;
            if (SENSOR_QUAL[INDEX]<20)
            {
              int CALIBRE = 0;
              if (SENSOR_QUAL[INDEX]>0) CALIBRE = SENSOR_CALIB[INDEX] * SENSOR_QUAL[INDEX];
              CALIBRE += readingSensor - readingStandard;
              CALIBRE /= SENSOR_QUAL[INDEX] + 1;
              if (CALIBRE < -128 ) CALIBRE = -128;
              else if (CALIBRE > 127 ) CALIBRE = 127;
              SENSOR_CALIB[INDEX] = CALIBRE;
              SENSOR_QUAL[INDEX]++;
              TOTAL_QUAL++;
              Update_calib(INDEX + 1,CALIBRE*2,SENSOR_QUAL[INDEX]);
              
              PROGRESS =  TOTAL_QUAL * 5 / NUM_CALIB;

              #ifdef DEBUG_INFO
//                Serial.print(SENSOR_CALIB[INDEX]);
//                Serial.print(",");
//                Serial.print(SENSOR_QUAL[INDEX]);
//                Serial.print(",");
//                Serial.println(PROGRESS);
              #endif

              if (PROGRESS != PROGRESS_PREV) 
              {
                if (PROGRESS >= 50) font.setColor(YELLOW,BLACK);
                if (PROGRESS >= 80) font.setColor(GREEN,BLACK);
                sprintf(prog, "%d%%   ", PROGRESS);
                font.printStr(X_BASE + X_CALIB, Y_CALIB, prog);
                PROGRESS_PREV = PROGRESS;
              }
            }
          }
      }
      delay(10);
    }
  }
}

void actionReadSettings()
{
  uint8_t compareVersion = 0;
  EEPROM.get(0, compareVersion);
  EEPROM.get(1, settingsOffset);
  if (compareVersion == versionUID)  //only load settings if saved by the current version, otherwise reset to 'factory' defaults
  {
    for (uint8_t i = 0; i < 3; i++)
    {
      EEPROM.get(settingsOffset + i, Settings[i]);
    }
  }

}

//saves our settings struct
void actionSaveSettings() {
  EEPROM.update(0, versionUID);  //only saves changed bytes!
  EEPROM.update(1, settingsOffset);
  for (uint8_t i = 0; i < 3; i++)
  {
    EEPROM.update(settingsOffset + i, Settings[i]);
  }
  delay(100);  //eeprom settle time

  if (0 != verifySettings())
  {
    settingsOffset++;
    actionSaveSettings();
    waitForAnyKey();
  }
}

//compares freshly loaded settings to the freshly saved verion, if there is a difference the save must have failed
//fail on write is the most common NVRAM failure by far
bool verifySettings() {
  uint8_t Settings_prev[3];
  for (uint8_t i = 0; i < 3; i++)
  {
    Settings_prev[0] = Settings[0];
    EEPROM.get(settingsOffset + i, Settings[i]);
  }
  return memcmp(Settings_prev, Settings, sizeof(Settings));
}


void readCalibrationFromEEPROM() {
  for (unsigned int i = 0; i < NUM_CALIB; i++) {
    uint8_t value = EEPROM.read(calibrationOffset + i);
    SENSOR_CALIB[i] = (value == 0xFF) ? 0 : value - 128;
    //Serial.print(i);
    //Serial.print(" | Calibration Value: ");
    //Serial.println(calibration[i]-128);
  }
}

void writeCalibrationToEEPROM() {
  char * points [] = {(char*) "    ", (char*) ".", (char*) "..", (char*) "..."};
  tft.fillScreen(BLACK);
  font.setColor(YELLOW,BLACK);
  font.setScale(3);
  font.printStr(40, 90,(char*) "SAVING");
  uint8_t point_num = 0;
  for (unsigned int i = 0; i < NUM_CALIB; i++) {
    font.printStr(167, 90, points[point_num]);
    if (i%10 == 0) point_num++;
    if (point_num == 4) point_num=0;
    uint8_t value = SENSOR_CALIB[i] + 128;
//    if ( value != EEPROM.read(calibrationOffset + i)) EEPROM.write(calibrationOffset + i, value);
    EEPROM.update(calibrationOffset + i, value);
    delay(10);
  }
  font.setColor(GREEN,BLACK);
  font.printStr(32, 90,(char*) "  SAVED!  ");
    tft.writeFastHLine(0, 220, SCREEN_MAX+1, YELLOW);
    font.setColor(YELLOW,BLACK);
    font.setScale(1);
    font.printStr(65, Y_BOT,(char*) "PRESS ANY BUTTON");
//    font.setColor(WHITE,BLACK);
//    font.setScale(2);
  waitForAnyKey();
}

uint8_t buttonPressed() {
  for (unsigned int i = 0; i < NUM_BUTTONS; i++) {
    uint8_t reading = digitalRead(i + LEFT);
    if (reading != lastButtonState[i]) debounceTimes[i] = millis();
    if (millis() - debounceTimes[i] > debounceDelay && reading != buttonState[i]) {
      buttonState[i] = reading;
      if (buttonState[i] == LOW) return i + LEFT;
    }
    lastButtonState[i] = reading;
  }
  //return 0xFF; // No button press
  return false; // No button press
}

void waitForAnyKey() {
	while (!buttonPressed()) {
		delay(50);
	}
}