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

#define DEBUG_B 0

//Screen
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define HALF_HEIGHT SCREEN_HEIGHT/2
#define HALF_WIDTH SCREEN_WIDTH/2
#define SCREEN_MODO_ANGULO "ANGULO"
#define SCREEN_MODO_DELAY "DELAY"
#define SCREEN_MODO_DELAY "UPDATE"

//button
#define BUTTON_change_state 13
#define BUTTON_DECREASE 15
#define BUTTON_INCREASE 4

#define BUTTON_SELECTED 0

#define STATE_ANGLE 0
#define STATE_TIME 1

unsigned int angle = 90;
int time_between_moviments = 10;
int current_state = STATE_TIME;

TaskHandle_t task_low;

//servo




String sModo;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void vLowTask(void *pvParameters)
{
  while (true)
  {

    int button_state = digitalRead(BUTTON_change_state);
    changeModo(button_state);

    int button_decrease = digitalRead(BUTTON_DECREASE);
    if (button_decrease == BUTTON_SELECTED) {
      (current_state == STATE_ANGLE) ? angle-- : time_between_moviments--;
      vTaskDelay(pdMS_TO_TICKS(500));
    }
    int button_increase = digitalRead(BUTTON_INCREASE);
    if (button_increase == BUTTON_SELECTED) {
      (current_state == STATE_ANGLE) ? angle++ : time_between_moviments++;
      vTaskDelay(pdMS_TO_TICKS(500));
    }

    vTaskDelay(pdMS_TO_TICKS(1));
  }
}

void configButton() {
  pinMode(BUTTON_change_state, INPUT_PULLUP);
  pinMode(BUTTON_DECREASE, INPUT_PULLUP);
  pinMode(BUTTON_INCREASE, INPUT_PULLUP);
}

void configScreen() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  delay(2000);
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(20, 10);
  // Display static text
  display.println("Hello, world!");
  display.display();
}

void setup() {
  Serial.begin(115200);
  configButton();
  configScreen();
  servo.attach(servoPin, 500, 2400);
  xTaskCreate(vLowTask, "vLowTasks", 4096, NULL, 2, &task_low);
}

void changeModo(int bState) {
  //button
  if (bState == BUTTON_SELECTED) {
    int next = (current_state == STATE_TIME) ? STATE_ANGLE : STATE_TIME;
    current_state = next;
  }
  //screen
  sModo = (current_state == STATE_ANGLE) ? SCREEN_MODO_ANGULO : SCREEN_MODO_DELAY;
#if DEBUG_B
  Serial.print("Modo atualizado: ");
  Serial.print(sModo);
  Serial.print("\n");
#endif
}
int waitSetValue(){

}
bool isModo  = false;
void readKeypad(){
  
  char key = '';
  if(!isModo){
    isModo = true;
    switch (key){
    case 'A':
      sModo = SCREEN_MODO_ANGULO;
    break;
    case 'B':
      sModo = SCREEN_MODO_DELAY;
    break;
    case '*':
      sModo = SCREEN_MODO_UPDATE;
    break;
    case '#':
      isModo = false;
    break;
  }else{
    char a = 'a';
    int iKey = (int)key; 
  }
  }
  
}

void drawScreen() {
  display.clearDisplay();
  display.setCursor(2, 2);
  display.print("MODO: "); display.println(sModo);
  display.setCursor(12, HALF_HEIGHT - 10);
  display.print(" ANGULO    DELAY ");
  display.setCursor(20, HALF_HEIGHT + 10);
  display.print(angle);
  display.setCursor(HALF_WIDTH + 20, HALF_HEIGHT + 10);
  display.print(time_between_moviments);
  display.drawRect(2, HALF_HEIGHT, HALF_WIDTH, HALF_HEIGHT - 2, WHITE);
  display.drawRect(HALF_WIDTH, HALF_HEIGHT, HALF_WIDTH, HALF_HEIGHT - 2, WHITE);
  display.display();
}

void moveServo() {
  int pos;
  for (pos = 0; pos <= angle; pos += 1) {
    servo.write(pos);
    delay(time_between_moviments);
  }
  for (pos = angle; pos >= 0; pos -= 1) {
    servo.write(pos);
    delay(time_between_moviments);
  }
}

void loop() {
  readKeypad();

  drawScreen();

  moveServo();

  delay(10);
}