// Arduino MEGA sketch per Pulley
//#include <SPI.h>           // Not needed with I2C
#include <Wire.h>
#include <EEPROM.h>
//#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>
#include <Bounce2.h>
#include <HX711.h> //HX711 loadcell;

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

// Load Cell Y circuit wiring
//const int LoadCell_Y_DOUT_PIN = A1;
//const int LoadCell_Y_SCK_PIN = A0;
//Camera connector wiring --------------------------------
//const int ShutterPin= A8;
//const int FocusPin= A9;

//X axis stepper motor wiring with driver A4988
const int dirPin_stepperX = 2;
const int stepPin_stepperX = 3;
const int enablePin_stepperX = 12;
//Y axis stepper motor wiring with driver A4988
//const int dirPin_stepperY = 19;
//const int stepPin_stepperY = 18;
//const int enablePin_stepperY = 13;
// 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
// microswitch wiring
#define homeSw_x_PIN 14   //homeSw X
#define endSw_x_PIN 15    //endSw X
//#define homeSw_y_PIN 16   //homeSw Y
//#define endSw_y_PIN 17    //endSw Y
//------------------------------------------------------------
//----Istantiate ---------------------------------------------
//Instatiate Accelstepper
AccelStepper stepperX(1, stepPin_stepperX, dirPin_stepperX);
//AccelStepper stepperY(1, stepPin_stepperY, dirPin_stepperY);

//instantiate Load Cell
HX711 LoadCell_X;
//HX711 LoadCell_Y;

//Instatiate Oled SSD1306
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     4 // 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();

Bounce homeSw_x = Bounce();
Bounce endSw_x = Bounce();
//Bounce homeSw_y = Bounce();
//Bounce endSw_y = Bounce();
//---------------------------------------------------------------

// Calibration factors of Load Cells
float calibration_X = 113047.9f;  //1109000 coefficiente cal in kg --> 1109000/9.81=113047.9f calib in Newton



float Tension_X = 0;       //Initial mesh tension


float TensTarget = 10.0;    //Target Mesh tension N/m
float PullTarget = 10.0;	//Pull target Mesh tension N/m
float TargetToll = 1.0;
float X_position = 0.0;     //Starting position
int TensSpeed = 50.0;     //Tensioning speed in mm/min

float screwPitch = 1.0;         //screw pitch in mm
float displacementPerStep = 1;

long initial_homingX = -1;  // Used to Home Stepper at startup
//long initial_homingY = -1;  // Used to Home Stepper at startup
long step = 0;        // initial step
long stpRunX = 1;
//long stpRunY = 1;
const long interval = 100;     //display update time in millisecond


int stepPerRev = 200 * 8; //8 microstepping MS1=H, MS2=H, MS3=L
//int NumSteps=1;
int stato = 0;          // initial step
const int addressTravel = 0; // Indirizzo di memoria EEPROM dove salvare la variabile
const int addressSpeed= 10;
int MaxTravel=50;

//int Homing_done = 0;
int nCycles = 2;
int i=0; //contatore
const int debounce = 30;    //debounce time in millisecond

bool FIRST = true;
bool LoadCompleted = false;
bool UnloadCompleted = false;
//bool toggle=0;

//time constants
unsigned long previousMillis = 0;
//unsigned long previousMillis2 = 0;
unsigned long startTime = 0;
unsigned long pressDuration = 0;

//------------------------------------------------------------
ISR(TIMER5_COMPA_vect){
stepperX.runSpeed();

}


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 );
  pinMode(homeSw_x_PIN , INPUT_PULLUP);
  pinMode(endSw_x_PIN , INPUT_PULLUP);
  pinMode(enablePin_stepperX, OUTPUT);

  

  //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);

  homeSw_x.attach(homeSw_x_PIN , INPUT_PULLUP);
  homeSw_x.interval(debounce);

  endSw_x.attach(endSw_x_PIN , INPUT_PULLUP);
  endSw_x.interval(debounce);




  //inizializzazione Stepper
  stepperX.setMaxSpeed(300.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(1000.0);
  stepperX.setEnablePin(enablePin_stepperX);
  stepperX.setPinsInverted(false, false, false, false, true);





  // 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.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);   // Draw white text
  display.setCursor(20, 10);
  display.setTextSize(2);                // Medium
  display.println("starting");
  show();
  // Inizializzazione LoadCells -------------------------------------
  // Inizializzazione LoadCells X
  Serial.println("Inialization LoadCell X");

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



  displacementPerStep = screwPitch / stepPerRev;

// setup timer 5
  TCCR5A = 0;          // normal operation
  //TCCR5B = bit(WGM12) | bit(CS10)| bit(CS12);   // CTC, prescaler 1024
  TCCR5B = bit(WGM12) | bit(CS10);   // CTC, prescaler 1 0B00001001 WGM12 è settatto a 1 per avere il CTC (Cleat Timer Compare) con il valore di OCR5A 
 //   Serial.print("TCCR5A = ");
 //   Serial.println(TCCR5A, BIN);
  //  Serial.print("TCCR5B = ");
  //  Serial.println(TCCR5B, BIN);
  OCR5A =  4000;    // con prescaler 1, periodo clock 62.5nS --> periodo di interrupt 250microS -->4000 Step/s
// TIMSK5 = bit (OCIE1A);             // Attiva timer inteterrupt settando a 1 il bit OCIE1A
  TIMSK5 = 0;      				// Disattiva interrupt timer 5
//    Serial.print("TIMSK5 = ");
//    Serial.println(TIMSK5, BIN);
// end of setup TIMER 5



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

   MaxTravel = EEPROM.read(addressTravel);
   TensSpeed= EEPROM.read(addressSpeed);

}


void loop() {

  unsigned long currentMillis = millis();

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

  switch (stato) {
    case 0:
      mainloop();
      break;
	  
    case 1010:
      Max_travel();
      break;
    case 1020:
      Balance();
      break;
    case 1030:
      SetSpeed();
      break;
    case 1040:
      Start_Tension();
      break;
	case 1050:  
      Homing_menu();
	  break;
  }

}

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


void mainloop() {            //stato 0

  Tens_X_measurement();


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

    FIRST = false;
  }
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    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, 35);	
	  display.print("Position:");
    display.setCursor(70, 35);
    display.print(X_position);
    display.setCursor(107, 35);
    display.print("mm");

    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 Max_travel() {       //stato 1010
  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("Max travel");
    display.setCursor(5, 20);
    display.setTextSize(1);
    display.print("Travel:");
    display.setCursor(70, 20);
    display.print(MaxTravel);
    display.setCursor(107, 20);
    display.print("mm");

    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 valore del pulsante è 1
    MaxTravel = MaxTravel + 1; //Increase speed
  }
  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //Se il valore del pulsante è 1
    MaxTravel = MaxTravel + 10; //Increase speed
    delay(100);
  }

  if (button_L.fell()) { //Se il valore del pulsante è 1
    MaxTravel = MaxTravel - 1; //decrease speed
  }
  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //Se il valore del pulsante è 1
    MaxTravel = MaxTravel - 10; //decrease speed
    delay(100);
  }
  if (button_S.fell()) { //Se il valore del pulsante è 1
    EEPROM.write(addressTravel, MaxTravel);
    Serial.println("Saved on EEPROM"); 
  }

  if (button_B.fell()) { //Se il valore del pulsante è 1
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //Se il valore del pulsante è 1
    setStato(1050);  //Vai al menu "Homing"
   // stepperX.disableOutputs();
    stepperX.enableOutputs();

  }

  if (button_D.fell()) { //Se il valore del pulsante è 1
    setStato(1020);  //Vai al menu "Balance"
    stepperX.disableOutputs();

  }
 

}


void Balance() {       //stato 1020
  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 valore del pulsante è 1
    LoadCell_X.tare();  //Azzera

  }

  if (button_B.fell()) { //Se il valore del pulsante è 1
    setStato(0);  //Torna al "Menù principale"
    stepperX.disableOutputs();
  }

  if (button_D.fell()) { //Se il valore del pulsante è 1
    setStato(1030);  //Vai al menu "Travel Speed"
    stepperX.disableOutputs();
  }

  if (button_U.fell()) { //Se il valore del pulsante è 1
    setStato(1010);  //Vai al menu "Max Travel"
    stepperX.disableOutputs();
  }
}


void SetSpeed() {             //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(20, 5);
    display.println("Travel Speed");
    display.setCursor(5, 20);
    display.print("Speed");
    display.setCursor(55, 20);
    display.print(TensSpeed);
    display.setCursor(90, 20);
    display.print("mm/min");
    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 valore del pulsante è 1
    TensSpeed = TensSpeed + 1; //Increase speed
  }
  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //Se il valore del pulsante è 1
    TensSpeed = TensSpeed + 10; //Increase speed
    delay(200);
  }


  if (button_L.fell()) { //Se il valore del pulsante è 1
    TensSpeed = TensSpeed - 1; //decrease speed
  }

  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //Se il valore del pulsante è 1
    TensSpeed = TensSpeed - 10; //decrease speed
    delay(200);
  }

  if (button_S.fell()) { //Se il valore del pulsante è 1
    EEPROM.write(addressSpeed, TensSpeed);
    Serial.println("Saved on EEPROM"); 
  }

  if (button_B.fell()) { //Se il valore del pulsante è 1
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //Se il valore del pulsante è 1
    setStato(1020);  //Vai al menu "Balance Tensioning"
    stepperX.disableOutputs();

  }

  if (button_D.fell()) { //Se il valore del pulsante è 1
    setStato(1040);  //Vai al menu "Start Tensioning"
    stepperX.enableOutputs();

  }
  stepperX.setMaxSpeed(TensSpeed / screwPitch / 60 * stepPerRev);

}


void Start_Tension() {       //stato 1040
  int NumSteps=1;
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    stepperX.setSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
  
    FIRST = false;
  }

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

    display.setCursor(1, 20);
    display.setTextSize(1);
    display.print("Force:");
    display.setCursor(70, 20);
    display.print(Tension_X);
    display.setCursor(107, 20);
    display.print("N");

    display.setCursor(1, 30);	
	  display.print("Position:");
    display.setCursor(70, 30);
    display.print(X_position);
    display.setCursor(107, 30);
    display.print("mm");

    display.setCursor(1, 45);
    display.print("S/R:Strt/Stp");
    display.setCursor(90, 45);
    display.print("U:Prev");
    display.setCursor(1, 55);
    display.print("L/B:UnLd/Bck");
    display.setCursor(90, 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 valore del pulsante è 1
    Serial.println("Start"); //Fa partire il motore
	  LoadCompleted=false;
    NumSteps = MaxTravel * stepPerRev / screwPitch;

	  Load(NumSteps); 
	  Tens_X_measurement();
 
    stepperX.stop();

    Serial.println("Travel completed");
  }

  if (button_R.fell()) { //Se il valore del pulsante è 1
    Serial.println("Stop"); //Arresta il motore
    stepperX.stop();

  }

  if (button_U.fell()) { //Se il valore del pulsante è 1
    setStato(1030); //Set tension Speed
    stepperX.disableOutputs();
   // stepperY.disableOutputs();
  }
  if (button_D.fell()) { //Se il valore del pulsante è 1
    setStato(1050); //homing
    stepperX.enableOutputs();

  }
  if (button_B.fell()) { //Se il valore del pulsante è 1
    setStato(0);  //Vai al menu "main menu"
    stepperX.disableOutputs();
   // stepperY.disableOutputs();
  }
  if (button_L.fell()) { //Se il valore del pulsante è 1
    homing_procedure(); //Chiama la funzione homing
  }

}


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


    display.setCursor(28, 10);
    display.setTextSize(1);
    display.print("Homing Menu");

    display.setCursor(5, 45);
    display.print("S:Start");
    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();
    FIRST = false;
  }
  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();

  // key = digitalRead(button_S);  //Lettura del pulsante
  if (button_S.fell()) { //Se il valore del pulsante è 1
    homing_procedure();
    setStato(0);  //Esegui l'homing e passa a "Tensioning_menu"
  }

  //key = digitalRead(button_B);  //Lettura del pulsante
  if (button_B.fell()) { //Se il valore del pulsante è 1
    setStato(0);  //Torna al Main menù
  }
  //key = digitalRead(button_D);  //Lettura del pulsante
  if (button_D.fell()) { //Se il valore del pulsante è 1
    setStato(1010);  //Vai al menu "Max travel"
  }
  //key = digitalRead(button_U);  //Lettura del pulsante
  if (button_U.fell()) { //Se il valore del pulsante è 1
    setStato(1040);  //Vai al menu "Start"
  }
}


void homing_procedure() {   
  display.clearDisplay();
  //display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);   // Draw white text
  display.setCursor(20, 10);
  display.setTextSize(2);                // Medium
  display.println("Homing X");
  show();

  stepperX.enableOutputs();

  homeSw_x.update();
 // homeSw_y.update();
  Serial.println("Stepper X is Homing . . . . . . . . . . . ");

  while (homeSw_x.read() == 1) {     // Make the Stepper move CCW until the switch is activated
    homeSw_x.update();
    stepperX.moveTo(initial_homingX);   // Set the position to move to
    initial_homingX--;                  // Decrease by 1 for next move if needed
    stepperX.run();                     // Start moving the stepper
  }

  stepperX.setCurrentPosition(0);       // Set the current position as zero for now
  initial_homingX = 1;


  while (homeSw_x.read() == 0) {    // Make the Stepper move CW until the switch is deactivated
    homeSw_x.update();
    stepperX.moveTo(initial_homingX);
    stepperX.run();
    initial_homingX++;
  }

  stepperX.setCurrentPosition(0);
  Serial.println("Homing X Completed");
  Serial.println("");
  stepperX.disableOutputs();

  // Homing X procedure Completed

}


void Load(int Steps) { 

  Tens_X_measurement();
  //Tens_Y_measurement();
  TIMSK5 = bit (OCIE1A); //attiva timer 5
  stepperX.setSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
  Serial.print("step/sec:  ");
  Serial.println(TensSpeed / screwPitch / 60 * stepPerRev);
  //stepperY.setSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
  stepperX.moveTo(Steps);


  while ((stepperX.distanceToGo() != 0)) {
      button_R.update();

      unsigned long currentMillis = millis();
      if (currentMillis - previousMillis >= 250) {
        previousMillis = currentMillis;
        Tens_X_measurement();
       // Tens_Y_measurement();
        X_position = stepperX.currentPosition() * displacementPerStep;
       // Y_position = stepperY.currentPosition() * displacementPerStep;

        Serial.print(X_position);
        Serial.print(",");
        Serial.println(Tension_X);
     

        /*

        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(32, 5);
        display.println("Start Tens");

        display.setCursor(1, 20);
        display.setTextSize(1);
        display.print("Force:");
        display.setCursor(70, 20);
        display.print(Tension_X);
        display.setCursor(107, 20);
        display.print("N");
        display.setCursor(1, 30);	
	      display.print("Position:");
        display.setCursor(70, 30);
        display.print(X_position);
        display.setCursor(107, 30);
        display.print("mm");


        display.setCursor(1, 45);
        display.print("S/R:Strt/Stp");
        display.setCursor(90, 45);
        display.print("U:Prev");
        display.setCursor(1, 55);
        display.print("L/B:UnLd/Bck");
        display.setCursor(90, 55);
        display.print("D:Next");
        show();
        */

      }


      if (button_R.fell()) { //Se il valore del pulsante è 1
        Serial.println("Stop"); //Arresta il motore
        stepperX.stop();
        Tension_X = TensTarget;
		    LoadCompleted=true;
    
      }
	  
    }
	LoadCompleted=true;
	TIMSK5 =0;
}



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)*125;
}