#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <FluxGarage_RoboEyes.h>
#include <MPU6050.h>

MPU6050 mpu;

// BUZZER PIN
#define BUZZER_PIN D5

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

// RoboEyes instance
RoboEyes<Adafruit_SSD1306> roboEyes(display);

void setup() {
  Serial.begin(9600);
  Wire.begin(D2, D1);   // SDA, SCL for NodeMCU

  pinMode(BUZZER_PIN, OUTPUT);

  // Initialize OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED failed");
    while (1);
  }

  // Initialize RoboEyes
  roboEyes.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
  roboEyes.setAutoblinker(ON, 3, 2);
  roboEyes.setIdleMode(ON, 2, 2);

  // Initialize MPU6050
  Serial.println("Initializing MPU6050...");
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }
  Serial.println("MPU6050 OK");
}

void loop() {

  roboEyes.update();   // Keep animations smooth

  // Read raw gyro + accel
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

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

  // Convert accel to angles
  float angleX = atan2(ay, az) * 57.2958;  // Roll (left/right)
  float angleY = atan2(ax, az) * 57.2958;  // Pitch (forward/back)

  // Convert gyro to movement speed (shake detector)
  float gForce = abs(gx) + abs(gy) + abs(gz);


  //  SHAKE 

  if (gForce > 50000) {

    // Sound effect for shaking
    tone(BUZZER_PIN, 600, 80);
    delay(100);
    tone(BUZZER_PIN, 400, 80);

    roboEyes.anim_confused();
    roboEyes.setMood(ANGRY);
    delay(1000);

    return; // stop other expressions
  }

  
  //  UPWARD 

  if (angleY < -25) {

    tone(BUZZER_PIN, 600, 100);
    delay(120);
    tone(BUZZER_PIN, 1200, 120);

    roboEyes.anim_laugh();
    return;
  }


  // TAP / sudden 
 
  static int16_t last_ax = 0;
  int diff = abs(ax - last_ax);
  last_ax = ax;

  if (diff > 12000) {   // sudden tap or hit

    tone(BUZZER_PIN, 1500, 60);

    roboEyes.setAutoblinker(OFF, 0, 0);
    roboEyes.setMood(HAPPY);
    delay(80);
    roboEyes.setMood(DEFAULT);
    roboEyes.setAutoblinker(ON, 3, 2);
    return;
  }

 
  // Expression Logic 
 
  if (angleX > 25) {      // RIGHT = HAPPY
    tone(BUZZER_PIN, 1300, 40);
    roboEyes.setMood(HAPPY);
  }
  else if (angleX < -25) { // LEFT = ANGRY
    tone(BUZZER_PIN, 300, 60);
    roboEyes.setMood(ANGRY);
  }
  else if (angleY > 25) {  // DOWN = TIRED
    tone(BUZZER_PIN, 200, 40);
    roboEyes.setMood(TIRED);
  }
  else {                   // DEFAULT
    roboEyes.setMood(DEFAULT);
  }
}