#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>
#include <ezButton.h>  // the library to use for SW pin

// Display
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Stepper
#define MOTOR_STEP_PIN 7
#define MOTOR_DIR_PIN 6
#define MOTOR_EN_PIN 5
#define STEPS_PER_REV 200 // Nombre de pas par révolution pour le moteur NEMA 17
#define STEPS_PER_MM 40 // Nombre de pas par millimètre (ajustez selon votre configuration)
int MOVEMENT_SPEED = 1000; // Vitesse de mouvement en pas par seconde
int maxPosition;
int minPosition;
long int targetPosition;
long int currentPositionstep;
int positionMM;
AccelStepper stepper(AccelStepper::DRIVER, MOTOR_STEP_PIN, MOTOR_DIR_PIN);

// Push buttons
#define UPbtn 9
#define DOWNbtn 8
#define HOMEbtn 13
#define LIMIT_SWITCH_DOWN 11
#define HOME_SWITCH 10
int upSwitchState = HIGH;
int downSwitchState = HIGH;
ezButton up(UPbtn);
ezButton down(DOWNbtn);
ezButton home(HOMEbtn);
ezButton downSwitch(LIMIT_SWITCH_DOWN);
ezButton homeSwitch(HOME_SWITCH);

// Rotary Encoder
#define CLK_PIN 2
#define DT_PIN 3
#define SW_PIN 4
#define DIRECTION_CW 0   // clockwise direction
#define DIRECTION_CCW 1  // EncPos-clockwise direction
int EncPos = 0;
int Position = 0;
int newPosition = 0;
int countClick = 0;
int direction = DIRECTION_CW;
int CLK_state;
int prev_CLK_state;
ezButton encoder(SW_PIN);  // create ezButton object that attach to pin 4

void setup() {
  Serial.begin(9600); // start the serial monitor link

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  delay(2000);
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  // Display static text
  display.println("Hello, world!");
  display.display(); 

 // configure ouputs
  pinMode(MOTOR_EN_PIN, OUTPUT);
  digitalWrite(MOTOR_EN_PIN, LOW); // Active le driver

  // configure encoder
  pinMode(CLK_PIN, INPUT_PULLUP);
  pinMode(DT_PIN, INPUT_PULLUP);
  encoder.setDebounceTime(50);  // set debounce time to 50 milliseconds
  prev_CLK_state = digitalRead(CLK_PIN);

  // configure inputs
  up.setDebounceTime(100);
  down.setDebounceTime(100);
  home.setDebounceTime(100);
  downSwitch.setDebounceTime(100);
  homeSwitch.setDebounceTime(100);

}


void loop() {
 encoder.loop();  // MUST call the loop() function first
 up.loop(); 
 down.loop(); 
 home.loop(); 
 downSwitch.loop(); 
 homeSwitch.loop();
 downSwitchState =  downSwitch.getState();
 
  if (encoder.isPressed()) {
    Serial.println("The encoder is pressed");
    if (countClick == 2) {
          countClick--;
        }
        else {
          countClick++;
        }
        Serial.println(countClick);
  } 
  
  if (countClick == 1) {
    modReglage ();
  }

  if (countClick == 2) {
    modDeplacement ();
  }  

  if (up.isPressed()){
    Serial.println("The upButton is pressed");
    upfn();
  }

  if (down.isPressed()){
    Serial.println("The downButton is pressed");
    downfn();
  }

  if (home.isPressed()){
    Serial.println("The homeButton is pressed");
    homeProcedure();
  }

}

void homeProcedure() {
  stepper.setCurrentPosition(0); // Initialise la position actuelle du moteur
  stepper.setMaxSpeed(MOVEMENT_SPEED);
  stepper.setSpeed(MOVEMENT_SPEED); // Définir la vitesse positive (CCW)
  while (digitalRead(LIMIT_SWITCH_DOWN) == HIGH) {
    stepper.runSpeed(); // Déplace le moteur dans le sens CCW
  }
  stepper.setCurrentPosition(0); // Réinitialisez la position actuelle
  delay(500); // Attendez un peu avant de changer de direction
  
  // Faites tourner le moteur dans le sens des aiguilles d'une montre (CW) jusqu'à ce que le limiteur de fin de course CW soit atteint
  stepper.setSpeed(-MOVEMENT_SPEED); // Définir la vitesse négative (CW)
  while (digitalRead(HOME_SWITCH) == HIGH) {
    stepper.runSpeed(); // Déplace le moteur dans le sens CW
  }
    stepper.setCurrentPosition(0); // Réinitialisez la position actuelle

  // 
  stepper.setSpeed(MOVEMENT_SPEED); 
  while (digitalRead(LIMIT_SWITCH_DOWN) == HIGH) {
    stepper.runSpeed(); // Déplace le moteur dans le sens CCW
  }
  minPosition = stepper.currentPosition(); // Réinitialisez la position actuelle
  maxPosition = minPosition + 50000;
  delay(500); // Attendez un peu avant de changer de direction

  stepper.moveTo (0);
  stepper.run();
  // Affichez un message de "home" réussi sur l'écran OLED
  Serial.println("home set");


  delay(2000);
}

void upfn (){
  stepper.setMaxSpeed(MOVEMENT_SPEED);
  stepper.setSpeed(-MOVEMENT_SPEED); // Définir la vitesse positive (CCW)
  while (stepper.currentPosition() < maxPosition) {
    stepper.runSpeed(); // Déplace le moteur dans le sens CCW
  }
}

void downfn (){
  stepper.setMaxSpeed(MOVEMENT_SPEED);
  stepper.setSpeed(MOVEMENT_SPEED); // Définir la vitesse positive (CCW)
  while (digitalRead(LIMIT_SWITCH_DOWN) == HIGH) {
    stepper.runSpeed(); // Déplace le moteur dans le sens CCW
  }
}

void modReglage () {
    // read the current state of the rotary encoder's CLK pin
  CLK_state = digitalRead(CLK_PIN);

  // If the state of CLK is changed, then pulse occurred
  // React to only the rising edge (from LOW to HIGH) to avoid double count
  if (CLK_state != prev_CLK_state && CLK_state == HIGH) {
    // if the DT state is HIGH
    // the encoder is rotating in EncPos-clockwise direction => decrease the EncPos
    if (digitalRead(DT_PIN) == HIGH) {
      EncPos--;
      direction = DIRECTION_CCW;
    } else {
      // the encoder is rotating in clockwise direction => increase the EncPos
      EncPos++;
      direction = DIRECTION_CW;
    }

    Serial.print("DIRECTION: ");
    if (direction == DIRECTION_CW)
      Serial.print("Clockwise");
    else
      Serial.print("EncPos-clockwise");

    Serial.print(" | EncPos: ");
    Serial.println(EncPos);
  }

  // save last CLK state
  prev_CLK_state = CLK_state;
  
  newPosition = Position + EncPos;
  targetPosition = newPosition * STEPS_PER_MM;

}

void modDeplacement () {
  stepper.setSpeed(MOVEMENT_SPEED);
  stepper.moveTo(targetPosition);
  stepper.run();
  
 Position = newPosition;

}