#include <TinyGPS++.h>
#include <HardwareSerial.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// ==================== PINS ====================
#define BUTTON_PIN 25

#define RGB_RED_PIN 26
#define RGB_GREEN_PIN 27
#define RGB_BLUE_PIN 14

#define BATTERY_LED_PIN 33


// Create GPS and Serial objects
TinyGPSPlus gps;
HardwareSerial gpsSerial(1); // Use UART1 for GPS

// Create MPU6050 object
Adafruit_MPU6050 mpu;

unsigned long lastSensorRead = 0;

// ==================== DEVICE STATES ====================
enum DeviceState {
  DEVICE_OFF,
  DEVICE_RECORDING,
  DEVICE_BLE_MODE
};

DeviceState currentState = DEVICE_OFF;

// ==================== BUTTON ====================
bool lastButtonState = HIGH;
unsigned long buttonPressStart = 0;
bool buttonPressed = false;

void setup() {

  // ==================== GPIO ====================
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE_PIN, OUTPUT);

  pinMode(BATTERY_LED_PIN, OUTPUT);

  // Initialize Serial Monitor
  Serial.begin(115200);
  Serial.println("Statifly V1 Initializing...");

  // Initialize GPS UART
  gpsSerial.begin(9600, SERIAL_8N1, 16, 17); // RX = GPIO16, TX = GPIO17


  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip!");
    while (1);
  }
  Serial.println("MPU6050 connected successfully!");

  // Configure MPU6050
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void setRGB(bool r, bool g, bool b) {
  digitalWrite(RGB_RED_PIN, r);
  digitalWrite(RGB_GREEN_PIN, g);
  digitalWrite(RGB_BLUE_PIN, b);
}

void updateState(DeviceState newState) {

  currentState = newState;

  switch (currentState) {

    case DEVICE_OFF:
      setRGB(0, 0, 0);
      Serial.println("STATE: OFF");
      break;

    case DEVICE_RECORDING:
      setRGB(0, 1, 0);
      Serial.println("STATE: RECORDING");
      break;

    case DEVICE_BLE_MODE:
      setRGB(0, 0, 1);
      Serial.println("STATE: BLE MODE");
      break;
  }
}

void handleButton() {

  bool currentButton = digitalRead(BUTTON_PIN);

  // Button pressed
  if (currentButton == LOW && lastButtonState == HIGH) {
    buttonPressStart = millis();
    buttonPressed = true;
  }

  // Button released
  if (currentButton == HIGH && lastButtonState == LOW) {

    unsigned long pressDuration = millis() - buttonPressStart;

    // LONG PRESS
    if (pressDuration > 1500) {

      if (currentState == DEVICE_OFF) {
        updateState(DEVICE_RECORDING);
      }
      else if (currentState == DEVICE_RECORDING) {
        updateState(DEVICE_OFF);
      }
      else if (currentState == DEVICE_BLE_MODE) {
        updateState(DEVICE_OFF);
      }

    }

    buttonPressed = false;
  }

  lastButtonState = currentButton;
}

void loop() {

  // ==================== GPS ====================
  while (gpsSerial.available() > 0) {

    char c = gpsSerial.read();

    // DEBUG opcional:
    // Serial.write(c);

    gps.encode(c);
  }

  // ==================== BUTTON ====================
  handleButton();

  // ==================== RECORDING ====================
  if (currentState == DEVICE_RECORDING) {

    if (millis() - lastSensorRead >= 100) {

      lastSensorRead = millis();

      displayData();
    }
  }

  delay(10);
}

void displayData() {
  // GPS Data
  if (gps.location.isValid()) {
    Serial.print("Latitude: ");
    Serial.print(gps.location.lat(), 6);
    Serial.print(", Longitude: ");
    Serial.println(gps.location.lng(), 6);
  } else {
    Serial.println("Waiting GPS signal...");
  }



  // MPU6050 Data
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Print Accelerometer data
  Serial.print("Accel X: ");
  Serial.print(a.acceleration.x);
  Serial.print(" m/s^2, Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(" m/s^2, Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  // Print Gyroscope data
  Serial.print("Gyro X: ");
  Serial.print(g.gyro.x);
  Serial.print(" rad/s, Y: ");
  Serial.print(g.gyro.y);
  Serial.print(" rad/s, Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  // Optional: Add GPS timestamp
  if (gps.time.isUpdated()) {
    Serial.print("Time: ");
    Serial.print(gps.time.hour());
    Serial.print(":");
    Serial.print(gps.time.minute());
    Serial.print(":");
    Serial.println(gps.time.second());
  }
}