#include <Adafruit_ILI9341.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// Pins for hardware
#define HEAD_TOUCH_SENSOR 2
#define BELLY_TOUCH_SENSOR 4
#define KY037_PIN A0
#define SPEAKER_PIN 9
#define SERVO_BELLY 3
#define SERVO_ARM_LEFT 5
#define SERVO_ARM_RIGHT 6
#define TFT_DC 8
#define TFT_CS 10

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

// Servo setup
Servo bellyServo, leftArmServo, rightArmServo;

// Accelerometer setup
Adafruit_MPU6050 mpu;

// State variables
bool isAwake = false; // Tracks if the robot is awake

void setup() {
  // Initialize pins
  pinMode(HEAD_TOUCH_SENSOR, INPUT);
  pinMode(BELLY_TOUCH_SENSOR, INPUT);
  pinMode(SPEAKER_PIN, OUTPUT);
  pinMode(KY037_PIN, INPUT);

  // Initialize servos
  bellyServo.attach(SERVO_BELLY);
  leftArmServo.attach(SERVO_ARM_LEFT);
  rightArmServo.attach(SERVO_ARM_RIGHT);

  // Initialize TFT
  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);

  // Initialize MPU6050
  if (!mpu.begin()) {
    tft.println("MPU6050 not found!");
    while (1);
  }

  // Start with closed eyes
  drawClosedEyes();
  delay(2000); // Simulate sleep

  // Wake-up sequence
  wakeUpAnimation();
  drawFace("open"); // Default open eyes
  isAwake = true;

  Serial.begin(9600);
}

void loop() {
  if (isAwake) {
    // Belly Tap: Show smiling face, vibrate, and laugh
    if (digitalRead(BELLY_TOUCH_SENSOR) == LOW) {
      drawFace("smile");
      vibrateServo(bellyServo); // Vibrate belly servo
      playLaughSound();         // Play laughing sound
      delay(2000);
      drawFace("open");
    }

    // Head Tap: Show embarrassed face and laugh
    if (digitalRead(HEAD_TOUCH_SENSOR) == LOW) {
      drawFace("embarrassed");
      playEmbarrassedSound(); // Play embarrassed laugh
      delay(2000);
      drawFace("open");
    }

    // Accelerometer: Detect actions
    sensors_event_t accel, gyro, temp;
    mpu.getEvent(&accel, &gyro, &temp);

    if (detectShake(accel)) {
      drawFace("dizzy");
      playDizzySound(); // Play dizzy sound
      delay(2000);
      drawFace("open");
    }

    if (detectPickUp(accel)) {
      drawFace("fear");
      moveServosInFear();
      playFearSound(); // Play screaming sound
      delay(2000);
      drawFace("open");
    }
  }
}

// Helper Functions

void playLaughSound() {
  tone(SPEAKER_PIN, 440, 300); // Low tone
  delay(400);
  tone(SPEAKER_PIN, 880, 300); // High tone
  delay(400);
  tone(SPEAKER_PIN, 660, 300); // Mid tone
  delay(400);
  noTone(SPEAKER_PIN);
}

void playEmbarrassedSound() {
  tone(SPEAKER_PIN, 330, 200); // Short tone
  delay(300);
  tone(SPEAKER_PIN, 330, 200); // Repeat
  delay(300);
  tone(SPEAKER_PIN, 330, 200); // Repeat
  delay(300);
  noTone(SPEAKER_PIN);
}

void playDizzySound() {
  for (int freq = 400; freq < 1000; freq += 50) {
    tone(SPEAKER_PIN, freq, 100);
    delay(100);
  }
  for (int freq = 1000; freq > 400; freq -= 50) {
    tone(SPEAKER_PIN, freq, 100);
    delay(100);
  }
  noTone(SPEAKER_PIN);
}

void playFearSound() {
  for (int i = 0; i < 5; i++) {
    tone(SPEAKER_PIN, 600, 200);
    delay(300);
  }
  noTone(SPEAKER_PIN);
}

void drawFace(String expression) {
  tft.fillRect(20, 20, 240, 200, ILI9341_BLACK);
  drawEyes();
  if (expression == "open") drawNeutralMouth();
  if (expression == "smile") drawSmilingMouth();
  if (expression == "embarrassed") drawBlush();
  if (expression == "dizzy") drawDizzyEffect();
  if (expression == "fear") drawFearMouth();
}

void drawClosedEyes() {
  tft.fillScreen(ILI9341_BLACK);
  tft.drawLine(60, 100, 140, 100, ILI9341_WHITE); // Left closed eye
  tft.drawLine(180, 100, 260, 100, ILI9341_WHITE); // Right closed eye
  tft.setCursor(100, 180);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.print("Zzz...");
}

void wakeUpAnimation() {
  for (int i = 20; i >= 0; i--) {
    tft.fillRect(100 - 40, 100 - i, 80, i * 2, ILI9341_BLACK);
    tft.fillRect(220 - 40, 100 - i, 80, i * 2, ILI9341_BLACK);
    delay(100);
  }
}

void drawEyes() {
  tft.fillCircle(100, 100, 40, ILI9341_WHITE);
  tft.fillCircle(220, 100, 40, ILI9341_WHITE);
  tft.fillCircle(100, 100, 15, ILI9341_BLACK);
  tft.fillCircle(220, 100, 15, ILI9341_BLACK);
}

void drawNeutralMouth() {
  tft.drawLine(140, 180, 200, 180, ILI9341_WHITE);
}

void drawSmilingMouth() {
  for (int angle = 200; angle <= 340; angle++) {
    float radian = angle * 3.14159 / 180;
    int x = 170 + cos(radian) * 40;
    int y = 180 + sin(radian) * 40;
    tft.drawPixel(x, y, ILI9341_GREEN);
  }
}

void drawBlush() {
  tft.fillCircle(80, 140, 15, ILI9341_RED);
  tft.fillCircle(240, 140, 15, ILI9341_RED);
}

void drawDizzyEffect() {
  for (int i = 0; i < 10; i++) {
    int x1 = random(20, 260);
    int y1 = random(20, 220);
    int x2 = random(20, 260);
    int y2 = random(20, 220);
    tft.drawLine(x1, y1, x2, y2, ILI9341_RED);
  }
}

void drawFearMouth() {
  tft.drawCircle(170, 180, 20, ILI9341_BLUE);
}

void vibrateServo(Servo &servo) {
  for (int i = 0; i < 5; i++) {
    servo.write(0);
    delay(100);
    servo.write(90);
    delay(100);
  }
}

bool detectShake(sensors_event_t &accel) {
  return abs(accel.acceleration.x) > 10 || 
         abs(accel.acceleration.y) > 10 || 
         abs(accel.acceleration.z) > 10;
}

bool detectPickUp(sensors_event_t &accel) {
  return accel.acceleration.z > 15;
}

void moveServosInFear() {
  for (int i = 0; i < 5; i++) {
    leftArmServo.write(45);
    rightArmServo.write(135);
    delay(100);
    leftArmServo.write(135);
    rightArmServo.write(45);
    delay(100);
  }
}