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

// === Code1 sleep frames ===
#include "sleep2_left_frames.h"
#include "sleep2_right_frames.h"

// === Code1 smile frames ===
#include "smily_left_frames.h"
#include "smily_right_frames.h"

// === Code2 move frames ===
#include "move_left_frames.h"
#include "move_right_frames.h"

// ===== OLED Setup =====
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define I2C_SDA 8
#define I2C_SCL 9
#define ADDR_LEFT  0x3C
#define ADDR_RIGHT 0x3D
#define OLED_RESET -1

Adafruit_SSD1306 displayL(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_SSD1306 displayR(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);  // 0x3C
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);  // 0x3D

// === Eye Size and Position ===
int eyeX = 44;
int eyeY = 12;
int eyeWidth = 40;
int eyeHeight = 40;

// === Eye Movement Variables ===
int targetOffsetX = 0;
int targetOffsetY = 0;
int moveSpeed = 3;

// === Blinking Logic ===
int blinkState = 0;
int blinkDelay = 4000;
unsigned long lastBlinkTime = 0;
unsigned long moveTime = 0;

// Smooth movement tracking
static int offsetX = 0;
static int offsetY = 0;

// ===== WS2812 Setup =====
#define LED1_PIN 11
#define LED2_PIN 12
#define NUMPIXELS 16  

Adafruit_NeoPixel ring1(NUMPIXELS, LED1_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel ring2(NUMPIXELS, LED2_PIN, NEO_GRB + NEO_KHZ800);

// ===== PIR + Smile Setup =====
#define PIR_PIN 13   // PIR sensor pin

// ===== TOUCH SENSOR SETUP =====
#define TOUCH_SENSOR_PIN 14  // Connect OUT pin of TTP223 to GPIO 15

// === Delay helpers ===
static inline uint16_t delayL(uint16_t i){ return pgm_read_word(&sleep2_left_frame_delay_ms[i]); }
static inline uint16_t delayR(uint16_t i){ return pgm_read_word(&sleep2_right_frame_delay_ms[i]); }

static inline uint16_t smileDelayL(uint16_t i){ return pgm_read_word(&smily_left_frame_delay_ms[i]); }
static inline uint16_t smileDelayR(uint16_t i){ return pgm_read_word(&smily_right_frame_delay_ms[i]); }

static inline uint16_t moveDelayL(uint16_t i){ return pgm_read_word(&move_left_frame_delay_ms[i]); }
static inline uint16_t moveDelayR(uint16_t i){ return pgm_read_word(&move_right_frame_delay_ms[i]); }

// ===== Servo Setup =====
Servo myServo1;
Servo myServo2;
int servoPin1 = 37;   // First servo
int servoPin2 = 36;   // Second servo

// ==========================
// === Utility Functions ===
// ==========================

void drawFrame(Adafruit_SSD1306 &d, const uint8_t *frame, uint16_t w, uint16_t h) {
  d.clearDisplay();
  int x = (SCREEN_WIDTH  - (int)w)/2;
  int y = (SCREEN_HEIGHT - (int)h)/2;
  d.drawBitmap(x, y, frame, w, h, WHITE);
  d.display();
}

void drawFrameSmile(Adafruit_SSD1306 &d, const uint8_t *frame, uint16_t w, uint16_t h) {
  d.clearDisplay();
  int x = (SCREEN_WIDTH  - (int)w)/2;
  int y = (SCREEN_HEIGHT - (int)h)/2;
  d.drawBitmap(x, y, frame, w, h, WHITE);
  d.display();
}

void drawFrameMove(Adafruit_SSD1306 &d, const uint8_t *frame, uint16_t w, uint16_t h){
  d.clearDisplay();
  int x = (SCREEN_WIDTH  - (int)w)/2;
  int y = (SCREEN_HEIGHT - (int)h)/2;
  d.drawBitmap(x, y, frame, w, h, WHITE);
  d.display();
}

void showSmile() {
  const uint16_t frames = min((uint16_t)SMILY_LEFT_FRAME_COUNT, (uint16_t)SMILY_RIGHT_FRAME_COUNT);
  for (uint16_t f = 0; f < frames; ++f) {
    drawFrameSmile(displayL, smily_left_frames[f],  SMILY_LEFT_WIDTH, SMILY_LEFT_HEIGHT);
    drawFrameSmile(displayR, smily_right_frames[f], SMILY_RIGHT_WIDTH, SMILY_RIGHT_HEIGHT);

    uint16_t dly = smileDelayL(f); 
    if (!dly) dly = smileDelayR(f); 
    if (!dly) dly = 100;
    delay(dly);
  }
}

void showMoveEyes() {
  const uint16_t frames = min((uint16_t)MOVE_LEFT_FRAME_COUNT, (uint16_t)MOVE_RIGHT_FRAME_COUNT);
  for (uint16_t f = 0; f < frames; ++f) {
    drawFrameMove(displayL, move_left_frames[f],  MOVE_LEFT_WIDTH,  MOVE_LEFT_HEIGHT);
    drawFrameMove(displayR, move_right_frames[f], MOVE_RIGHT_WIDTH, MOVE_RIGHT_HEIGHT);

    uint16_t dly = moveDelayL(f); 
    if (!dly) dly = moveDelayR(f); 
    if (!dly) dly = 80; // fallback
    delay(dly);
  }
}


void sleep()
{
  const uint16_t frames = min((uint16_t)SLEEP2_LEFT_FRAME_COUNT, (uint16_t)SLEEP2_RIGHT_FRAME_COUNT);
  for (uint16_t f = 0; f < frames; ++f) {
    drawFrame(displayL, sleep2_left_frames[f],  SLEEP2_LEFT_WIDTH,  SLEEP2_LEFT_HEIGHT);
    drawFrame(displayR, sleep2_right_frames[f], SLEEP2_RIGHT_WIDTH, SLEEP2_RIGHT_HEIGHT);

    uint16_t dly = delayL(f); 
    if (!dly) dly = delayR(f); 
    if (!dly) dly = 100; // fallback
    delay(dly);
  }
}

void clearScreens() {
  displayL.clearDisplay(); displayL.display();
  displayR.clearDisplay(); displayR.display();
}

void updateEyes() {
  unsigned long currentTime = millis();

  if (currentTime - lastBlinkTime > blinkDelay && blinkState == 0) {
    blinkState = 1;
    lastBlinkTime = currentTime;
  }
  if (currentTime - lastBlinkTime > 150 && blinkState == 1) {
    blinkState = 0;
    lastBlinkTime = currentTime;
  }

  if (currentTime - moveTime > random(1500, 3000) && blinkState == 0) {
    int direction = random(0, 6);
    if (direction == 0) { targetOffsetX = -8; targetOffsetY = 0; }
    else if (direction == 1) { targetOffsetX = 8; targetOffsetY = 0; }
    else if (direction == 2) { targetOffsetX = 0; targetOffsetY = -6; }
    else if (direction == 3) { targetOffsetX = 0; targetOffsetY = 6; }
    else { targetOffsetX = 0; targetOffsetY = 0; }
    moveTime = currentTime;
  }

  offsetX += (targetOffsetX - offsetX) / moveSpeed;
  offsetY += (targetOffsetY - offsetY) / moveSpeed;

  display1.clearDisplay();
  display2.clearDisplay();

  if (blinkState == 0) {
    drawEye(display1, eyeX + offsetX, eyeY + offsetY, eyeWidth, eyeHeight);
    drawEye(display2, eyeX + offsetX, eyeY + offsetY, eyeWidth, eyeHeight);
  } else {
    display1.fillRect(eyeX + offsetX, eyeY + offsetY + eyeHeight / 2 - 2, eyeWidth, 4, WHITE);
    display2.fillRect(eyeX + offsetX, eyeY + offsetY + eyeHeight / 2 - 2, eyeWidth, 4, WHITE);
  }

  display1.display();
  display2.display();
}

void drawEye(Adafruit_SSD1306 &disp, int x, int y, int w, int h) {
  disp.fillRoundRect(x, y, w, h, 8, WHITE);
}

void setRingColor(Adafruit_NeoPixel &ring, uint8_t r, uint8_t g, uint8_t b) {
  for (int i = 0; i < NUMPIXELS; i++) {
    ring.setPixelColor(i, ring.Color(r, g, b));
  }
  ring.show();
}

// === Servo Motion Function ===
void waveServos() {
  myServo1.writeMicroseconds(1300);
  myServo2.writeMicroseconds(1300);
  delay(500);

  myServo1.writeMicroseconds(1500);
  myServo2.writeMicroseconds(1500);
  delay(300);

  myServo1.writeMicroseconds(1700);
  myServo2.writeMicroseconds(1700);
  delay(500);

  myServo1.writeMicroseconds(1500);
  myServo2.writeMicroseconds(1500);
}

// ====================
// === SETUP =====
// ====================
void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL);

  // Touch sensor pin
  pinMode(TOUCH_SENSOR_PIN, INPUT);
  Serial.println("TTP223 Touch Sensor Test - ESP32 S3");

  // Init both displays (code1 eyes)
  if (!display1.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Display 1 failed"));
    while (1);
  }
  if (!display2.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("Display 2 failed"));
    while (1);
  }

  display1.clearDisplay();
  display2.clearDisplay();
  display1.display();
  display2.display();

  // Init LED Rings
  ring1.begin();
  ring2.begin();

  updateEyes();   
  setRingColor(ring1, 255, 255, 0);
  setRingColor(ring2, 0, 0, 255);
  delay(500);
  setRingColor(ring1, 0, 0, 0);
  setRingColor(ring2, 0, 0, 0);

  // === Setup PIR + Smile Displays ===
  pinMode(PIR_PIN, INPUT);
  if (!displayL.begin(SSD1306_SWITCHCAPVCC, ADDR_LEFT)) while(true){}
  if (!displayR.begin(SSD1306_SWITCHCAPVCC, ADDR_RIGHT)) while(true){}

  myServo1.attach(servoPin1);
  myServo2.attach(servoPin2);
}

// ====================
// === LOOP =====
// ====================
void loop() {
  // Code1 sleep frames
  sleep();

  // Touch sensor check
  int touchState = digitalRead(TOUCH_SENSOR_PIN);
  if (touchState == HIGH) {
    showMoveEyes();
    Serial.println("Touch detected!");
    delay(300);
  }

  if (digitalRead(PIR_PIN) == HIGH) {
    showSmile();
    setRingColor(ring1, 255, 255, 0);
    setRingColor(ring2, 0, 0, 255);
    waveServos();
    //clearScreens();
  }
}