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

//definição de pinos
#define STEP_PIN 33
#define DIR_PIN  32
#define ENABLE_PIN 25
#define LED_VERMELHO 15
#define LED_VERDE 16
#define LED_AZUL 17
#define B1 2
#define B2 4
#define B3 5



// Cria um objeto AccelStepper
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Cria um objeto para o lcd
LiquidCrystal_I2C lcd(0x27, 16, 2);

//declaração de variaveis
int pos = 0, pos_set = 0, angle_step = 0;
int t1 = 0, t2 = 0;
byte Graus[] = { 
  B00000, 
  B00111, 
  B00101, 
  B00111, 
  B00000, 
  B00000, 
  B00000, 
  B00000 
}; 
bool set1 = 0, set2 = 0, set3 = 0, time_set = 0;
int s1 = 0, s2 = 0;
int pos_set_m = 0;
bool tr = 0;

void setup() {
  // Configura os pinos
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(B1, INPUT);
  pinMode(B2, INPUT);
  pinMode(B3, INPUT);
  digitalWrite(ENABLE_PIN, LOW); 
    
 

  // Inicializa a biblioteca AccelStepper
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(1000); 
  stepper.setAcceleration(500); 

  // Inicializa o lcd
  lcd.init();
  lcd.backlight();
  lcd.createChar(0, Graus); 

  lcd.setCursor(0,0);
  lcd.print("  Gabriel Luz  ");
  lcd.setCursor(0,1);
  lcd.print("TEC.MECATRONICA");
  RGB(0,0,255);
  delay(1000);
  RGB(0,255,0);
  delay(1000);
  RGB(0,0,255);
  delay(1000);

  //prepara LCD 
  lcd.clear();
  lcd.setCursor(0,1);
  lcd.print("Pos atual: ");
  lcd.print(pos_set);
  lcd.write(0);
  lcd.print("              ");
  lcd.setCursor(0,0);
  lcd.print("Motor PRONTO      ");
  RGB(0,255,0);
}


void loop() {

  botoes();
  
  angle_step = map(pos_set, -180, 180, -100, 100);
  // Move o motor para a nova posição
  
  
  if(pos_set != pos_set_m){
    lcd.setCursor(0,0);
    lcd.print("Motor em OP.  ");
    RGB(255,255,0);
    pos_set_m = pos_set;
    stepper.moveTo(angle_step);
    stepper.runToPosition();
    lcd.setCursor(0,0);
    lcd.print("Motor PRONTO       ");
    RGB(0,255,0);
  }
  
  delay(10);

}
void botoes(){
  if(digitalRead(B1) == 1){
    pos++;
    if(pos > 180)pos = -180;

    if(set1 == 0){
      lcd.setCursor(0,1);
      lcd.print("Sel pos: ");
      lcd.setCursor(0,0);
      lcd.print("Motor em ");
      lcd.print(pos_set);
      lcd.write(0);
      lcd.print("   ");

      RGB(255,0,0);
      
    }

    set1 = 1;
    delay(50);
    lcd.setCursor(9,1);
    lcd.print(pos);
    lcd.write(0);
    lcd.print("   ");
  }
  if(digitalRead(B2) == 1){
    pos--;
    if(pos < -180)pos = 180;

    if(set2 == 0){
      lcd.setCursor(0,1);
      lcd.print("Sel pos: ");
      lcd.setCursor(0,0);
      lcd.print("Motor em ");
      lcd.print(pos_set);
      lcd.write(0);
      lcd.print("   ");

      RGB(255,0,0);
      
    }
    set2 = 1;
    delay(50);
    lcd.setCursor(9,1);
    lcd.print(pos);
    lcd.write(0);
    lcd.print("   ");
  }
  if(digitalRead(B3) == 1 && set3 == 0){
    
    set3 = 1;
    pos_set = pos;
    time_set = 1;
    lcd.setCursor(0,1);
    lcd.print("Movendo: ");
    lcd.print(pos_set);
    lcd.write(0);
    lcd.print("              ");
    
  }
  if(digitalRead(B1) == 0 && set1 == 1)set1 = 0;
  if(digitalRead(B2) == 0 && set2 == 1)set2 = 0;
  if(digitalRead(B3) == 0 && set3 == 1)set3 = 0;
  
  if(time_set == 1){
    t1++;
    t2++;
    if(t2 < 5 && t1 > 1){
      lcd.setCursor(0,1);
      lcd.print("Pos atual: ");
      lcd.print(pos_set);
      lcd.write(0);
      lcd.print("              ");
      RGB(0,255,0);
    } else if (t1 > 1){
      lcd.setCursor(0,1);
      lcd.print("           ");
      if(t2 == 10)t2 = 0;
    }
  }
 
  if(t1 == 31){
    t1 = 0;
    time_set = 0;
    lcd.setCursor(0,1);
    lcd.print("Pos atual: ");
    lcd.print(pos_set);
    lcd.write(0);
    lcd.print("              ");
  }


}
void RGB(int r, int g, int b){
  analogWrite(LED_VERMELHO, r);
  analogWrite(LED_VERDE, g);
  analogWrite(LED_AZUL, b);
}