// Arduino MEGA sketch per Tension Gauge

#include <Wire.h>
#include <EEPROM.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 = 1781200.0f; //Cella 0.1Kg coefficiente cal in N 
float Force = 0.0f; //Tension_X
float TensionTie=0.0f;
float RatioTiesTh =4.19656f; // 4.19656  Con cella calibrata in Newton e tension gauge avente distanza pulegge D=50mm e h=3mm il rapporto fra le forse dato da questa costante
float Delta_Tdue_toK=0.0179f; //0.0179
float minimum;
float Time_sec=0.0f;

float Avg_measurement=0.0f;
float Sum=0.0f;

int 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 NumbSample=1;
int Reading_Time=0;
int time=0; 
int Mode=0;

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

bool FIRST = true;

unsigned long previousMillis = 0;
float KValue=1.00;


int UseSD = 1;

//----------- SD CARD --------------

File dataFile;
int fileNumber = 1;
bool writingDone = false;
bool readingDone = false;
bool stop = false;
const int chipSelect = 53;
//----------------------------------


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--------------------------
 // Inizializza la scheda SD
  if (!SD.begin(chipSelect)) {
    Serial.println("Errore durante l'inizializzazione della scheda SD");
    return;
  }
  


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


  
   KValue = readLongFromEEPROM(addressKValue);
   Reading_Time = EEPROM.read(addressReadingTime);
   Mode = EEPROM.read(addressMode);


}


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(Force,3);
    display.setCursor(107, 25);
    display.print("N");


    display.setCursor(5, 45);
    display.print("S: Start");
    display.setCursor(85, 45);
    display.print("D: Menu");
    display.setCursor(5, 55);
    display.print("L: 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
    setStato(1010);  //Passa al menù Balance
  }
 

  if (button_S.fell()) { //Se il pulsante è premuto
    setStato(1050);  //Passa a start
  }
  
  if (button_L.fell()) { //Se il pulsante è premuto
    LoadCell_X.tare();  //Azzera
  }

}


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(25, 5);
    display.println("Balance force");
    display.setCursor(5, 30);
    display.setTextSize(1);
    display.print("Force:");
    display.setCursor(50, 30);
    display.print(Force,3);
    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 "K_Value"

  }

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

  }

}





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(15, 5);
    display.println("Spring stiffness");
    display.setCursor(5, 20);
    display.setTextSize(1);
    display.print("K:");
    display.setCursor(50, 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.1; // Increase K
  }

  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //se il pulsante è premuto
    KValue = KValue + 1; //Increase K
      delay(50);
      button_R.update();
      delay(50);
      if(button_R.changed()) {
          KValue = KValue-0.1;

      }
    
  }



  if (button_L.fell()) { //se il pulsante è premuto
     KValue = KValue - 0.1; //decrease K
  }

  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
      KValue = KValue - 1; //decrease K
      delay(50);
      button_L.update();
      delay(50);
      if(button_L.changed()) {
        KValue = KValue+0.1;
      }
  }

  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(40, 5);
        display.println("K Value");
        display.setCursor(20, 20);
        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(30, 5);
    display.println("Reading mode");

    if (Mode==0) {
       display.setCursor(30, 25);
       display.print("Istantaneous");
    } else if (Mode==1){
      display.setCursor(45, 25);
      display.print("Average");
    }else if (Mode==2){
      display.setCursor(45, 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(30, 5);
      display.println("Reading mode");
      display.setCursor(20, 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(25, 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 (Reading_Time<=0) {
      Reading_Time=0;
    }
  }

  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
    Reading_Time = Reading_Time - 10; //decrease Reading time
      if (Reading_Time<=0) {
      Reading_Time=0;
    }
    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(25, 5);
        display.println("Reading time");
        display.setCursor(20, 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(1010);  //Vai al menu "Balance"
  }
  
}



void Start() {       //stato 1050

  
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
	  time=Reading_Time*1000;
    FIRST = false;
    Tens_X_measurement();
    TensionTie = Force*RatioTiesTh - KValue*0.36/1000;
	  minimum= TensionTie;
    NumbSample=1;
    Sum=0;
  }
  
  unsigned long currentMillis = millis();


  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
	
    if (Mode==0 && time>0) { //Istantaneous

      Tens_X_measurement();
      time= (time-interval);
      Time_sec=time/1000;

      TensionTie = Force*RatioTiesTh - KValue*0.36/1000; //0.36/1000 + la variazione di lunghezza della molla a causa dell'inserimento dello strumento che moltiplicato per il K molla di fornisce la variazione di forza

      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(45, 5);
      display.println("Measuring");


      display.setCursor(5, 15);
      display.print("LC Force:");
      display.setCursor(70, 15);
      display.print(Force,3);
      display.setCursor(107, 15);
      display.print("N");

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

      display.setCursor(5, 35);
      display.print("Time");
      display.setCursor(35, 35);
      display.print(Time_sec);
      display.setCursor(80, 35);
      display.print("sec");


      display.setCursor(5, 45);
      display.print("S:Stop");
      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();
      if ((time<=0) && Mode==0) {
		     display.clearDisplay();
         display.setTextSize(1);
         display.setCursor(20, 5);
         display.println("Measure done ");

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

         display.setCursor(5, 45);
         display.print("S:Stop");
         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();
      }
	
    } else if (Mode==1){ //Average
	
	  if (time<=0) {
		 display.clearDisplay();
         display.setTextSize(1);
         display.setCursor(20, 5);
         display.println("Measuring done ");

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

         display.setCursor(5, 45);
         display.print("S:Stop");
         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();



	  } else if (time>0 && Mode==1){
         Serial.println(time);
         Serial.println(interval);
         Serial.println(NumbSample);
         Tens_X_measurement();
         TensionTie = Force*RatioTiesTh - KValue*0.36/1000; //0.36/1000 + la variazione di lunghezza della molla a causa dell'inserimento dello strumento che moltiplicato per il K molla di fornisce la variazione di forza
	       Sum = Sum + TensionTie;
        
		     Avg_measurement=Sum/NumbSample;
		     Serial.println(Avg_measurement);
         time= (time-interval);
         Time_sec=time/1000;
         display.clearDisplay();
         display.setTextSize(1);
         display.setCursor(11, 5);
         display.println("AVG Measuring");

         display.setCursor(5, 15);
         display.print("LC Force:");
         display.setCursor(70, 15);
         display.print(Force, 3);
         display.setCursor(107, 15);
         display.print("N");

         display.setCursor(5, 25);
         display.print("Tie Force:");
         display.setCursor(70, 25);
         display.print(Avg_measurement, 3);
         display.setCursor(107, 25);
         display.print("N");
         display.setCursor(5, 35);
         display.print("Time");
         display.setCursor(35, 35);
         display.print(Time_sec);
         display.setCursor(80, 35);
         display.print("sec");


         display.setCursor(5, 45);
         display.print("S:Stop");
         display.setCursor(85, 45);
         display.print("U:prev");
         display.setCursor(5, 55);
         display.print("B:Back");
         display.setCursor(85, 55);
         display.print("D:Next");
	       NumbSample=NumbSample+1;
	      
        
         show();
     }
	

    } else if (Mode==2){ //Minimum 
  
      Tens_X_measurement();
      TensionTie = Force*RatioTiesTh - KValue*0.36/1000; //0.36/1000 + la variazione di lunghezza della molla a causa dell'inserimento dello strumento che moltiplicato per il K molla di fornisce la variazione di forza

	    if (TensionTie < minimum) {
		   minimum = TensionTie;
	    }
	  
      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(15, 5);
      display.println("Measuring minimum");


      display.setCursor(5, 20);
      display.print("Force LC:");
      display.setCursor(70, 20);
      display.print(Force, 3);
      display.setCursor(107, 20);
      display.print("N");

      display.setCursor(5, 30);
      display.print("Min F Tie:");
      display.setCursor(70, 30);
      display.print(minimum,3);
      display.setCursor(107, 30);
      display.print("N");


      display.setCursor(5, 45);
      display.print("S:Stop");
      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
    time=0; //Stop
  }


  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() {
	 Force = LoadCell_X.get_units(1)*1000;

}


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