#include <ESP32Servo.h>
  #include <Wire.h>
  #include <Adafruit_GFX.h>
  #include <Adafruit_SSD1306.h>

  #define SCREEN_WIDTH 128
  #define SCREEN_HEIGHT 64

  Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

  Servo headServo;
  Servo handServo1;
  Servo handServo2;

  const int headPin = D0;     // GPIO0
  const int handPin1 = D3;    // GPIO3
  const int handPin2 = D6;    // GPIO6

  const int led1Pin = D1;     // GPIO1
  const int led2Pin = D7;     // GPIO7

  String inputString = "";
  bool stringComplete = false;

  unsigned long lastReceivedMillis = 0;
  unsigned long commandTimeout = 3000;  // 3 sec no command = neutral

  unsigned long lastSweepMillis = 0;
  const long sweepInterval = 20;
  unsigned long lastBlinkMillis = 0;
  const long blinkInterval = 500;

  bool ledsBlinkState = false;
  int headPos = 90;
  int handPos = 90;
  bool sweepingForward = true;

  char lastCommand = 'N'; // 'A', 'B', or 'N' for neutral

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

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
      Serial.println(F("SSD1306 allocation failed"));
      while (1);
    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);

    delay(1000);

    headServo.attach(headPin);
    handServo1.attach(handPin1);
    handServo2.attach(handPin2);

    pinMode(led1Pin, OUTPUT);
    pinMode(led2Pin, OUTPUT);

    headServo.write(90);
    handServo1.write(90);
    handServo2.write(90);
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, LOW);

    Serial.println("Setup complete.");
  }

  void loop() {
    // Read Serial
    if (Serial.available()) {
      char inChar = (char)Serial.read();
      if (inChar == 'A' || inChar == 'B') {
        inputString = inChar;
        stringComplete = true;
      }
    }

    // Handle Command
    if (stringComplete) {
      lastReceivedMillis = millis();

      if (inputString == "A") {
        lastCommand = 'A';
        Serial.println("Received A - Start Action A");
      } else if (inputString == "B") {
        lastCommand = 'B';
        Serial.println("Received B - Start Action B");
      }

      stringComplete = false;
    }

    // Timeout to neutral
    if (millis() - lastReceivedMillis > commandTimeout && lastCommand != 'N') {
      Serial.println("No command for 3s - Resetting to neutral.");
      headServo.write(90);
      handServo1.write(90);
      handServo2.write(90);
      digitalWrite(led1Pin, LOW);
      digitalWrite(led2Pin, LOW);
      lastCommand = 'N';
    }

    // Perform continuous actions
    if (lastCommand == 'A') {
      performActionA();
      showHappyFace();  
    } else if (lastCommand == 'B') {
      performActionB();
      showSadFace();
    }
  }

  void performActionA() {
    unsigned long currentMillis = millis();

    if (currentMillis - lastSweepMillis >= sweepInterval) {
      lastSweepMillis = currentMillis;

      if (sweepingForward) {
        headPos += 2;
        handPos += 2;
        if (headPos >= 135) sweepingForward = false;
      } else {
        headPos -= 2;
        handPos -= 2;
        if (headPos <= 45) sweepingForward = true;
      }

      headServo.write(headPos);
      handServo1.write(handPos);
      handServo2.write(handPos);
    }

    if (currentMillis - lastBlinkMillis >= blinkInterval) {
      lastBlinkMillis = currentMillis;
      ledsBlinkState = !ledsBlinkState;
      digitalWrite(led1Pin, ledsBlinkState ? HIGH : LOW);
      digitalWrite(led2Pin, ledsBlinkState ? LOW : HIGH);
    }
  }

  void performActionB() {
    headServo.write(180);
    handServo1.write(120);
    handServo2.write(70);
    digitalWrite(led1Pin, HIGH);
    digitalWrite(led2Pin, HIGH);
  }

  void showHappyFace() {
    display.clearDisplay();

    // Eyes
    display.fillCircle(40, 20, 4, SSD1306_WHITE);  // Left eye
    display.fillCircle(80, 20, 4, SSD1306_WHITE);  // Right eye

    // Smile (arc simulated with short line segments)
    display.drawLine(45, 40, 50, 45, SSD1306_WHITE);
    display.drawLine(50, 45, 60, 47, SSD1306_WHITE);
    display.drawLine(60, 47, 70, 45, SSD1306_WHITE);
    display.drawLine(70, 45, 75, 40, SSD1306_WHITE);

    display.setCursor(40, 55);
    display.print("Happy :)");

    display.display();
  }

  void showSadFace() {
    display.clearDisplay();

    // Eyes
    display.fillCircle(40, 20, 4, SSD1306_WHITE);  // Left eye
    display.fillCircle(80, 20, 4, SSD1306_WHITE);  // Right eye

    // Frown (arc simulated with short line segments)
    display.drawLine(45, 45, 50, 40, SSD1306_WHITE);
    display.drawLine(50, 40, 60, 38, SSD1306_WHITE);
    display.drawLine(60, 38, 70, 40, SSD1306_WHITE);
    display.drawLine(70, 40, 75, 45, SSD1306_WHITE);

    display.setCursor(45, 55);
    display.print("Sad :(");

    display.display();
  }