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

#define SV1 23
#define SV2 4
#define SV3 16
#define SV4 17
#define SV5 5
#define SV6 18
#define B1 2
#define B2 19
#define RGB_S0 27
#define RGB_S1 26
#define RGB_S2 25
#define RGB_S3 33
#define EN 14
#define sig 32
#define pot 34

Servo servoMotor;
const int servo_in = 13;
const int servo_out = 12;

int8_t rgb_set[16]{
  0b00000000,
  0b00000001,
  0b00000010,
  0b00000011,
  0b00000100,
  0b00000101,
  0b00000110,
  0b00000111,
  0b00001000,
  0b00001001,
  0b00001010,
  0b00001011,
  0b00001100,
  0b00001101,
  0b00001110,
  0b00001111
};


byte ct1[] = { 
  B01110, 
  B11011, 
  B10001, 
  B10001, 
  B10001, 
  B10001, 
  B10001, 
  B11111 
}; 

LiquidCrystal_I2C LCD((0x27), 20, 4);


int teste = 0, vsp = 0, tempo = 0, tempo1 = 0;

bool set = 1;
bool ss1 = 0;
bool ss2 = 0;
bool ss3 = 0;
bool ss4 = 0;
bool ss5 = 0;
bool ss6 = 0;



void setup() {
  servoMotor.attach(servo_in);
  servoMotor.attach(servo_out);
  Serial.begin(115200);

  LCD.init();
  LCD.backlight();
  LCD.createChar(0, ct1); 

  pinMode(SV1, INPUT);
  pinMode(SV2, INPUT);
  pinMode(SV3, INPUT);
  pinMode(SV4, INPUT);
  pinMode(SV5, INPUT);
  pinMode(SV6, INPUT);
  pinMode(B1, INPUT);
  pinMode(B2, INPUT);
  pinMode(EN, OUTPUT);
  pinMode(RGB_S0, OUTPUT);
  pinMode(RGB_S2, OUTPUT);
  pinMode(RGB_S3, OUTPUT);
  pinMode(RGB_S1, OUTPUT);
  pinMode(sig, OUTPUT);
}

void loop() {

  vsp = map(analogRead(pot), 0, 4095, 0, 3);

  
  delay(5);
  digitalWrite(sig, 0);
  digitalWrite(EN, 0);

  
  if(tempo >= 1){
    vaga1();
    vaga2();
    vaga3();
    vaga4();
    vaga5();
    vaga6();
    
    tempo = 0;
  }
  delay(1);
  digitalWrite(EN, 0);
  RGB(teste);
  teste++;
  if(teste >= 16)teste = 0;
  tempo++;

}
void RGB(int V){
  // led vaga 1
  if(teste == 0 && ss1 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 1 && ss1 == 0 && vsp <= 0){
    digitalWrite(sig, 1);
  }
  if(teste == 2 && ss1 == 0 && vsp >= 1){
    digitalWrite(sig, 1);
  }

  // led vaga 2
  if(teste == 3 && ss2 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 4 && ss2 == 0 && vsp <= 1){
    digitalWrite(sig, 1);
  }
  if(teste == 5 && ss2 == 0 && vsp >= 2){
    digitalWrite(sig, 1);
  }

  //led vaga 3
  if(teste == 6 && ss3 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 7 && ss3 == 0 && vsp <= 2){
    digitalWrite(sig, 1);
  }
  if(teste == 8 && ss3 == 0 && vsp >= 3){
    digitalWrite(sig, 1);
  }

  //led vaga 4
  if(teste == 9 && ss4 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 10 && ss4 == 0){
    digitalWrite(sig, 1);
  }

  //led vaga 5
  if(teste == 11 && ss5 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 12 && ss5 == 0){
    digitalWrite(sig, 1);
  } 

  //led vaga 6
  if(teste == 13 && ss6 == 1){
    digitalWrite(sig, 1);
  }
  if(teste == 14 && ss6 == 0){
    digitalWrite(sig, 1);
  } 

  digitalWrite(RGB_S0, rgb_set[V] & 0b00000001);
  digitalWrite(RGB_S1, rgb_set[V] & 0b00000010);
  digitalWrite(RGB_S2, rgb_set[V] & 0b00000100);
  digitalWrite(RGB_S3, rgb_set[V] & 0b00001000);
}
void vaga1 (){
  if(digitalRead(SV1) == 1 && ss1 == 0)ss1 = 1;
  if(digitalRead(SV1) == 0 && ss1 == 1)ss1 = 0;
}
void vaga2 (){
  if(digitalRead(SV2) == 1 && ss2 == 0)ss2 = 1;
  if(digitalRead(SV2) == 0 && ss2 == 1)ss2 = 0;
}
void vaga3 (){
  if(digitalRead(SV3) == 1 && ss3 == 0)ss3 = 1;
  if(digitalRead(SV3) == 0 && ss3 == 1)ss3 = 0;
}
void vaga4 (){
  if(digitalRead(SV4) == 1 && ss4 == 0)ss4 = 1;
  if(digitalRead(SV4) == 0 && ss4 == 1)ss4 = 0;
}
void vaga5 (){
  if(digitalRead(SV5) == 1 && ss5 == 0)ss5 = 1;
  if(digitalRead(SV5) == 0 && ss5 == 1)ss5 = 0;
}
void vaga6 (){
  if(digitalRead(SV6) == 1 && ss6 == 0)ss6 = 1;
  if(digitalRead(SV6) == 0 && ss6 == 1)ss6 = 0;
}