#include <EnableInterrupt.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

#include <elapsedMillis.h>


#include <AccelStepper.h>
#define ENCODER_PIN_A 5
#define ENCODER_PIN_B 6
#define SWITCH_PIN 7

#define ENCODER_DEBOUNCE_DELAY 1
#define SWITCH_DEBOUNCE_DELAY 50

#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 16
#define LCD_ROWS 2

volatile int encoderValue = 0;
volatile bool switchPressed = false;
volatile bool stop = false;
unsigned long esperaTimeExit = 0;

LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

int mainMenuSelection = 0;
int submenuSelection = 1;
int submenuValue = 0;

unsigned long lastEncoderChangeTime = 0;
unsigned long lastSwitchChangeTime = 0;
unsigned long MAX_LENGTH =0;

// Define the Pins used
#define home_switch 12 // Pin 12 connected to Home Switch (MicroSwitch)


// nuevos caracteres
byte cara[8] = {
B00000,
B10001,
B00000,
B00000,
B10001,
B01110,
B00000,
};
byte cuerpo[8] = {
B01110,
B01110,
B00100,
B11111,
B00100,
B01010,
B10001,
B00000,
};

 
//Define analog input pin for ultrasonic sensor
#define sensorin A0

#define estop_switch 11 // Pin 11 connected to Estop

// Stepper Travel Variables
long TravelX;  // Used to store the X value entered in the Serial Monitor
int move_finished=1;  // Used to check if move is completed
long initial_homing=-1;  // Used to Home Stepper at startup

const int maxSpeedLimit = 4000.0;  // set this to the maximum speed you want to use.
const int maxAccelLimit = 600.0;  // set this to the maximum acc you want to use.
elapsedMillis printTime;


    AccelStepper stepper1(AccelStepper::DRIVER, 3, 2);   // Defaults to AccelStepper::driver,pin pulse, pin dir
    // attach() replaces the AccelStepper constructor, so it could also be called like this: 
    // stepper.attach(AccelStepper::DRIVER, 5, 6);

void(* resetFunc) (void) = 0;


void setup() {
  //se lee la longitud maxima configudara
  EEPROM.get(3,MAX_LENGTH);
  pinMode(home_switch, INPUT_PULLUP);//definition home switch must be updated to sensor
  pinMode(estop_switch, INPUT_PULLUP); //definition Estop switch
  pinMode(SWITCH_PIN, INPUT_PULLUP);
  enableInterrupt(ENCODER_PIN_A, readEncoder, CHANGE);
  enableInterrupt(ENCODER_PIN_B, readEncoder, CHANGE);
  enableInterrupt(SWITCH_PIN, switchISR, FALLING);
  Serial.begin(9600);
  lcd.begin(LCD_COLUMNS, LCD_ROWS);
  //crea los caracteres especiales
  lcd.createChar (6,cara);
  lcd.createChar (7,cuerpo);
  //Set Max Speed and Acceleration of each Steppers at startup for homing
  stepper1.setMaxSpeed(200.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepper1.setAcceleration(200.0);  // Set Acceleration of Stepper

  //lcd.backlight();
  //llama a la funcion de display sieca
  InitialDisplay ();

  //llama a la funcion de espera por entrar en configuracion o no
  esperaConfig();

  lcd.clear();
  lcd.setCursor(0, 0);
  
  
  lcd.print("OPERANDO");
  lcd.setCursor(0, 1);
  lcd.print("Sensor--> ");
//inicio de la secuencia de home del stepper con el sensor de home
while (digitalRead(home_switch)) {  // Make the Stepper move CCW until the switch is activated   
  stepper1.moveTo(initial_homing);  // Set the position to move to
  initial_homing--;  // Decrease by 1 for next move if needed
  stepper1.run();  // Start moving the stepper
  delay(5);
}

  stepper1.setCurrentPosition(0);  // Set the current position as zero for now
  stepper1.setMaxSpeed(100.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepper1.setAcceleration(100.0);  // Set Acceleration of Stepper
  initial_homing=1;

  while (!digitalRead(home_switch)) { // Make the Stepper move CW until the switch is deactivated
    stepper1.moveTo(initial_homing);  
    stepper1.run();
    initial_homing++;
    delay(5);
  }
  
  stepper1.setCurrentPosition(0);
  delay(100);
  Serial.println("Homing Completed");
  Serial.println("");
  // Serial.println("Homing Completed");
  // Serial.println("");
  stepper1.setMaxSpeed(maxSpeedLimit);      // Set Max Speed of Stepper (Faster for regular movements)
  stepper1.setAcceleration(maxAccelLimit);  // Set Acceleration of Stepper

}

void loop() {
  //llama la funcion de movimiento
  MotionControl();

}

//funcion de movimiento
void MotionControl() {
  
  float mSpeed;
  int motorSpeed;
  long ActualPos;
  int SensorReadedValue = analogRead(sensorin);
  //Serial.print("Valor leido del sensor");
  //Serial.print("   ");
  //Serial.print(SensorReadedValue);
  //map it to a the maximum speed range
  motorSpeed = map(SensorReadedValue, 0, 1023, maxSpeedLimit, -maxSpeedLimit);
  //Serial.print("   ");
  //Serial.print("motorSpeed");
  //Serial.print("   ");
  //Serial.print(motorSpeed);
  //Serial.print("   ");
  //set the motor speed:
  stepper1.setSpeed(float(motorSpeed));

  mSpeed = stepper1.speed();
  
  //Serial.print("Valor velocidad");
  //Serial.print("   ");
  //Serial.println(mSpeed);
  if (stop){
    
    stepper1.stop();
  }
  else {
    stepper1.run();
    //stepper1.runSpeed();
  }
  ActualPos = map(stepper1.currentPosition(),0,40000,0,300);
    if (ActualPos>=50) {
      stepper1.stop();
      stop = true;
      Serial.print("Mayor a 50  ");
      Serial.print(stop);
    }

  if (printTime >= 2000) { 
   printTime=0;
    
  lcd.setCursor(0, 0);
  
  lcd.print("Sensor---> "+String(float(SensorReadedValue)));
   lcd.setCursor(0, 1);
   lcd.print("Pos -> "+String(float(ActualPos)));
   //lcd.setCursor(0, 1);
   //lcd.print("test");
  }
}

 //funcion de configuracion de parametros al inicio
void configuracion() {
 
 bool exit =false;
 esperaTimeExit=millis();
 while(!exit) {
   Serial.println("en configuacion");
   if (millis()-esperaTimeExit> 15000) { 
    exit = true;
    }
   if (encoderValue != 0 && (millis() - lastEncoderChangeTime) > ENCODER_DEBOUNCE_DELAY) {
    submenuSelection = (submenuSelection + encoderValue) % 5;
    esperaTimeExit=millis();
    displayMainMenu();
    Serial.println(submenuSelection);
    encoderValue = 0;
    lastEncoderChangeTime = millis();
   }

   if (switchPressed && (millis() - lastSwitchChangeTime) > SWITCH_DEBOUNCE_DELAY) {
    switchPressed = false;
    esperaTimeExit=millis();
    enterSubMenu();
   }
  }
}

//funcion que espera por entrar en menu de config durante los primeros 5 segundos con push del boton
void esperaConfig() {
  volatile int contar = 0;
  bool exit = false;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Press button to");
  lcd.setCursor(0, 1);
  lcd.print("Config");
  lcd.setCursor(8, 1);
  while (!exit) {
    
    contar++;
    lcd.print(contar);
    Serial.println(contar);
   if ((contar > 5) || (switchPressed)) {
    Serial.println("exit");
    exit = true;
    
   }
   
   delay(1000);
  }
  //llama a la funcion de configuracion 
  if  (switchPressed) {
    Serial.print("llamando configuracion ");
    switchPressed=false;
   configuracion();
 }
}


// mensaje inicial del display
void InitialDisplay () {
    lcd.noBacklight();
    delay(1000);
    //Encender la luz de fondo.
    lcd.backlight();
    delay(1000);
    lcd.noBacklight();
    delay(1000);
    lcd.backlight();
    // Escribimos el Mensaje en el LCD.
    lcd.setCursor(0, 0);
    lcd.print("www.SIECA.com.gt");
    lcd.setCursor(0, 1);
    lcd.print("Full Automation");
    delay(5000);
    lcd.backlight();
  }


void displayMainMenu() {
  lcd.setCursor(0, 0);
  lcd.print("Main Menu");
  lcd.setCursor(0, 1);
  switch (submenuSelection) {
    case 1:
    lcd.print("              ");
    lcd.setCursor(0, 1);
    lcd.print("> Velocidad");
    break;
    case 2:
    lcd.print("            ");
    lcd.setCursor(0, 1);
    lcd.print("> Aceleracion");
    break;
    case 3:
    lcd.print("            ");
    lcd.setCursor(0, 1);
    lcd.print("> Longitud mm");
    break;
    case 4:
    lcd.print("             ");
    lcd.setCursor(0, 1);
    lcd.print("> Exit");
    break;
    default:
    break;
  } 
  //lcd.print("> Menu ");
  //lcd.print(submenuSelection + 1);

}

void enterSubMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  switch (submenuSelection) {
    case 1:
    lcd.print("Velocidad");
    break;
    case 2:
    lcd.print("Aceleracion");
    break;
    case 3:
    lcd.print("Longitud mm");
    break;
    case 4:
    bool exit = true;
    break;
    default:
    break;
  } 
  lcd.setCursor(0, 1);
  lcd.print("Value: ");

  int oldValue = submenuValue;
  bool valueConfirmed = false;

  while (!valueConfirmed) {
    if (encoderValue != 0 && (millis() - lastEncoderChangeTime) > ENCODER_DEBOUNCE_DELAY) {
      submenuValue += encoderValue;
      displaySubMenuValue();
      encoderValue = 0;
      lastEncoderChangeTime = millis();
    }

    if (switchPressed && (millis() - lastSwitchChangeTime) > SWITCH_DEBOUNCE_DELAY) {
      switchPressed = false;
      valueConfirmed = true;
      EEPROM.update(submenuSelection,submenuValue);
    }
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Confirm:");
  lcd.setCursor(0, 1);
  lcd.print("Value: ");
  lcd.print(submenuValue);

  // Use this section to perform any actions with the confirmed value

  delay(2000); // Display the confirmation for 2 seconds

  submenuValue = oldValue;
  displayMainMenu();
}

void displaySubMenuValue() {
  lcd.setCursor(7, 1);
  lcd.print("     ");
  lcd.setCursor(7, 1);
  lcd.print(submenuValue);
}

void readEncoder() {
  unsigned long currentTime = millis();
  if (currentTime - lastEncoderChangeTime > ENCODER_DEBOUNCE_DELAY) {
    static unsigned int previousState = 0;
    unsigned int currentState = (digitalRead(ENCODER_PIN_A) << 1) | digitalRead(ENCODER_PIN_B);
    if ((previousState == 0b00 && currentState == 0b01) || (previousState == 0b01 && currentState == 0b11) ||
        (previousState == 0b11 && currentState == 0b10) || (previousState == 0b10 && currentState == 0b00)) {
      encoderValue++;
    } else if ((previousState == 0b00 && currentState == 0b10) || (previousState == 0b10 && currentState == 0b11) ||
               (previousState == 0b11 && currentState == 0b01) || (previousState == 0b01 && currentState == 0b00)) {
      encoderValue--;
    }
    previousState = currentState;
    lastEncoderChangeTime = currentTime;
  }
}

void switchISR() {
  unsigned long currentTime = millis();
  if (currentTime - lastSwitchChangeTime > SWITCH_DEBOUNCE_DELAY) {
    if (!switchPressed) {
      switchPressed = true;
      lastSwitchChangeTime = currentTime;
    }
  }
}