#include <AccelStepper.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Definindo os pinos para o motor de passo e botões
#define DIR_PIN  2        // Pino de direção
#define STEP_PIN 4        // Pino de passo
#define EN_PIN   5        // Pino ENABLE do motor de passo

// Pinos dos botões
#define BUTTON_INCREMENT  12  // B1 - Botão para incrementar posição
#define BUTTON_DECREMENT  13  // B2 - Botão para decrementar posição
#define BUTTON_SELECT     14  // B3 - Botão para selecionar e mover motor

// Pinos do LED RGB
#define RED_PIN    15      // LED Vermelho
#define GREEN_PIN  16      // LED Verde
#define BLUE_PIN   17      // LED Azul

// Pino do LCD (endereço 0x27, 16 colunas, 2 linhas)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Inicializa o motor de passo
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

int targetPosition = 0;  // Posição desejada do motor
int currentPosition = 0; // Posição atual do motor
int incrementStep = 100; // Quantidade de incremento ou decremento por botão pressionado

void setup() {
  // Inicializando comunicação serial
  Serial.begin(115200);
  
  // Definindo os pinos como entrada ou saída
  pinMode(BUTTON_INCREMENT, INPUT_PULLUP);
  pinMode(BUTTON_DECREMENT, INPUT_PULLUP);
  pinMode(BUTTON_SELECT, INPUT_PULLUP);
  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);
  
  // Inicializando o motor de passo
  stepper.setMaxSpeed(1000);   // Velocidade máxima (passos por segundo)
  stepper.setAcceleration(500);  // Aceleração (passos por segundo ao quadrado)

  // Inicializando o display LCD
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.clear();

  // Feedback inicial do sistema
  lcd.setCursor(0, 0);
  lcd.print("Posicao: 0");
  setLEDColor(255, 0, 0);  // LED Vermelho como estado inicial
}

void loop() {
  // Leitura dos botões
  if (digitalRead(BUTTON_INCREMENT) == LOW) {
    targetPosition += incrementStep;  // Aumenta a posição
    updateLCD();
    delay(200);  // Debounce do botão
  }

  if (digitalRead(BUTTON_DECREMENT) == LOW) {
    targetPosition -= incrementStep;  // Diminui a posição
    updateLCD();
    delay(200);  // Debounce do botão
  }

  if (digitalRead(BUTTON_SELECT) == LOW) {
    moveToPosition();  // Mover o motor para a posição desejada
    delay(200);  // Debounce do botão
  }

  // Atualizando a posição do motor
  stepper.run();
  
  // Atualizar o feedback do LED RGB com base na proximidade da posição
  updateLEDFeedback();
}

// Função para mover o motor para a posição alvo
void moveToPosition() {
  stepper.moveTo(targetPosition);  // Definir a posição alvo
  while (stepper.distanceToGo() != 0) {
    stepper.run();  // Mover até a posição
  }
  currentPosition = stepper.currentPosition();  // Atualizar a posição atual
  updateLCD();
}

// Função para atualizar o LCD com a posição atual
void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Posicao: ");
  lcd.print(targetPosition);
  lcd.setCursor(0, 1);
  lcd.print("Atual: ");
  lcd.print(currentPosition);
}

// Função para atualizar o feedback do LED RGB
void updateLEDFeedback() {
  int distance = abs(currentPosition - targetPosition);  // Distância entre a posição atual e a desejada
  
  if (distance == 0) {
    setLEDColor(0, 255, 0);  // Verde (Posição alcançada)
  } else if (distance <= 100) {
    setLEDColor(255, 255, 0);  // Amarelo (Próximo da posição)
  } else {
    setLEDColor(255, 0, 0);  // Vermelho (Distante da posição)
  }
}

// Função para definir a cor do LED RGB
void setLEDColor(int red, int green, int blue) {
  analogWrite(RED_PIN, red);    // Ajusta o LED vermelho
  analogWrite(GREEN_PIN, green); // Ajusta o LED verde
  analogWrite(BLUE_PIN, blue);  // Ajusta o LED azul
}