#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <BluetoothSerial.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enable! Please run 'make menuconfig' and enable it
#endif

#define ANCHO_PANTALLA 128
#define ALTO_PANTALLA 64
#define pinServo 32
#define pinSalida 33
#define pinEntrada 19

Adafruit_SSD1306 pantalla(ANCHO_PANTALLA, ALTO_PANTALLA, &Wire, -1);
Servo servo;
int current_position_servo = 90;
int n_cars = 0;
boolean delay_time = false;
boolean last_up = true;
boolean salida_activada = false;
boolean entrada_activada = false;
String last_signal = "salida";
BluetoothSerial ESP32_BT;


static const unsigned char PROGMEM datos_imagen[406] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x0f, 0xff, 0xff, 0xff, 0xff, 0xac, 0x00, 
    0x1f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 
    0x0c, 0x5f, 0xfb, 0xfd, 0xdd, 0xde, 0x00, 
    0x1c, 0xd9, 0x9b, 0xf5, 0x99, 0x86, 0x00, 
    0x0c, 0x5d, 0x99, 0xed, 0xd9, 0x8e, 0x00, 
    0x1c, 0x5c, 0xb3, 0xe4, 0x99, 0x8e, 0x00, 
    0x0c, 0xdc, 0xd9, 0xed, 0x99, 0x8e, 0x00, 
    0x1e, 0x5c, 0xd9, 0xe4, 0xd9, 0x8e, 0x00, 
    0x0c, 0x5c, 0xf9, 0xed, 0x99, 0x8e, 0x00, 
    0x1c, 0xdc, 0xf3, 0xff, 0x99, 0xce, 0x00, 
    0x1c, 0x5c, 0x79, 0xff, 0x99, 0x8e, 0x00, 
    0x0c, 0xdc, 0x7b, 0xe6, 0x9b, 0x8e, 0x00, 
    0x1c, 0x5c, 0x7f, 0xe6, 0x19, 0x8e, 0x00, 
    0x0e, 0xcc, 0x3f, 0xe6, 0x1d, 0x8e, 0x00, 
    0x1c, 0x6c, 0x37, 0xe2, 0x19, 0x8e, 0x00, 
    0x1c, 0xcc, 0x39, 0xe6, 0x1d, 0x8e, 0x00, 
    0x0c, 0x4c, 0x33, 0xe6, 0x3b, 0x8e, 0x00, 
    0x1c, 0xee, 0x39, 0xe2, 0x1f, 0x8e, 0x00, 
    0x1c, 0xec, 0x33, 0xe6, 0x7f, 0x1e, 0x00, 
    0x1e, 0x7e, 0x3b, 0xe7, 0xfc, 0x1e, 0x00, 
    0x1e, 0x1f, 0xfb, 0xff, 0xf8, 0x3e, 0x00, 
    0x0f, 0x0f, 0xf3, 0xff, 0x18, 0x7e, 0x00, 
    0x1f, 0x87, 0xff, 0xf8, 0x1c, 0xee, 0x00, 
    0x1f, 0xe7, 0x3f, 0xe0, 0x1f, 0xde, 0x00, 
    0x1d, 0xe6, 0x0f, 0xe7, 0xff, 0xce, 0x00, 
    0x1c, 0xff, 0x03, 0xff, 0xf8, 0x0e, 0x00, 
    0x1c, 0x7f, 0x01, 0xff, 0x40, 0x0e, 0x00, 
    0x0e, 0x1f, 0xf9, 0xe0, 0x00, 0x0e, 0x00, 
    0x1c, 0x0f, 0xff, 0xe0, 0x00, 0x0e, 0x00, 
    0x1c, 0x00, 0xbf, 0xe0, 0x00, 0x0e, 0x00, 
    0x0e, 0x00, 0x03, 0xe0, 0x00, 0x0e, 0x00, 
    0x1c, 0x00, 0x01, 0xe0, 0x00, 0x0e, 0x00, 
    0x1e, 0x00, 0x01, 0xe0, 0x00, 0x0e, 0x00, 
    0x0c, 0x00, 0x03, 0xe0, 0x00, 0x0c, 0x00, 
    0x0f, 0x00, 0x01, 0xe0, 0x00, 0x1c, 0x00, 
    0x0f, 0x80, 0x01, 0xe0, 0x00, 0x7c, 0x00, 
    0x07, 0xe0, 0x03, 0xe0, 0x00, 0xf8, 0x00, 
    0x01, 0xf8, 0x01, 0xe0, 0x03, 0xe0, 0x00, 
    0x00, 0xfc, 0x01, 0xe0, 0x0f, 0xc0, 0x00, 
    0x00, 0x3f, 0x03, 0xe0, 0x1f, 0x00, 0x00, 
    0x00, 0x1f, 0xc1, 0xe0, 0x7e, 0x00, 0x00, 
    0x00, 0x07, 0xe3, 0xe1, 0xf8, 0x00, 0x00, 
    0x00, 0x01, 0xf9, 0xe3, 0xe0, 0x00, 0x00, 
    0x00, 0x00, 0x7f, 0xef, 0x80, 0x00, 0x00, 
    0x00, 0x00, 0x1f, 0xfe, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x07, 0xf8, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x03, 0xf0, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

void setup() {
  Serial.begin(9600);
  while(!Serial);
  pinMode(pinServo, OUTPUT);
  pinMode(pinEntrada, INPUT);
  pinMode(pinSalida, INPUT);

  Serial.println("Inicializando ESP32!");
  ESP32_BT.begin("ESP32 Santiago Vivas");
  Serial.print("La tarjeta está lista para emparejarse.");
  servo.attach(pinServo, 500, 2500);

  randomSeed(analogRead(0));

  if (!pantalla.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Falló al incializar la pantalla OLED"));
    while (true);
  }

  delay(200);
  init_setup_oled(pantalla);
}

void loop() {
  delay(5);
  if(ESP32_BT.available()){
    int inData = ESP32_BT.read();
  }

  control_servo(digitalRead(pinEntrada), digitalRead(pinSalida));
  display_servo_state(current_position_servo);
}

void init_setup_oled(Adafruit_SSD1306 p) {
  p.clearDisplay();
  p.setTextSize(1);
  p.setTextColor(SSD1306_WHITE);
  p.setCursor(0, 0);
  pantalla.println("Iniciando pantalla");
  p.display();
}

void control_servo(bool signalEntrada, bool signalSalida) {
  if (signalEntrada || (inData == 64)) {
    if (current_position_servo < 90) {
      current_position_servo++;
    } else {
      last_signal = "entrada";
    }
    entrada_activada = true;
    delay_time = true;
  } else if (signalSalida || (inData == 97)){
    if (current_position_servo < 90) {
      current_position_servo++;
    } else {
      last_signal = "salida";
    }
    delay_time = true;
    salida_activada = true;
  } else if (current_position_servo > 0) {
    if (delay_time){
      delay(1400);
    }
    current_position_servo--;
    delay_time = false;
  }
  servo.write(current_position_servo);
}

void display_servo_state(int position_servo) {
  if (position_servo == 0 && last_up) {
    if (entrada_activada && salida_activada){
      if (last_signal == "entrada"){
       n_cars--;
      } else if (last_signal == "salida"){
        n_cars++;
      }
      entrada_activada = false;
      salida_activada = false;
    }
    pantalla.setTextColor(SSD1306_WHITE);
    pantalla.clearDisplay();
    pantalla.setCursor(0, 0);
    pantalla.println("autos: " + String(n_cars));
    pantalla.println("Barricada abajo");
    pantalla.drawBitmap(40, 10, datos_imagen, 50, 58, SSD1306_WHITE);
    pantalla.display();
    last_up = !last_up;
  } else if (position_servo == 90 && !last_up) {
    pantalla.setTextColor(SSD1306_WHITE);
    pantalla.clearDisplay();
    pantalla.setCursor(0, 0);
    pantalla.println("autos: " + String(n_cars));
    pantalla.println("Barricada arriba");
    pantalla.drawBitmap(40, 10, datos_imagen, 50, 58, SSD1306_WHITE);
    pantalla.display();
    last_up = !last_up;
  }
}