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

Adafruit_SSD1306 lcd(128, 64, &Wire, -1);

#define pinSW_UP 2
#define pinSW_DW 12
#define pinSW_PL 4
#define pinSW_MN 13

#define SW_UP !digitalRead(pinSW_UP)
#define SW_DW !digitalRead(pinSW_DW)
#define SW_PL !digitalRead(pinSW_PL)
#define SW_MN !digitalRead(pinSW_MN)

int8_t nilai1 = 0;
int8_t nilai2 = 0;
int8_t nilai3 = 0;
int8_t nilai4 = 0;

int8_t cursor = 0;
int8_t minMenu = 0, maxMenu = 5;

int8_t plan = 0;

struct eepromConfig {
  int8_t sensorPlan[99];
  int8_t aksiPlan[99];
  int8_t delayPlan[99];
  int8_t timerPlan[99];
  int8_t speedPlan[99];
}; eepromConfig EEc;

String strAksi[6] = {
  "FINISH",
  "KANAN",
  "KIRI",
  "MAJU",
  "MUNDUR",
  "TABRAK",
};

void setup() { 
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  
  if(!lcd.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  pinMode(pinSW_UP, INPUT_PULLUP);
  pinMode(pinSW_DW, INPUT_PULLUP);
  pinMode(pinSW_PL, INPUT_PULLUP);
  pinMode(pinSW_MN, INPUT_PULLUP);

  lcd.setTextSize(1);
  lcd.setTextColor(SSD1306_WHITE);
}

void loop() { 
  lcd.clearDisplay(); 

  lcd.setCursor(0, cursor * 10);
  lcd.print(">"); 
  
  lcd.setCursor(10, 0);
  lcd.print("Plan   :"); 
  lcd.print(plan); 

  lcd.setCursor(10, 10);
  lcd.print("Sensor :"); 
  lcd.print(EEc.sensorPlan[plan]); 
  
  lcd.setCursor(10, 20);
  lcd.print("Aksi   :"); 
  lcd.print(strAksi[EEc.aksiPlan[plan]]); 
  
  lcd.setCursor(10, 30);
  lcd.print("Delay  :"); 
  lcd.print(EEc.delayPlan[plan]); 
  lcd.print(" mS"); 
  
  lcd.setCursor(10, 40);
  lcd.print("Timer  :"); 
  lcd.print(EEc.timerPlan[plan]); 
  lcd.print(" mS"); 

  lcd.setCursor(10, 50);
  lcd.print("Speed  :"); 
  lcd.print(EEc.speedPlan[plan]); 
 

  lcd.display();

  if(SW_PL && cursor == 0){
    plan++;
    if(plan > 99) {plan = 99;}
  } if(SW_MN && cursor == 0){
    plan--;
    if(plan < 0) {plan = 0;}
  }

  if(SW_PL && cursor == 1){
    EEc.sensorPlan[plan]++;  
  } if(SW_MN && cursor == 1){
    EEc.sensorPlan[plan]--;
  }

  if(SW_PL && cursor == 2){
    EEc.aksiPlan[plan]++;
  } if(SW_MN && cursor == 2){
    EEc.aksiPlan[plan]--;
  }

  if(SW_PL && cursor == 3){
    EEc.delayPlan[plan]++;
  } if(SW_MN && cursor == 3){
    EEc.delayPlan[plan]--;
  }

  if(SW_PL && cursor == 4){
    EEc.timerPlan[plan]++;
  } if(SW_MN && cursor == 4){
    EEc.timerPlan[plan]--;
  } 

  if(SW_PL && cursor == 5){
    EEc.speedPlan[plan]++;
  } if(SW_MN && cursor == 5){
    EEc.speedPlan[plan]--;
  }

  if(SW_DW){
    cursor++;
    if(cursor > maxMenu) {cursor = maxMenu;}
    delay(100);
  }
  if(SW_UP){
    cursor--;
    if(cursor < minMenu) {cursor = minMenu;}
    delay(100);
  }


  // Serial.println("UP:" + String(SW_UP) + " DW:" + String(SW_DW) + " EN:" + String(SW_EN) + " BC:" + String(SW_BC));
  // delay(100);  
}

void setCursor(int x, int y){

}

void scanGaris(int scan){
  while(1){
    // selama belum detek tanda robot jalan ikut garis
  }
}

void aksi(int aksiPlan, int delayAksi){
  if(aksiPlan == 1){ //belok kanan
    // moror(100, -100);
  }
  if(aksiPlan == 2){ //belok kiri
    // moror(-100, 100);
  }
  delay(delayAksi);
}

void timer(int timer, int speed){
  uint64_t acuanTimer = millis();
  while(1){
    if(millis() - acuanTimer > timer){
      break;
    }
    // selama timer belum selesai ikut garis dengan speed
  }
}


void runRobot(int planCP){
  int runPlan = planCP;
  while(1){
    scanGaris(EEc.sensorPlan[runPlan]); 
    aksi(EEc.aksiPlan[runPlan], EEc.delayPlan[runPlan]);
    timer(EEc.timerPlan[runPlan], EEc.speedPlan[runPlan]); 

    runPlan++; 
    if(runPlan > 90 || EEc.aksiPlan == 0){
      // stop();
      break;
    }
  }
}