#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <RoboEyesManager.h>

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

// Devices
Adafruit_MPU6050 mpu;
RoboEyesManager roboEyesManager(display);

// Shared I2C pins
#define I2C_SDA_PIN 6
#define I2C_SCL_PIN 7

// Input & Output Pins
#define BUTTON_PIN 5
#define MIC_PIN 4
#define BUZZER_PIN 8 // NEW

bool buttonPressed = false;
int micValue = 0;

float accelX, accelY, accelZ;
float gyroX, gyroY, gyroZ;
float temperature;
bool mpuConnected = false;

bool isLifted = false;
bool isShaken = false;
bool isTilted = false;
unsigned long lastShakeTime = 0;
const unsigned long shakeCooldown = 500;

unsigned long lastLogTime = 0;
const unsigned long logInterval = 500;

void scanI2C()
{
  Serial.println("Scanning I2C devices...");
  for (byte address = 1; address < 127; address++)
  {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() == 0)
    {
      Serial.print("Found device at 0x");
      Serial.println(address, HEX);
    }
  }
}

void detectMotion()
{
  float accelMag = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
  float gyroMag = sqrt(gyroX * gyroX + gyroY * gyroY + gyroZ * gyroZ);

  if (!isLifted && accelMag < 7.0)
  {
    isLifted = true;
    roboEyesManager.isLifted();
  }
  else if (isLifted && accelMag > 9.5)
  {
    isLifted = false;
    roboEyesManager.isPutDown();
  }

  if (gyroMag > 3.0 && millis() - lastShakeTime > shakeCooldown)
  {
    isShaken = true;
    lastShakeTime = millis();
    roboEyesManager.isShaken();
  }

  if (abs(accelX) > 4.0)
  {
    if (!isTilted)
    {
      isTilted = true;
      (accelX < 0) ? roboEyesManager.isTiltedLeft() : roboEyesManager.isTiltedRight();
    }
  }
  else
  {
    isTilted = false;
  }
}

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

  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  Wire.setClock(100000);
  scanI2C();

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS))
  {
    Serial.println("SSD1306 Failed");
    while (1)
      ;
  }

  pinMode(BUTTON_PIN, INPUT);
  pinMode(MIC_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT); // NEW

  if (mpu.begin(0x68, &Wire))
  {
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
    mpuConnected = true;
  }

  roboEyesManager.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
}

void loop()
{
  buttonPressed = digitalRead(BUTTON_PIN);
  micValue = analogRead(MIC_PIN);

  if (mpuConnected)
  {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    accelX = a.acceleration.x;
    accelY = a.acceleration.y;
    accelZ = a.acceleration.z;
    gyroX = g.gyro.x;
    gyroY = g.gyro.y;
    gyroZ = g.gyro.z;
    temperature = temp.temperature;
    detectMotion();
  }

  roboEyesManager.update();

  if (buttonPressed)
  {
    roboEyesManager.isButtonPressed();
    digitalWrite(BUZZER_PIN, HIGH);
    delay(50);
    digitalWrite(BUZZER_PIN, LOW);
  }

  if (micValue > 2900)
  {
    roboEyesManager.isMicTriggered();
    tone(BUZZER_PIN, 800, 60); // short beep
  }

  if (millis() - lastLogTime >= logInterval)
  {
    lastLogTime = millis();
    Serial.printf("MIC=%d BTN=%d\n", micValue, buttonPressed);
  }
}