#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>

// Flex initialization
int flexSensorPin = 25; // Flex sensor on pin 25
int emgSensorPin = 26; // EMG sensor on pin 26
int flex; // Flex sensor readings
int flexAngle; // Angle calculated from flex value
int emg; // EMG sensor reading

// OLED and MPU6050 setup
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 // OLED reset pin
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_MPU6050 mpu;

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2); // Set the LCD I2C address and size

// Variables for state switching
unsigned long previousMillis = 0; // Stores last time state was updated
const long interval = 5000;       // 5 seconds interval for switching
int lcdState = 0;                 // Tracks the current LCD state

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

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

  // Initialize SSD1306 OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Check OLED I2C address (usually 0x3C)
    Serial.println(F("SSD1306 allocation failed"));
    while (1); // Don't proceed if OLED fails
  }
  display.display(); // Show initial splash screen
  delay(2000);       // Pause for 2 seconds

  display.clearDisplay();  // Clear the buffer
  display.setTextSize(1);  // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);  // White color for text
  display.setCursor(0, 0); // Start at top-left corner

  // Initialize 16x2 I2C LCD
  lcd.begin(16, 2); // Set up LCD with 16 columns and 2 rows
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("System Ready");
  delay(1000);
}

void loop() {
  // Flex sensor readings, converts raw flex value to angle.
  flex = analogRead(flexSensorPin);
  flexAngle = map(flex, 0, 1023, 0, 180);
  
  // EMG sensor readings
  emg = analogRead(emgSensorPin);

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

  // Update OLED display with MPU6050 data
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE, 0);
  display.setCursor(0, 0);
  display.println("Accelerometer:");
  display.print(a.acceleration.x);
  display.print(",");
  display.print(a.acceleration.y);
  display.print(",");
  display.println(a.acceleration.z);
  display.println("");

  display.println("Gyroscope:");
  display.print(g.gyro.x);
  display.print(",");
  display.print(g.gyro.y);
  display.print(",");
  display.print(g.gyro.z);

  display.println("");
  display.print("Flex Angle: ");
  display.print(flexAngle);

  if (flexAngle > 90) {
    display.println(" Bent");
  } else {
    display.println(" Flat");
  }

  display.println("");
  display.print("EMG Value:");
  display.print(emg);

  display.display();

  // Check if it's time to switch the LCD state
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    lcdState = (lcdState + 1) % 2; // Toggle between states 0 and 1
  }

  // Display information on the LCD based on the current state
  lcd.clear();
  if (lcdState == 0) {
    // State 1: Show FlexAngle and EMG values
    lcd.setCursor(0, 0);
    lcd.print("Angle: ");
    lcd.print(flexAngle);
    lcd.setCursor(0, 1);
    lcd.print("EMG: ");
    lcd.print(emg);
  } else if (lcdState == 1) {
    // State 2: Show accelerometer and gyroscope values
    lcd.setCursor(0, 0);
    lcd.print("Acc X:");
    lcd.print(a.acceleration.x);
    lcd.setCursor(8, 0);
    lcd.print(" Y:");
    lcd.print(a.acceleration.y);
    lcd.setCursor(0, 1);
    lcd.print("Gyro X:");
    lcd.print(g.gyro.x);
    lcd.setCursor(8, 1);
    lcd.print(" Y:");
    lcd.print(g.gyro.y);
  }


  //Print EMG, Flex Value, Flex Angle 
  Serial.print("EMG Value: ");
  Serial.println(emg);;

  Serial.print("Flex Value: ");
  Serial.println(flex);
  Serial.print("Flex Angle: ");
  Serial.println(flexAngle);
  
  Serial.println("Accelerometer:");
  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.println(a.acceleration.z);
  Serial.println("");
  
  delay(500);
}