/* Smart e-Minner ATmega328P */
//---MPU6050
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;

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

//---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;

//---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 DHT21
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
bool jalan = true; 
float gyroX;
float gyroY;
float gyroZ;

int an1, an2, an3, an4, an5, an6;

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

//Kondisi
#define ON  LOW  //KOndisi ON

void setup() {
  Serial.begin(9600);
  jalan = false;
  Wire.begin();
  
  mpu.initialize();
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  
  lcd.init();
  lcd.backlight();

  dht.begin();

  if (! rtc.begin()) {
    Serial.println("RTC TIDAK TERBACA");
    while (1);
  }
  if (! rtc.isrunning()) {
    Serial.println("RTC is NOT running!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));//update rtc dari waktu komputer
  }

  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();
}

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(){
    lcd.setCursor(0, 0);
    DateTime now = rtc.now();
    lcd.print(Day[now.dayOfTheWeek()]);  
    lcd.print(',');    
    lcd.print(now.day(), DEC);
    lcd.print('/');
    lcd.print(now.month(), DEC);
    lcd.print('/');
    lcd.print(now.year(), DEC);
    lcd.print(" ");   
    lcd.setCursor(0, 1);
    lcd.print(now.hour(), DEC);
    lcd.print(':');
    lcd.print(now.minute(), DEC);
    lcd.print(':');
    lcd.print(now.second(), DEC);
}

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){
    delay(BOUN);
    while(digitalRead(BOK) == PUSH){}
    lcd.clear();
    goto setSUBMENU1;
  }
  else if(digitalRead(BUP) == PUSH){
    delay(BOUN);
    while(digitalRead(BUP) == PUSH){}
    lcd.clear();
    goto setExit;
  }
  else if(digitalRead(BDN) == PUSH){
    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. About         ");
    lcd.setCursor(0, 2); lcd.print("07. Exit          ");
    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 setExit;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4     ");
    lcd.setCursor(0, 1); lcd.print("06. About       <-");
    lcd.setCursor(0, 2); lcd.print("07. Exit          ");
    goto setMenu6;
    
//---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 setMenu6;
  }
  else if(digitalRead(BDN) == PUSH){
    delay(BOUN);
    while(digitalRead(BDN) == PUSH){}
    lcd.clear();
    goto setMenu1;
  }
    logic();
    lcd.setCursor(0, 0); lcd.print("05. Monitor 4     ");
    lcd.setCursor(0, 1); lcd.print("06. About         ");
    lcd.setCursor(0, 2); lcd.print("07. Exit        <-");
    goto setExit;


//------------------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;
  }
  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 setSUBMENU6;

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

void logic(){

   //code for gyroscope
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Mengubah nilai gyro menjadi derajat per detik
  float gyroX = (float)gx / 131.0;
  float gyroY = (float)gy / 131.0;
  float gyroZ = (float)gz / 131.0;

  //---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 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();
  }

  //---weight = 0
  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();
  }
  Serial.println(gyroX);
  Analog_Input();
}