// #include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Wire.h>
#include <MPU6050.h>

// Configuration constants
#define LEVEL_THRESHOLD 0.5    // degrees
#define WHEELBASE_MM 3000      // T6 California wheelbase in mm  
#define TRACK_WIDTH_MM 1904    // T6 California track width in mm
#define CALIBRATION_HOLD_TIME 2000 // ms

// Pin definitions (adjust for your simulator)
#define TFT_CS     10
#define TFT_DC     9
#define TFT_RST    8
#define BUTTON_CAL 2
#define BUTTON_WAKE 3

// Display setup
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// MPU6050 setup
MPU6050 mpu;

// Colors
#define COLOR_BLACK    0x0000
#define COLOR_WHITE    0xFFFF
#define COLOR_RED      0xF800
#define COLOR_GREEN    0x07E0
#define COLOR_BLUE     0x001F
#define COLOR_YELLOW   0xFFE0

// Global variables
int currentScreen = 0;
float rollOffset = 0.0;
float pitchOffset = 0.0;
bool showNotification = false;
unsigned long notificationTimer = 0;
unsigned long lastActivityTime = 0;
unsigned long lastButtonPress = 0;
bool calButtonPressed = false;

// Sensor data
float accelX, accelY, accelZ;
float roll, pitch;
float batteryVoltage = 3.8; // Simulated

void setup() {
  Serial.begin(115200);

  // Initialize display
  tft.begin();
  tft.setRotation(0);
  tft.fillScreen(COLOR_BLACK);

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
  }

  // Initialize buttons
  pinMode(BUTTON_CAL, INPUT_PULLUP);
  pinMode(BUTTON_WAKE, INPUT_PULLUP);

  lastActivityTime = millis();

  Serial.println("Camper Level System initialized");
}

void loop() {
  updateSensors();
  handleButtons();
  updateDisplay();
  handleNotifications();

  delay(100); // 100ms update rate
}

void updateSensors() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert to g values
  accelX = ax / 16384.0;
  accelY = ay / 16384.0;
  accelZ = az / 16384.0;

  // Calculate roll and pitch
  roll = atan2(accelY, accelZ) * 180.0 / PI;
  pitch = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180.0 / PI;
}

void handleButtons() {
  bool calButton = !digitalRead(BUTTON_CAL);
  bool wakeButton = !digitalRead(BUTTON_WAKE);

  // Handle calibration button (hold for 2 seconds)
  if (calButton && !calButtonPressed) {
    lastButtonPress = millis();
    calButtonPressed = true;
    updateActivity();
  } else if (calButton && calButtonPressed) {
    if (millis() - lastButtonPress >= CALIBRATION_HOLD_TIME) {
      calibrateSensors();
      calButtonPressed = false;
    }
  } else if (!calButton && calButtonPressed) {
    calButtonPressed = false;
  }

  // Handle wake/cycle button
  static bool lastWakeState = false;
  if (wakeButton && !lastWakeState) {
    cycleScreen();
    updateActivity();
  }
  lastWakeState = wakeButton;
}

void calibrateSensors() {
  rollOffset = roll;
  pitchOffset = pitch;
  showNotification = true;
  notificationTimer = millis() + 3000; // Show for 3 seconds
  Serial.println("Calibration complete!");
}

void cycleScreen() {
  currentScreen = (currentScreen + 1) % 2;
  Serial.print("Switched to screen ");
  Serial.println(currentScreen);
}

void updateActivity() {
  lastActivityTime = millis();
}

void handleNotifications() {
  if (showNotification && millis() > notificationTimer) {
    showNotification = false;
  }
}

void updateDisplay() {
  tft.fillScreen(COLOR_BLACK);

  // Draw status bar
  drawStatusBar();

  // Draw main content based on current screen
  if (currentScreen == 0) {
    drawLevelScreen();
  } else if (currentScreen == 1) {
    drawT6CaliforniaScreen();
  }

  // Draw notification overlay
  if (showNotification) {
    drawNotification();
  }
}

void drawStatusBar() {
  // Status bar background
  tft.fillRect(0, 0, 320, 20, COLOR_BLACK);

  // Time (simulated)
  tft.setTextColor(COLOR_WHITE);
  tft.setTextSize(1);
  tft.setCursor(5, 10);
  tft.print("12:34");

  // Battery status
  float batteryPct = (batteryVoltage - 3.0) / (4.2 - 3.0) * 100;
  batteryPct = constrain(batteryPct, 0, 100);

  // Battery icon
  tft.drawRect(280, 5, 25, 10, COLOR_WHITE);
  tft.drawRect(305, 7, 3, 6, COLOR_WHITE);

  // Battery level
  int fillWidth = (int)(23 * batteryPct / 100);
  uint16_t batteryColor = (batteryPct > 20) ? COLOR_GREEN : COLOR_RED;
  tft.fillRect(281, 6, fillWidth, 8, batteryColor);

  // Battery percentage
  tft.setCursor(250, 10);
  tft.print((int)batteryPct);
  tft.print("%");
}

void drawLevelScreen() {
  float correctedRoll = roll - rollOffset;
  float correctedPitch = pitch - pitchOffset;

  // Title
  tft.setTextColor(COLOR_WHITE);
  tft.setTextSize(3);
  tft.setCursor(120, 40);
  tft.print("LEVEL");

  // Roll indicator
  tft.setTextSize(2);
  tft.setCursor(30, 70);
  tft.print("ROLL:");
  tft.setTextSize(3);
  tft.setCursor(30, 90);
  tft.print(correctedRoll, 1);
  tft.print("°");

  bool rollLevel = abs(correctedRoll) <= LEVEL_THRESHOLD;
  tft.setTextSize(1);
  tft.setCursor(30, 115);
  if (rollLevel) {
    tft.setTextColor(COLOR_GREEN);
    tft.print("✓ LEVEL");
  } else {
    tft.setTextColor(COLOR_RED);
    tft.print("✗ TILTED");
  }

  // Pitch indicator
  tft.setTextColor(COLOR_WHITE);
  tft.setTextSize(2);
  tft.setCursor(170, 70);
  tft.print("PITCH:");
  tft.setTextSize(3);
  tft.setCursor(170, 90);
  tft.print(correctedPitch, 1);
  tft.print("°");

  bool pitchLevel = abs(correctedPitch) <= LEVEL_THRESHOLD;
  tft.setTextSize(1);
  tft.setCursor(170, 115);
  if (pitchLevel) {
    tft.setTextColor(COLOR_GREEN);
    tft.print("✓ LEVEL");
  } else {
    tft.setTextColor(COLOR_RED);
    tft.print("✗ TILTED");
  }

  // Visual level indicator (bubble level)
  int centerX = 160;
  int centerY = 180;

  tft.drawCircle(centerX, centerY, 50, COLOR_WHITE);
  tft.drawCircle(centerX, centerY, 48, COLOR_BLACK);

  // Calculate bubble position
  int bubbleX = centerX + (int)(correctedRoll * 2);
  int bubbleY = centerY + (int)(correctedPitch * 2);

  // Clamp bubble to circle
  int dx = bubbleX - centerX;
  int dy = bubbleY - centerY;
  int dist = sqrt(dx * dx + dy * dy);
  if (dist > 45) {
    bubbleX = centerX + (dx * 45 / dist);
    bubbleY = centerY + (dy * 45 / dist);
  }

  // Draw bubble
  uint16_t bubbleColor = (rollLevel && pitchLevel) ? COLOR_GREEN : COLOR_RED;
  tft.fillCircle(bubbleX, bubbleY, 8, bubbleColor);

  // Crosshairs
  tft.drawLine(centerX - 10, centerY, centerX + 10, centerY, COLOR_WHITE);
  tft.drawLine(centerX, centerY - 10, centerX, centerY + 10, COLOR_WHITE);
}

void drawT6CaliforniaScreen() {
  float correctedRoll = roll - rollOffset;
  float correctedPitch = pitch - pitchOffset;

  // Title
  tft.setTextColor(COLOR_WHITE);
  tft.setTextSize(2);
  tft.setCursor(80, 40);
  tft.print("T6 CALIFORNIA");
  tft.setTextSize(1);
  tft.setCursor(110, 65);
  tft.print("Wheel Elevation");

  // Calculate wheel elevations in cm
  float wheelbaseM = WHEELBASE_MM / 1000.0;
  float trackM = TRACK_WIDTH_MM / 1000.0;

  float pitchRad = correctedPitch * PI / 180.0;
  float rollRad = correctedRoll * PI / 180.0;

  float frontElevation = sin(pitchRad) * (wheelbaseM / 2) * 100;
  float rearElevation = -sin(pitchRad) * (wheelbaseM / 2) * 100;
  float leftElevation = sin(rollRad) * (trackM / 2) * 100;
  float rightElevation = -sin(rollRad) * (trackM / 2) * 100;

  float flElevation = frontElevation + leftElevation;  // Front Left
  float frElevation = frontElevation + rightElevation; // Front Right
  float rlElevation = rearElevation + leftElevation;   // Rear Left
  float rrElevation = rearElevation + rightElevation;  // Rear Right

  // Van outline (top view)
  int vanX = 160;
  int vanY = 150;
  int vanWidth = 80;
  int vanHeight = 120;

  tft.drawRect(vanX - vanWidth / 2, vanY - vanHeight / 2, vanWidth, vanHeight, COLOR_WHITE);

  // Front indicator
  tft.fillRect(vanX - 20, vanY - vanHeight / 2 - 8, 40, 8, COLOR_WHITE);
  tft.setTextSize(1);
  tft.setCursor(vanX - 15, vanY - vanHeight / 2 - 20);
  tft.print("FRONT");

  // Wheel positions and elevations
  int wheelSize = 10;

  // Front Left
  int flX = vanX - vanWidth / 2 + 15;
  int flY = vanY - vanHeight / 2 + 20;
  uint16_t flColor = (flElevation > 0) ? COLOR_RED : COLOR_BLUE;
  tft.fillCircle(flX, flY, wheelSize, flColor);
  tft.setTextSize(1);
  tft.setCursor(flX - 30, flY - 5);
  tft.setTextColor(COLOR_WHITE);
  tft.print(flElevation, 1);

  // Front Right
  int frX = vanX + vanWidth / 2 - 15;
  int frY = vanY - vanHeight / 2 + 20;
  uint16_t frColor = (frElevation > 0) ? COLOR_RED : COLOR_BLUE;
  tft.fillCircle(frX, frY, wheelSize, frColor);
  tft.setCursor(frX + 15, frY - 5);
  tft.print(frElevation, 1);

  // Rear Left
  int rlX = vanX - vanWidth / 2 + 15;
  int rlY = vanY + vanHeight / 2 - 20;
  uint16_t rlColor = (rlElevation > 0) ? COLOR_RED : COLOR_BLUE;
  tft.fillCircle(rlX, rlY, wheelSize, rlColor);
  tft.setCursor(rlX - 30, rlY + 15);
  tft.print(rlElevation, 1);

  // Rear Right
  int rrX = vanX + vanWidth / 2 - 15;
  int rrY = vanY + vanHeight / 2 - 20;
  uint16_t rrColor = (rrElevation > 0) ? COLOR_RED : COLOR_BLUE;
  tft.fillCircle(rrX, rrY, wheelSize, rrColor);
  tft.setCursor(rrX + 15, rrY + 15);
  tft.print(rrElevation, 1);

  // Legend
  tft.setTextColor(COLOR_RED);
  tft.setCursor(50, 220);
  tft.print("● HIGH");
  tft.setTextColor(COLOR_BLUE);
  tft.setCursor(150, 220);
  tft.print("● LOW");
  tft.setTextColor(COLOR_WHITE);
  tft.setCursor(120, 235);
  tft.print("Elevation in cm");
}

void drawNotification() {
  // Semi-transparent background (simulate with dark rectangle)
  tft.fillRect(40, 100, 240, 60, COLOR_BLACK);
  tft.drawRect(40, 100, 240, 60, COLOR_GREEN);

  // Notification text
  tft.setTextColor(COLOR_GREEN);
  tft.setTextSize(2);
  tft.setCursor(80, 115);
  tft.print("✓ CALIBRATED");

  tft.setTextColor(COLOR_WHITE);
  tft.setTextSize(1);
  tft.setCursor(100, 140);
  tft.print("Level reference set");
}