// Arduino MEGA sketch per Pulley con interrupt su timer5

#include <Wire.h>
#include <EEPROM.h>
//#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include <Bounce2.h>
#include <HX711.h> //HX711 loadcell;
#include <SD.h>
#include <SPI.h>

//-------wiring -----------------------------------------------------
// Load Cell X  HX711 wiring
const int LoadCell_X_DOUT_PIN = 4;
const int LoadCell_X_SCK_PIN = 5;

// button wiring
#define button_D_PIN 6    //Down button
#define button_R_PIN 7    //Right button
#define button_S_PIN 8    //Select button
#define button_L_PIN 9    //Left button
#define button_U_PIN 10   //Up button
#define button_B_PIN 11   //Back button

//----Istantiate ---------------------------------------------


//instantiate Load Cell
HX711 LoadCell_X;

//Instatiate Oled SSD1306
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin  (verificare se inserire -1)
#define SCREEN_ADDRESS 0x3C ///< see datasheet or use I2C Scanner
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Instatiate the Bounce
Bounce button_D = Bounce();
Bounce button_R = Bounce();
Bounce button_S = Bounce();
Bounce button_L = Bounce();
Bounce button_U = Bounce();
Bounce button_B = Bounce();



//---------------------------------------------------------------

// Calibration factors of Load Cells
float calibration_X = 8662.9f;  // calib in Newton (cella TEDEA model 614 sn 32561360)
float Tension_X = 0;

const long interval = 100;     //display update time in millisecond

int stato = 0;          // initial step
const int addressKValue = 0; // Indirizzo di memoria EEPROM dove salvare la variabile
const int addressReadingTime= 10;
const int addressMode= 20;

int Reading_Time=0;
int Mode=0;

const int debounce = 5;    //debounce time in millisecond

bool FIRST = true;

unsigned long previousMillis = 0;
float KValue=1.00;


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


  pinMode(button_D_PIN, INPUT_PULLUP);
  pinMode(button_R_PIN, INPUT_PULLUP );
  pinMode(button_S_PIN, INPUT_PULLUP );
  pinMode(button_L_PIN, INPUT_PULLUP );
  pinMode(button_U_PIN, INPUT_PULLUP );
  pinMode(button_B_PIN, INPUT_PULLUP );

  

  //Attach debounce & define time interval
  button_D.attach( button_D_PIN);
  button_D.interval(debounce);
  button_R.attach( button_R_PIN);
  button_R.interval(debounce);
  button_S.attach( button_S_PIN);
  button_S.interval(debounce);
  button_L.attach( button_L_PIN);
  button_L.interval(debounce);
  button_U.attach( button_U_PIN);
  button_U.interval(debounce);
  button_B.attach( button_B_PIN);
  button_B.interval(debounce);

  // Inizializzazione display ---------------------------------------------
  // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Old Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  // Title screen
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);   // Draw white text
  display.setCursor(20, 10);
  display.setTextSize(2);                // Medium
  display.println("starting");
  show();
  // Inizializzazione LoadCells -------------------------------------
 
  Serial.println("Inialization LoadCell ");

  LoadCell_X.begin(LoadCell_X_DOUT_PIN, LoadCell_X_SCK_PIN);
  LoadCell_X.set_scale(calibration_X);  // this value is obtained by calibrating the scale with known weights; see the README for details   scale.set_scale(2280.f)
  LoadCell_X.tare();                // reset the scale to 0

  Serial.println("LoadCells X calibrated");
  // Inizializzazione LoadCells X Completed ---------------------------------


  


  //---------Setup completed--------------------------


}


void loop() {


  switch (stato) {
    case 0:
    mainloop();
    break;
    case 1010:
    Balance();
    break;
    case 1020:
    K_Value();
    break;
    case 1030:
    ReadingMode();
    break;    
    case 1040:
    ReadingTime();
    break;
    case 1050:
    Start();
  break;

  }

}

void setStato(int s) {
  stato = s;
  FIRST = true;
  display.clearDisplay();
  delay(100);
}


void mainloop() {            //stato 0

 


  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    Tens_X_measurement();
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(45, 4);
    display.println("Ready");
    display.setCursor(5, 25);

    display.print("Force:");
    display.setCursor(70, 25);
    display.print(Tension_X);
    display.setCursor(107, 25);
    display.print("N");


    display.setCursor(5, 55);

    display.print("S: Menu");
    display.setCursor(85, 55);
    display.print("D: Bal");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();

  if (button_D.fell()) { //Se il pulsante è premuto
    LoadCell_X.tare();  //Azzera
  }
 

  if (button_S.fell()) { //Se il pulsante è premuto
    setStato(1010);  //Passa al menù tensioning
  }

}


void Balance() {       //stato 1010
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }

  Tens_X_measurement();


  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(20, 5);
    display.println("Balance force");
    display.setCursor(5, 30);
    display.setTextSize(1);
    display.print("Force:");
    display.setCursor(50, 30);
    display.print(Tension_X);
    display.setCursor(107, 30);
    display.print("N");

    display.setCursor(5, 45);
    display.setTextSize(1);
    display.print("S:Balance");
    display.setCursor(85, 45);
    display.print("U:Prev");
    display.setCursor(5, 55);
    display.print("B:Back");
    display.setCursor(85, 55);
    display.print("D:Next");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_S.fell()) { //se il pulsante è premuto
    LoadCell_X.tare();  //Azzera

  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Torna al "Menù principale"

  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1020);  //Vai al menu "Travel Speed"

  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1050);  //Vai al menu "Max Travel"

  }

}





void K_Value() {       //stato 1020
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(32, 5);
    display.println("K_Value");
    display.setCursor(5, 20);
    display.setTextSize(1);
    display.print("K:");
    display.setCursor(70, 20);
    display.print(KValue);
    display.setCursor(107, 20);
    display.print("N/m");

    display.setCursor(5, 35);
    display.print("S:Save");
    display.setCursor(85, 35);
    display.print("U:prev");
    display.setCursor(5, 45);
    display.print("B:Back");
    display.setCursor(85, 45);
    display.print("D:Next");
    display.setCursor(5, 55);
    display.print("L:Dec");
    display.setCursor(85, 55);
    display.print("R:Inc");
    show();
  }


  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  
  if (button_R.fell()) { //se il pulsante è premuto
    KValue = KValue + 0.01; // Increase K
  }
  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //se il pulsante è premuto
    KValue = KValue + 0.1; //Increase K
    delay(100);
  }

  if (button_L.fell()) { //se il pulsante è premuto
     KValue = KValue - 0.01; //decrease K
  }
  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
     KValue = KValue - 0.1; //decrease K
    delay(100);
  }
  if (button_S.fell()) { //se il pulsante è premuto
    //EEPROM.write(addressTravel, MaxTravel);
    writeLongToEEPROM(addressKValue, KValue);
    Serial.println("KValue Saved on EEPROM"); 
    int pausa=0;
    while (pausa==0){
        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(32, 5);
        display.println("K_Value");
        display.setCursor(5, 20);
        display.setTextSize(1);
        display.print("SAVED in EEPROM");

        display.setCursor(5, 35);
        display.print("S:Save");
        display.setCursor(85, 35);
        display.print("U:prev");
        display.setCursor(5, 45);
        display.print("B:Back");
        display.setCursor(85, 45);
        display.print("D:Next");
        display.setCursor(5, 55);
        display.print("L:Dec");
        display.setCursor(85, 55);
        display.print("R:Inc");
        show();
        delay(500);
        pausa=1;
      }
        
  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1010);  //Vai al menu "Balance"


  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1030);  //Vai al menu "Reading Mode"


  }
 

}


void ReadingMode() {       //stato 1030
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }

  unsigned long currentMillis = millis();
  
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(40, 5);
    display.println("Reading mode");

    if (Mode==0) {
       display.setCursor(45, 25);
       display.print("Istantaneous");
    } else if (Mode==1){
      display.setCursor(55, 25);
      display.print("Average");
    }else if (Mode==2){
      display.setCursor(55, 25);
      display.print("Minimum");
    }
  
   
    display.setCursor(5, 45);
    display.setTextSize(1);
    display.print("S/B:Save/Back");
    display.setCursor(85, 45);
    display.print("U:Prev");
    display.setCursor(5, 55);
    display.print("L/R:Mode");
    display.setCursor(85, 55);
    display.print("D:Next");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_S.fell()) { //Se il pulsante è premuto
    EEPROM.write(addressMode, Mode);
    Serial.println("Data on ... saved on EEPROM"); 
    int pausa=0;
    while (pausa==0){
      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(40, 5);
      display.println("Reading Mode");
      display.setCursor(17, 25);
      display.print("SAVED in EEPROM");
  
      display.setCursor(5, 45);
      display.setTextSize(1);
      display.print("S/B:Save/Back");
      display.setCursor(85, 45);
      display.print("U:Prev");
      display.setCursor(5, 55);
      display.print("L/R:Mode");
      display.setCursor(85, 55);
      display.print("D:Next");
      show();
      delay(500);
      pausa=1;  
      
    }
  
  }
  
  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Torna al "Menù principale"
   
  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1040);  //Vai al menu "Reading time"
 
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1020);  //Vai al menu "K Value"
   
  }
  if (button_L.fell()) { //se il pulsante è premuto
   Mode = Mode + 1;
    if (Mode>2) { 
      Mode=0;
    } 
    Serial.print("Mode = ");
    Serial.println(Mode);
  }
  if (button_R.fell()) { //se il pulsante è premuto
      Mode= Mode -1;
       if (Mode<0) { 
      Mode=2;
	 } 
	 
    Serial.print("Mode = ");
    Serial.println(Mode);
  

  
 }
}



void ReadingTime() {             //stato 1040

  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);

    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(20, 5);
    display.println("Reading time");
    display.setCursor(5, 20);
    display.print("Time");
    display.setCursor(55, 20);
    display.print(Reading_Time);
    display.setCursor(90, 20);
    display.print("Sec");
    display.setCursor(5, 35);
    display.print("S:Save");
    display.setCursor(85, 35);
    display.print("U:prev");
    display.setCursor(5, 45);
    display.print("B:Back");
    display.setCursor(85, 45);
    display.print("D:Next");
    display.setCursor(5, 55);
    display.print("L:Dec");
    display.setCursor(85, 55);
    display.print("R:Inc");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_R.fell()) { //se il pulsante è premuto
    Reading_Time = Reading_Time + 1; //Increase Reading time
  }

  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //se il pulsante è premuto
    Reading_Time = Reading_Time + 10; //Increase Reading time
    delay(200);
  }


  if (button_L.fell()) { //se il pulsante è premuto
    Reading_Time = Reading_Time - 1; //decrease Reading time
  }

  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
    Reading_Time = Reading_Time - 10; //decrease Reading time
    delay(200);
  }

  if (button_S.fell()) { //se il pulsante è premuto
    EEPROM.write(addressReadingTime, Reading_Time);
    Serial.println("Reading Time Saved on EEPROM"); 

     int pausa=0;
       while (pausa==0){
        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(20, 5);
        display.println("Reading time");
        display.setCursor(5, 20);
        display.print("SAVED on EEPROM");

        display.setCursor(5, 35);
        display.print("S:Save");
        display.setCursor(85, 35);
        display.print("U:prev");
        display.setCursor(5, 45);
        display.print("B:Back");
        display.setCursor(85, 45);
        display.print("D:Next");
        display.setCursor(5, 55);
        display.print("L:Dec");
        display.setCursor(85, 55);
        display.print("R:Inc");
        show();
        delay(500);
        pausa=1;
      }
  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1030);  //Vai al menu "Reading Mode"
  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1050);  //Vai al menu "Start"
  }
  
}



void Start() {       //stato 1050
  
  
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);

    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    Tens_X_measurement();
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(20, 5);
    display.println("Measuring");


    display.setCursor(5, 25);
    display.print("Force:");
    display.setCursor(70, 25);
    display.print(Tension_X);
    display.setCursor(107, 25);
    display.print("N");


    display.setCursor(5, 35);
    display.print("S:Stop");
    display.setCursor(85, 35);
    display.print("U:prev");
    display.setCursor(5, 45);
    display.print("B:Back");
    display.setCursor(85, 45);
    display.print("D:Next");

    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_S.fell()) { //se il pulsante è premuto
    setStato(0); //Increase Reading time
  }


  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1040);  //Vai al menu "Reading Mode"
  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1010);  //Vai al menu "Start"
  }


}


void show() { // Often used sequence - Function to simplify code
  display.display();
  //delay(500); display.fillScreen(SSD1306_BLACK);
}


void Tens_X_measurement() {
  Tension_X = LoadCell_X.get_units(1);
}


void writeLongToEEPROM(int address, long value) {
  // Decompone il valore long in 4 byte
  byte byte1 = (value >> 24) & 0xFF;
  byte byte2 = (value >> 16) & 0xFF;
  byte byte3 = (value >> 8) & 0xFF;
  byte byte4 = value & 0xFF;

  // Scrive ogni byte in EEPROM
  EEPROM.write(address, byte4);
  EEPROM.write(address + 1, byte3);
  EEPROM.write(address + 2, byte2);
  EEPROM.write(address + 3, byte1);
}

long readLongFromEEPROM(int address) {
  long value = 0;
  value += ((long)EEPROM.read(address + 3)) << 24;
  value += ((long)EEPROM.read(address + 2)) << 16;
  value += ((long)EEPROM.read(address + 1)) << 8;
  value += EEPROM.read(address);
  return value;
}