/*e--minning set rtc with menu */
//--LCD I2C
int h = 0;
int m = 0;
int d = 0;

//---LCD I2C
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

//---Gyroscope
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;

//---RTC Module
#include "RTClib.h"
RTC_DS1307 rtc;
char Day[7][12] = {"Minggu", "Senin", "Selasa", "Rabu", "Kamis", "Jumat", "Sabtu"};
String hari;
int tanggal, bulan, tahun, jam, menit, detik;

int pjam,sjam,pmenit,smenit,pdetik,sdetik;
int ptgl,stgl,pbln,sbln,pth,sth;
char kursor;

//---Button Menu
#define BUP  25
#define BOK  27
#define BDN  29

#define PUSH LOW  //Ditekan
#define BOUN 10   //Bouncing

//---Sensor DHT
#include "DHT.h"
#define DHTPIN 23
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);
float Temp, Hum;

//---Switch (Digital Input)
#define switch1 2  //Netral / WO Netral
#define switch2 3  //Power / Eco
#define switch3 4  //Brake
#define switch4 5  //Loading
#define switch5 6  //Loaded
#define switch6 7  //Dumping
#define switch7 8  //Empty

//---Output
#define relay1 9   //Netral | W/O Netral
#define relay2 10  //Power | Eco

//---Sensor (Analog Input)
#define analog1 A0 //Speed
#define analog2 A1 //Weight
#define analog3 A2 //Front Suspension
#define analog4 A3 //Rear Suspension
#define analog5 A4 //Inclination
#define analog6 A5 //Accel

int speeds = 70; //varieble to change speed 
int an1, an2, an3, an4, an5, an6;
float gyroX, gyroY, gyroZ; //variable to save gyro
bool jalan,printSudut = true;
bool setMode = false;

//---Millis1 (Delay in Millisecond)
unsigned long previousMillis1 = 0;
const long Delay = 500;

//Kondisi
#define ON  LOW  //Kondisi ON

void setup() {
  Serial.begin(115200);
  jalan = false;
  printSudut = false;
  
  // LCD Init
  lcd.init();
  lcd.backlight();

  //start dht and rtc
  dht.begin();
  rtc.begin();
  
  pinMode(BUP, INPUT_PULLUP);
  pinMode(BOK, INPUT_PULLUP);
  pinMode(BDN, INPUT_PULLUP);

  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);

  lcd.setCursor(0, 0); lcd.print("  SMART e-MINNING");  
  lcd.setCursor(0, 2); lcd.print("  PT. KARYA MULTI");
  lcd.setCursor(0, 3); lcd.print(" SOLUTION INDONESIA");
  delay(1000);
  lcd.clear();

  RealTime();
  delay(2000);
  lcd.clear();
  
  // Gyro Init
  while (!mpu.begin(0x69)) 
  {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  // RTC Init
  if (! rtc.begin()) 
  {
  Serial.println("Couldn't find RTC");
  Serial.flush();
  abort();
  }
}
 // Declare sensor event to get readings
sensors_event_t event;

void loop() {
  logic();

  unsigned long currentMillis = millis();
  if(currentMillis - previousMillis1 >= Delay){
  previousMillis1 = currentMillis;

  Analog_Input();
  Digital_Input();
  lcd.setCursor(0,0);  lcd.print("Status:");
  lcd.setCursor(0,1);  lcd.print("Spd:");   lcd.print(an1);   lcd.print("km  "); //Machine Speed
  lcd.setCursor(11,1); lcd.print("Load: "); lcd.print(an2,1); lcd.print("t ");   //Live Weight
  lcd.setCursor(0,2);  lcd.print("Sus:");   lcd.print(an3);   lcd.print("  ");   //Suspension Front
  lcd.setCursor(7,2);  lcd.print("/");      lcd.print(an4);   lcd.print("  ");   //Suspension Rear
  lcd.setCursor(12,2); lcd.print("Inc:");   lcd.print(an5);   lcd.print("% ");   //Inclination
  lcd.setCursor(0,3);  lcd.print("Shift(Ntr/WO)  : ");
  }

  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
      setMenu();
  }
}

void RealTime(){
    pjam=jam/10;
    sjam=jam%10;
    pmenit=menit/10;
    smenit=menit%10;
    pdetik=detik/10;
    sdetik=detik%10;

    ptgl=tanggal/10;
    stgl=tanggal%10;
    pbln=bulan/10;
    sbln=bulan%10;
    pth=tahun/10;
    sth=tahun%10;

    if (setMode) {
      DateTime newTime(tahun+2000 ,bulan,tanggal,jam,menit,detik);
      rtc.adjust(newTime);
      setMode = false;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Waktu diset.");
      delay(1000);
      lcd.clear();
    }

  else{
  DateTime now = rtc.now();

  hari    = Day[now.dayOfTheWeek()];
  tanggal = now.day(),   DEC;
  bulan   = now.month(), DEC;
  tahun   = now.year(),  DEC;
  jam     = now.hour(),  DEC;
  menit   = now.minute(),DEC;
  detik   = now.second(),DEC;

  lcd.setCursor(2,0);
  lcd.print(String() +hari+ ", ");
  lcd.setCursor(9,0);
  lcd.print(String() +tanggal+ "-" +bulan+ "-" +tahun);
  lcd.setCursor(6,1);
  lcd.print(String() +jam+ ":" +menit+ ":" +detik);

  lcd.setCursor(0,2);
  lcd.print("--------------------");

  lcd.setCursor(0,3);
  lcd.print("  SEMANGAT PAGI !!!");}
}

void Analog_Input(){
//---Machine Speed
  int sensor1 = analogRead(analog1);
  an1 = map(sensor1, 0, 1023, 0, speeds);

//---Live Weight
  int sensor2 = analogRead(analog2);
  an2 = map(sensor2, 0, 1023, 0, 90);

//---Suspension Front
  int sensor3 = analogRead(analog3);
  an3 = map(sensor3, 0, 1023, 0, 300);

//---Suspension Rear
  int sensor4 = analogRead(analog4);
  an4 = map(sensor4, 0, 1023, 0, 300);

//---Inclination
  int sensor5 = analogRead(analog5);
  an5 = map(sensor5, 0, 1023, -10, 10);

//---Accel
  int sensor6 = analogRead(analog6);
  an6 = map(sensor6, 0, 1023, 0, 50); 
}

void Digital_Input(){
//---Switch Netral / WO
  if(digitalRead(switch1) == ON){
    lcd.setCursor(17,3); lcd.print("NTR");
    digitalWrite(relay1, HIGH);
  } else {
    lcd.setCursor(17,3); lcd.print("WO ");
    digitalWrite(relay1, LOW);
  }

//---Brake
  if(digitalRead(switch3) == ON){
    lcd.clear();
    lcd.setCursor(0,2); lcd.print(" Brake Activated !!");
    delay(2000);
    lcd.clear();
  }

//---Loading Mode Selector
  if(digitalRead(switch4) == ON){
    lcd.setCursor(8,0); lcd.print("LOADING  ");
  }

//---Loaded Mode Selector
  if(digitalRead(switch5) == ON){
    lcd.setCursor(8,0); lcd.print("LOADED   ");
  }

//---Dumping Mode Selector
  if(digitalRead(switch6) == ON){
    lcd.setCursor(8,0); lcd.print("DUMPING  ");
  }

//---Empty Mode Selector
  if(digitalRead(switch7) == ON){
    lcd.setCursor(8,0); lcd.print("EMPTY    ");
  }
}

void Temperature(){

  unsigned long currentMillis = millis();
  if(currentMillis - previousMillis1 >= Delay){
    previousMillis1 = currentMillis;

  Temp = random(0,50);//dht.readTemperature();
  Hum = random(60,90);//dht.readHumidity();

  lcd.setCursor(0, 0); lcd.print("Temperature Cabin :");
  lcd.setCursor(0, 2); lcd.print("Temperature : "); lcd.print(Temp,1); lcd.print((char)223);lcd.print("C  ");
  lcd.setCursor(0, 3); lcd.print("Humidity    : "); lcd.print(Hum,1); lcd.print("%  ");
  }
}

void setMenu(){

//---Set Menu 1
  setMenu1:
  if(digitalRead(BOK) == PUSH){ //OK || BACK
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU1;
  }
  else if(digitalRead(BUP) == PUSH){ //UP
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setExit;
  }
  else if(digitalRead(BDN) == PUSH){ //DOWN
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu2;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. Date/Time   <-");
    lcd.setCursor(0, 1); lcd.print("02. Cabin Temp.   ");
    lcd.setCursor(0, 2); lcd.print("03. Monitor 2     ");
    lcd.setCursor(0, 3); lcd.print("04. Monitor 3     ");
  goto setMenu1;

//---Set Menu 2
  setMenu2:
  if(digitalRead(BOK) == LOW){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU2;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu1;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu3;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. Date/Time     ");
    lcd.setCursor(0, 1); lcd.print("02. Cabin Temp. <-");
    lcd.setCursor(0, 2); lcd.print("03. Monitor 2     ");
    lcd.setCursor(0, 3); lcd.print("04. Monitor 3     ");
    goto setMenu2;

//---Set Menu 3
  setMenu3:
  if(digitalRead(BOK) == LOW){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU3;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu2;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu4;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. Date/Time     ");
    lcd.setCursor(0, 1); lcd.print("02. Cabin Temp.   ");
    lcd.setCursor(0, 2); lcd.print("03. Monitor 2   <-");
    lcd.setCursor(0, 3); lcd.print("04. Monitor 3     ");
    goto setMenu3;

//---Set Menu 4
  setMenu4:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU4;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu3;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu5;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. Date/Time     ");
    lcd.setCursor(0, 1); lcd.print("02. Cabin Temp.   ");
    lcd.setCursor(0, 2); lcd.print("03. Monitor 2     ");
    lcd.setCursor(0, 3); lcd.print("04. Monitor 3   <-");
    goto setMenu4;

//---Set Menu 5
  setMenu5:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU5;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu4;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu6;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4   <-");
    lcd.setCursor(0, 1); lcd.print("06. Gyro          ");
    lcd.setCursor(0, 2); lcd.print("07. SetTime       ");
    lcd.setCursor(0, 3); lcd.print("08. About         ");
    goto setMenu5;

//---Set Menu 6
  setMenu6:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU6;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu5;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu7;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4     ");
    lcd.setCursor(0, 1); lcd.print("06. Gyro        <-");
    lcd.setCursor(0, 2); lcd.print("07. SetTime       ");
    lcd.setCursor(0, 3); lcd.print("08. About         ");
    goto setMenu6;
    
//--Set Menu 7
  setMenu7:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU7;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu6;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu8;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4     ");
    lcd.setCursor(0, 1); lcd.print("06. Gyro          ");
    lcd.setCursor(0, 2); lcd.print("07. SetTime     <-");
    lcd.setCursor(0, 3); lcd.print("08. About         ");
    goto setMenu7;

//--Set Menu 8
  setMenu8:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU8;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu7;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setExit;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4     ");
    lcd.setCursor(0, 1); lcd.print("06. Gyro          ");
    lcd.setCursor(0, 2); lcd.print("07. SetTime       ");
    lcd.setCursor(0, 3); lcd.print("08. About       <-");
    goto setMenu8;

//---Exit Menu
  setExit:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto endSET;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setMenu8;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu1;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("09. Exit        <-");
  goto setExit;

//----------------------------------------------------------------------------
  //---Set Hari
  setHari:
    if(digitalRead(BOK) == PUSH){ //OK 
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    kursor=10;
    goto setPilihan1;
  }
  else if(digitalRead(BUP) == PUSH){ //UP
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setBack;
  }
  else if(digitalRead(BDN) == PUSH){ //DOWN
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setJam;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. set hari    <-");
    lcd.setCursor(0, 1); lcd.print("02. set jam       ");
    lcd.setCursor(0, 2); lcd.print("03. save time     ");
    lcd.setCursor(0, 3); lcd.print("04. set back      ");
  goto setHari;
  
  //---Set Jam
  setJam:
    if(digitalRead(BOK) == PUSH){ //OK || BACK
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    kursor=6;
    goto setPilihan2;
  }
  else if(digitalRead(BUP) == PUSH){ //UP
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setHari;
  }
  else if(digitalRead(BDN) == PUSH){ //DOWN
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setSaveTime;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. set hari      ");
    lcd.setCursor(0, 1); lcd.print("02. set jam     <-");
    lcd.setCursor(0, 2); lcd.print("03. save time     ");
    lcd.setCursor(0, 3); lcd.print("04. set back      ");
  goto setJam;

  //---Set save time
   setSaveTime:
    if(digitalRead(BOK) == PUSH){ //OK || BACK
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    setMode = !setMode;
    goto setPilihan3;
  }
  else if(digitalRead(BUP) == PUSH){ //UP
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setJam;
  }
  else if(digitalRead(BDN) == PUSH){ //DOWN
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setBack;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. set hari      ");
    lcd.setCursor(0, 1); lcd.print("02. set jam       ");
    lcd.setCursor(0, 2); lcd.print("03. save time   <-");
    lcd.setCursor(0, 3); lcd.print("04. set back      ");
  goto setSaveTime;

  //---Set back
  setBack:
    if(digitalRead(BOK) == PUSH){ //OK || BACK
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu7;
  }
  else if(digitalRead(BUP) == PUSH){ //UP
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setSaveTime;
  }
  else if(digitalRead(BDN) == PUSH){ //DOWN
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setHari;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("01. set hari      ");
    lcd.setCursor(0, 1); lcd.print("02. set jam       ");
    lcd.setCursor(0, 2); lcd.print("03. save time     ");
    lcd.setCursor(0, 3); lcd.print("04. set back    <-");
  goto setBack;

//------------------SET WAKTU--------------------
//---pilihan tanggal/bulan/tahun
  setPilihan1:
    lcd.setCursor(kursor,0);
    kedip(0);

    if(digitalRead(BOK) == PUSH){
      delay(BOUN);
      while(digitalRead(BOK) == PUSH){}
      lcd.clear();
      kanan();
    }
    else if(digitalRead(BUP) == PUSH){
      delay(BOUN);
      while(digitalRead(BUP) == PUSH){}
      lcd.clear();
      up();
    }
    else if(digitalRead(BDN) == PUSH){
      delay(BOUN);
      while(digitalRead(BDN) == PUSH){}
      lcd.clear();
      down();
    }
      logic();
      lcd.setCursor(2,0);
      lcd.print(String() +hari+ ", ");
      lcd.setCursor(9,0);
      lcd.print(String() +ptgl+stgl+ "-" +pbln+sbln+ "-" +pth+sth);
      lcd.setCursor(6,1);
      lcd.print(String() +pjam+sjam+ ":" +pmenit+smenit+ ":" +pdetik+sdetik);
      
  goto setPilihan1;

//---pilihan jam/menit/detik
  setPilihan2:
    lcd.setCursor(kursor,1);
    kedip(1);

    if(digitalRead(BOK) == PUSH){
      delay(BOUN);
      while(digitalRead(BOK) == PUSH){}
      lcd.clear();
      kanan();
    }
    else if(digitalRead(BUP) == PUSH){
      delay(BOUN);
      while(digitalRead(BUP) == PUSH){}
      lcd.clear();
      naik();
    }
    else if(digitalRead(BDN) == PUSH){
      delay(BOUN);
      while(digitalRead(BDN) == PUSH){}
      lcd.clear();
      turun();
    }
      logic();
      lcd.setCursor(2,0);
      lcd.print(String() +hari+ ", ");
      lcd.setCursor(9,0);
      lcd.print(String() +ptgl+stgl+ "-" +pbln+sbln+ "-" +pth+sth);
      lcd.setCursor(6,1);
      lcd.print(String() +pjam+sjam+ ":" +pmenit+smenit+ ":" +pdetik+sdetik);
      
  goto setPilihan2;

//---save waktu manual
setPilihan3:
   if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSaveTime;
  }
  RealTime();
goto setPilihan3;

//------------------SET SUBMENU--------------------
//---Sub Menu 1
  setSUBMENU1:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu1;
  }
  logic();
  RealTime();
  goto setSUBMENU1;

//---Sub Menu 2
  setSUBMENU2:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu2;
  }
  logic();
  Temperature();
  goto setSUBMENU2;

//---Sub Menu 3
  setSUBMENU3:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu3;
  }
  logic();
  lcd.setCursor(0, 1); lcd.print("-> Sub3");

  goto setSUBMENU3;

//---Sub Menu 4
  setSUBMENU4:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu4;
  }
  logic();
  lcd.setCursor(0, 1); lcd.print("-> Sub4");

  goto setSUBMENU4;

//---Sub Menu 5
  setSUBMENU5:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu5;
  }
  logic();
  lcd.setCursor(0, 1); lcd.print("-> Sub5");

  goto setSUBMENU5;

//---Sub Menu 6
  setSUBMENU6:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu6;
  }
  printSudut = true;
  //logic();
  Gyroscope();

  goto setSUBMENU6;

//---Sub Menu 7
  setSUBMENU7:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu7;
  }
  logic();
  goto setHari;
  goto setSUBMENU7;

//---Sub Menu 8
  setSUBMENU8:
  if(digitalRead(BOK) == PUSH){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setMenu8;
  }
  logic();
  lcd.setCursor(0,0);  lcd.print("Model   : "); lcd.print("Trucker.ID");
  lcd.setCursor(0,1);  lcd.print("Version : "); lcd.print("v0.0.1");
  lcd.setCursor(1,3);  lcd.print("Made in Indonesia");

  goto setSUBMENU8;
  
//---Back To Dashboard
  endSET:
  lcd.clear();
  delay(100);
  }

//---turn right cursor
void kanan(){
  kursor++;
  delay(10);
}

void naik() {
 if (kursor==6){
     pjam++;
     if (pjam>2) pjam=0;}
 if (kursor==7){
     sjam++;
     if (sjam>9) sjam=0;}
 if (kursor==9){
     pmenit++;
     if (pmenit>6) pmenit=0;}
 if (kursor==10){
     smenit++;
     if (smenit>9) smenit=0;}
 if (kursor==12){
     pdetik++;
     if (pdetik>6) pdetik=0;}
 if (kursor==13){
     sdetik++;
     if (sdetik>9) sdetik=0;}
}

void up() {
 if (kursor==9){
     ptgl++;
     if (ptgl>3) ptgl=0;}
 if (kursor==10){
     stgl++;
     if (stgl>9) stgl=0;}
 if (kursor==12){
     pbln++;
     if (pbln>1) pbln=0;}
 if (kursor==13){
     sbln++;
     if (sbln>9) sbln=0;}
 if (kursor==15){
     pth++;
     if (pth>9) pth=0;}
 if (kursor==16){
     sth++;
     if (sth>9) sth=0;}
}

void down() {
 if (kursor==9){
     ptgl--;
     if (ptgl<0) ptgl=3;}
 if (kursor==10){
     stgl--;
     if (stgl<0) stgl=9;}
 if (kursor==12){
     pbln--;
     if (pbln<0) pbln=1;}
 if (kursor==13){
     sbln--;
     if (sbln<0) sbln=9;}
 if (kursor==15){
     pth--;
     if (pth<0) pth=9;}
 if (kursor==16){
     sth--;
     if (sth<0) sth=9;}
}

void turun(){
  if (kursor==6){
      pjam--;
      if (pjam<0) pjam=2;}
  if (kursor==7){
      sjam--;
      if (sjam<0) sjam=9;}
  if (kursor==9){
      pmenit--;
      if (pmenit<0) pmenit=6;}
  if (kursor==10){
      smenit--;
      if (smenit<0) smenit=9;}
  if (kursor==12){
      pdetik--;
      if (pdetik<0) pdetik=6;}
  if (kursor==13){
      sdetik--;
      if (sdetik<0) sdetik=9;}
}

void kedip(int letak){
//---kursor kedip
  unsigned long sebelumnya;
  unsigned long sekarang = millis();
  if(sekarang - sebelumnya >= Delay){
    sebelumnya = sekarang;
    lcd.setCursor(kursor,letak);
    lcd.cursor();
    delay(200);
    lcd.noCursor();
    delay(200);
  }
}

void logic(){
  printSudut = false;
  Gyroscope();

  //---Gyro and max weight
  if((an2 >= 90) && (gyroX >= 15.00)){
    speeds = 70;// 100% of speeds
    jalan = false;
    digitalWrite(relay2, HIGH);// POWER IS ON
  }

  //---max weight and gyro (x,y) = 0
  if((an2 >= 90) && ((gyroX == 0.00) && (gyroY == 0.00))){
    speeds = 60;// 85% of 70km/h
    jalan = true;
    digitalWrite(relay2, LOW);// ECO IS ON
    lcd.clear();
    lcd.setCursor(3,0); lcd.print("MUATAN OVERLOAD");
    lcd.setCursor(3,1); lcd.print("KURANGI MUATAN");
    lcd.setCursor(3,2); lcd.print("LOADING  ");
    lcd.setCursor(3,3); lcd.print("DUMPING  ");
    delay(1000);
    lcd.clear();
  }

  //---weightless 
  if(an2 == 0){
    jalan = true;
    digitalWrite(relay2, LOW);// ECO IS ON
  } 
  
  //---Max Speed
  if(an1 >= 65){
    lcd.clear();
    lcd.setCursor(5,1); lcd.print("OVER SPEED");
    lcd.setCursor(2,2); lcd.print("KURANGI KECEPATAN");
    delay(1000);
    lcd.clear();
  }

  //---Gyro

  //---Power / Eco
  if(jalan){
    if(digitalRead(switch2) == ON){
      digitalWrite(relay2, HIGH);
    } else {
      digitalWrite(relay2, LOW);
    }
  }
  //---Eco

  //---Brake
  
  //---Temperature
  if(Temp >= 50){
    lcd.clear();
    lcd.setCursor(3,0); lcd.print("WARNING !!!");
    lcd.setCursor(3,1); lcd.print("OVERHEAT");
    lcd.setCursor(0,2); lcd.print("-------------------");
    lcd.setCursor(0,3); lcd.print("CABIN TERLALU PANAS");
    delay(1000);
    lcd.clear();
  }
  Analog_Input();
  Serial.println(speeds);
}

void Gyroscope() {
  //--Read MPU6050
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Konversi kecepatan sudut gyroscope ke derajat/s
  gyroX = g.gyro.x * 180.0 / PI;
  gyroY = g.gyro.y * 180.0 / PI;
  gyroZ = g.gyro.z * 180.0 / PI;

  if(printSudut){
  lcd.setCursor(0, 0);lcd.print("Kemiringan : ");
  lcd.setCursor(0, 1);lcd.print("sumbu x : ");
  lcd.setCursor(11, 1);lcd.print(gyroX,2);
  lcd.setCursor(0, 2);lcd.print("sumbu y : ");
  lcd.setCursor(11, 2);lcd.print(gyroY,2);
  lcd.setCursor(0, 3);lcd.print("sumbu z : ");
  lcd.setCursor(11, 3);lcd.print(gyroZ,2);
  }
}