//ROTOMOLDER
//VERSION 1.0

//LIBRARIES
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Acc

#define OLED_RESET    -1
#define SCREEN_WIDTH 128 // OLED display width
#define SCREEN_HEIGHT 64 // OLED display height

//SWITCH PINS
#define BUTTON_BACK   27
#define BUTTON_FORWARD 28
#define BUTTON_SELECT  29

//MOTOR 1 PARAMETERS
#define DIR 3
#define STEP 4
#define NSTEPS 1600
#define DELAYHIGH 10
#define DELAYLOW 1000 

//MOTOR 2 PARAMETERS
#define DIR_2 2
#define STEP_2 1
#define NSTEPS_2 1600
#define DELAYHIGH_2 10
#define DELAYLOW_2 1000 

//OLED INITIALIZATION
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//MENU VARIABLES
int menuIndex = 0;
int speedValues[] = {20, 40, 60, 80, 100};
int timeValues[] = {1, 2, 3, 4, 5};
int selectedSpeed = 0;
int selectedTime = 0;
bool inSpeedMenu = true; // Para controlar si estamos seleccionando velocidad o tiempo
bool inDecisionMenu = false; // Nuevo estado para el menú de decisión
const char* options[] = {"START", "BACK"};

void setup() {
//SWITCH PINS
  pinMode(BUTTON_BACK, INPUT);
  pinMode(BUTTON_FORWARD, INPUT);
  pinMode(BUTTON_SELECT, INPUT);

//MOTOR1 PINS
  pinMode(STEP,OUTPUT);
  digitalWrite(STEP,LOW);
  pinMode(DIR,OUTPUT); 
  digitalWrite(DIR,LOW);

//MOTOR2 PINS
  pinMode(STEP_2,OUTPUT);
  digitalWrite(STEP_2,LOW);
  pinMode(DIR_2,OUTPUT); 
  digitalWrite(DIR_2,LOW);  

//OLED INITIALIZATION
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); //OLED I2C DRECTON
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);

}

void loop() {
  // Detectar pulsaciones de botones
  if (digitalRead(BUTTON_BACK)) {
    menuIndex--;
    if (inSpeedMenu) {
      if (menuIndex < 0) {
        
        //menuIndex = sizeof(speedValues) / sizeof(speedValues[0]) - 1;
        menuIndex = std::size(speedValues) - 1;
      }
    } else if (!inDecisionMenu) {
      if (menuIndex < 0) {
        //menuIndex = sizeof(timeValues) / sizeof(timeValues[0]) - 1;
        menuIndex = std::size(timeValues) - 1;
      }
    } else {
      if (menuIndex < 0) {
        menuIndex = 1; // Solo hay dos opciones en el menú de decisión
      }
    }
    delay(200); // Debouncing
  }

  if (digitalRead(BUTTON_FORWARD)) {
    menuIndex++;
    if (inSpeedMenu) {
      if (menuIndex >= std::size(speedValues) ) {
        menuIndex = 0;
      }
    } else if (!inDecisionMenu) {
      if (menuIndex >= std::size(timeValues) ) {
        menuIndex = 0;
      }
    } else {
      if (menuIndex > 1) { // Solo hay dos opciones en el menú de decisión
        menuIndex = 0;
      }
    }
    delay(200); // Debouncing
  }
  if (digitalRead(BUTTON_SELECT)) {
    if (inSpeedMenu) {
      selectedSpeed = speedValues[menuIndex];
      inSpeedMenu = false;
      menuIndex = 0;
    } else if (!inDecisionMenu) {
      selectedTime = timeValues[menuIndex];
      inDecisionMenu = true; // Cambia al menú de decisión
      menuIndex = 0;
    } else {
      if (menuIndex % 2 == 0) {

        
         //MOTOR 2 START
        digitalWrite(DIR_2,HIGH);
        for (int i = 0; i < NSTEPS_2; ++i) {
        digitalWrite(STEP_2,HIGH);
        delayMicroseconds(DELAYHIGH_2);
        digitalWrite(STEP_2,LOW);
        delayMicroseconds(DELAYLOW_2);
        }


        //MOTOR 1 START
        int stepper_delay=6000-selectedSpeed*50;

        unsigned long startTime = millis();
        unsigned long duration = selectedTime*60*1000;

        while ((millis() - startTime) < duration) { 
        digitalWrite(DIR,HIGH);
        //for (int i = 0; i < NSTEPS; ++i) {
        digitalWrite(STEP,HIGH);
        delayMicroseconds(DELAYHIGH);
        digitalWrite(STEP,LOW);
        delayMicroseconds(stepper_delay);
        //}
        }

        //MOTOR 2 START
        digitalWrite(DIR_2,LOW);
        for (int i = 0; i < NSTEPS_2; ++i) {
        digitalWrite(STEP_2,HIGH);
        delayMicroseconds(DELAYHIGH_2);
        digitalWrite(STEP_2,LOW);
        delayMicroseconds(DELAYLOW_2);
        }

        inSpeedMenu = true;
        inDecisionMenu = false;
        menuIndex = 0;

      } else {
        
        inSpeedMenu = true;
        inDecisionMenu = false;
        menuIndex = 0;
      }
    }
    delay(200);
  }

  // Actualizar el menú en la pantalla
  display.clearDisplay();
  display.setCursor(0,0);

  if (inSpeedMenu) {
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("Select Speed:");

    display.setTextSize(2);
    display.setCursor(0, 28);
    display.print("> ");
    display.print(speedValues[menuIndex]);

  } else if (!inDecisionMenu) {
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("Select Time:");

    display.setTextSize(2);
    display.setCursor(0, 28);
    display.print("> ");
    display.print(timeValues[menuIndex]);
    display.print(" mins");

  } else {
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.println("Confirm:");

    display.setTextSize(2);
    display.setCursor(0, 28);
    display.print("> ");
    display.println(options[menuIndex % 2]);
  }
  display.display();
}