#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <LSM303.h>
#include <SD.h>
#include <SPI.h>
#include <RTClib.h>
#include <TinyGPS++.h>
#include <HardwareSerial.h>
#include <Wire.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//LSM303 compass;
RTC_DS3231 rtc;
TinyGPSPlus gps;
HardwareSerial MySerial(2);

#define SD_CS 5
#define RXD2 16
#define TXD2 17

#define GREEN_BUTTON_PIN 4
#define RED_BUTTON_PIN 15
#define BLUE_BUTTON_PIN 2

const unsigned long LONG_PRESS_THRESHOLD = 1000;
const unsigned long LOG_INTERVAL = 5000;  // Logging every 5 seconds
unsigned long lastLogTime = 0;
unsigned long ButtonPressStartTime = 0;
unsigned long lastButtonPressTime = 0;
bool longpress = false;
int currentMenuOption = 0;
int currentMode = 0;

String dataMessage;
String txt_minute, txt_detik;
int Vresistor = 0;
int Vrdata;
int jam, menit, detik;
int tahun, bulan, tanggal;
float latitude, longitude;

int green_button_state;
int red_button_state;
int blue_button_state;

char namaHari[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};

// Improved button debouncing
unsigned long debounceDelay = 50;
unsigned long lastGreenButtonTime = 0;
unsigned long lastRedButtonTime = 0;
unsigned long lastBlueButtonTime = 0;

//float pitch_print, aX, aY, aZ, mX, mY, mZ, X, Y, latitude, longitude;

const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;

// SD card functions
void writeFile(fs::FS &fs, const char *path, const char *message) {
    File file = fs.open(path, FILE_WRITE);
    if (!file) {
        Serial.println("Failed to open file for writing");
        return;
    }
    file.print(message);
    file.close();
}

void appendFile(fs::FS &fs, const char *path, const char *message) {
    File file = fs.open(path, FILE_APPEND);
    if (!file) {
        Serial.println("Failed to open file for appending");
        return;
    }
    file.print(message);
    file.close();
}

// GPS and SD card logging
void logSDCard() {
    DateTime now = rtc.now();
    txt_minute = String(now.minute() < 10 ? "0" : "") + String(now.minute());
    txt_detik = String(now.second() < 10 ? "0" : "") + String(now.second());
    jam = now.hour();
    menit = now.minute();
    detik = now.second();
    tanggal = now.day();
    bulan = now.month();
    tahun = now.year();

    if (gps.location.isValid()) {
        latitude = gps.location.lat();
        longitude = gps.location.lng();
    } else {
        latitude = 0.0;
        longitude = 0.0;
    }

    dataMessage = String(tanggal) + "/" + String(bulan) + "/" + String(tahun) + " " +
                  String(jam) + ":" + txt_minute + ":" + txt_detik + " " +
                  "Latitude: " + String(latitude, 6) + ", " +
                  "Longitude: " + String(longitude, 6) + ", " + "\n";
                  // "aX: " + String(compass.a.x) + "," + " " 
                  // "aY: " + String(compass.a.y) + "," + " "
                  // "aZ: " + String(compass.a.z) + "," + " "
                  // "mX: " + String(compass.m.x) + "," + " "
                  // "mY: " + String(compass.m.y) + ";" + " "
                  // "mZ: " + String(compass.m.z) + + "\n";

    Serial.println(dataMessage);
    appendFile(SD, "/C.txt", dataMessage.c_str());
}

// Reading analog sensor
void ReadVresistor() {
    Vrdata = analogRead(Vresistor);
    Serial.println(Vrdata);
}

void setup() {
    Serial.begin(9600);
    MySerial.begin(9600, SERIAL_8N1, RXD2, TXD2);

    pinMode(GREEN_BUTTON_PIN, INPUT_PULLUP);
    pinMode(RED_BUTTON_PIN, INPUT_PULLUP);
    pinMode(BLUE_BUTTON_PIN, INPUT_PULLUP);

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("Failed to start SSD1306 OLED"));
        while (1);
    }

    if (!rtc.begin()) {
        Serial.println("RTC not detected");
    }

    if (!SD.begin(SD_CS)) {
        Serial.println("SD card mount failed");
        return;
    }

    File file = SD.open("/C.txt");
    if (!file) {
        Serial.println("Creating log file...");
        writeFile(SD, "/C.txt", "Log Start\r\n");
    }
    file.close();
}

void loop() {
    unsigned long currentTime = millis();
    green_button_state = digitalRead(GREEN_BUTTON_PIN);
    red_button_state = digitalRead(RED_BUTTON_PIN);
    blue_button_state = digitalRead(BLUE_BUTTON_PIN);

    // Log data every 5 seconds
    if (currentTime - lastLogTime >= LOG_INTERVAL) {
        logSDCard();
        lastLogTime = currentTime;
    }

    // Debouncing logic for Green Button
    if (green_button_state == LOW && currentTime - lastGreenButtonTime > debounceDelay) {
        if (ButtonPressStartTime == 0) {
            ButtonPressStartTime = millis();
        }

        while (green_button_state == LOW) {
            green_button_state = digitalRead(GREEN_BUTTON_PIN);
            if (millis() - ButtonPressStartTime >= LONG_PRESS_THRESHOLD) {
                currentMode = 0;
                Serial.println("Long Press Detected");
                longpress = true;
                delay(1000);
            }
        }

        if (!longpress && green_button_state == HIGH) {
            if (currentMode == 0) {
                currentMode = 1;
            } else if (currentMode == 1) {
                switch (currentMenuOption) {
                    case 0: currentMode = 2; break;
                    case 1: currentMode = 3; break;
                    case 2: currentMode = 4; break;
                }
            }
        }
        ButtonPressStartTime = 0;
        longpress = false;
        lastGreenButtonTime = currentTime;
    }

    // Red Button (backwards menu navigation)
    if (red_button_state == LOW && currentTime - lastRedButtonTime > debounceDelay) {
        currentMenuOption = (currentMenuOption + 2) % 3;
        lastRedButtonTime = currentTime;
    }

    // Blue Button (forwards menu navigation)
    if (blue_button_state == LOW && currentTime - lastBlueButtonTime > debounceDelay) {
        currentMenuOption = (currentMenuOption + 1) % 3;
        lastBlueButtonTime = currentTime;
    }

    // Update display based on the current mode
    switch (currentMode) {
        case 0: defaultmenu(); break;
        case 1: optionmenu(); break;
        case 2: menuPressure(); break;
        case 3: menuGPS(); break;
        case 4: menuCompass(); break;
        default: break;
    }
}

// Default menu display
void defaultmenu() {
    DateTime now = rtc.now();
    String waktu = String(now.hour()) + ":" + (now.minute() < 10 ? "0" : "") + String(now.minute()) + ":" + (now.second() < 10 ? "0" : "") + String(now.second());
    String tanggal = String(namaHari[now.dayOfTheWeek()]) + ", " + String(now.day()) + "/" + String(now.month()) + "/" + String(now.year());

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.println(waktu);
    display.setCursor(0, 10);
    display.println(tanggal);
    display.display();
}

// Option menu display
void optionmenu() {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.setTextColor(WHITE);
    display.print("Menu Options:");

    for (int i = 0; i < 3; ++i) {
        if (i == currentMenuOption) {
            display.setTextColor(BLACK, WHITE);
        } else {
            display.setTextColor(WHITE);
        }
        switch (i) {
            case 0: display.setCursor(0, 10 + i * 10); display.print("1. Pressure"); break;
            case 1: display.setCursor(0, 10 + i * 10); display.print("2. Sensor GPS"); break;
            case 2: display.setCursor(0, 10 + i * 10); display.print("3. Sensor Compass"); break;
        }
    }
    display.display();
}

// Pressure menu (placeholder)
void menuPressure() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.println("  Pressure  ");
    display.setCursor(1, 12);
    display.print("Bars: ");
    display.setCursor(2, 20);
    display.print("Psi: ");
    display.setCursor(3, 30);
    display.print("Pump: ");
    display.display();
}

// GPS menu
void menuGPS() {
    while (MySerial.available() > 0) {
        char c = MySerial.read();
        gps.encode(c);
    }

    if (gps.location.isValid()) {
        latitude = gps.location.lat();
        longitude = gps.location.lng();
  
  // Print latitude and longitude to Serial Monitor
    Serial.print("Latitude: ");
    Serial.println(latitude, 6);
    Serial.print("Longitude: ");
    Serial.println(longitude, 6);

    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("GPS Menu");
    display.setCursor(0, 10);
    display.print("Lat: "); display.print(latitude, 6);
    display.setCursor(0, 20);
    display.print("Long: "); display.print(longitude, 6);
    display.display();
}

// Compass menu (placeholder)
void menuCompass() {
 // compass.read();
//  float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;

// // Accelerometer calibration
//   Xa_off = compass.a.x / 16.0 + 17.224598;
//   Ya_off = compass.a.y / 16.0 + 12.282323;
//   Za_off = compass.a.z / 16.0 - 3.162899;

//   // Magnetometer calibration
//   Xm_off = compass.m.x * (100000.0 / 1100.0) + 683.481495;
//   Ym_off = compass.m.y * (100000.0 / 1100.0) + 8449.523695;
//   Zm_off = compass.m.z * (100000.0 / 980.0 ) + 6823.709607;
   
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.println("Xa: "); 
   // display.print(Xa_off);
    display.display();
}