/*
  Nova Robot Dog - Final Complete Edition for ESP32 S3
  
  COMPLETE WORKING CODE - Designed specifically for ESP32 S3 pinout
  
  Features:
  - 8 Servo Motors (2 per leg) with ESP32 S3 compatible pins
  - MPU6050 Self-Balancing System
  - SD Card Learning & Memory Storage
  - OLED Real-time Display
  - Full Sensor Suite
  - Personality Development
  - Complete Command Interface
  
  Hardware Requirements:
  - ESP32 S3 (using your specific pinout)
  - 8x Servo Motors (MG996R recommended)
  - MPU6050 IMU Sensor
  - MicroSD Card Module
  - SSD1306 OLED Display (128x64)
  - HC-SR04 Ultrasonic Sensor
  - PIR Motion Sensor
  - LDR Light Sensor
  - Buzzer
  - Status LED
  
  ESP32 S3 Pin Assignments:
  - 8 Servos: GPIO 2,4,5,6,7,15,16,17
  - SD Card: GPIO 18(SCK), 19(MISO), 20(MOSI), 8(CS)
  - I2C Bus: GPIO 9(SDA), 10(SCL) for MPU6050 & OLED
  - Sensors: GPIO 1,3,35,36,37,38,39,40
  
  Version: 3.1 ESP32 S3 Edition
  Author: Enhanced Nova Dog Project
*/

#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <EEPROM.h>
#include <MPU6050_light.h>
#include <SD.h>
#include <SPI.h>
#include <ArduinoJson.h>

// ============================================================================
// ESP32 S3 HARDWARE CONFIGURATION
// ============================================================================


#define LEFT_EYE_LED_PIN  11
#define RIGHT_EYE_LED_PIN 12





// Display Setup (I2C)
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define I2C_SDA_PIN 9   // ESP32 S3 I2C
#define I2C_SCL_PIN 10  // ESP32 S3 I2C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);



// MPU6050 IMU (shares I2C bus)
MPU6050 mpu(Wire);



// 9 Servos - 2 per leg (ESP32 S3 compatible pins) and
#define NUM_SERVOS 9
#define FRONT_LEFT_SHOULDER   0
#define FRONT_LEFT_KNEE       1
#define FRONT_RIGHT_SHOULDER  2
#define FRONT_RIGHT_KNEE      3
#define REAR_LEFT_SHOULDER    4
#define REAR_LEFT_KNEE        5
#define REAR_RIGHT_SHOULDER   6
#define REAR_RIGHT_KNEE       7




// ESP32 S3 Servo pin assignments (all safe GPIO pins)
int servoPins[NUM_SERVOS] = {2, 4, 5, 6, 7, 15, 16, 17};

Servo servos[NUM_SERVOS];


// -----------------------------------------------------------------
// NEW TAIL SERVO PIN
#define TAIL_SERVO  13       // wag-tail output
#define HEAD_SERVO_PIN 39  // SPARE_PIN_1 used for moving head
Servo headServo;




// SD Card SPI pins (ESP32 S3 compatible)
#define SD_CS_PIN    8   // Chip Select
#define SD_SCK_PIN   18  // SPI Clock
#define SD_MOSI_PIN  20  // SPI Data Out  
#define SD_MISO_PIN  19  // SPI Data In

// Sensor pins (ESP32 S3 safe pins)
#define ULTRASONIC_TRIG_PIN  1   // Safe for output
#define ULTRASONIC_ECHO_PIN  3   // Safe for input
#define PIR_SENSOR_PIN       35  // Motion sensor
#define LIGHT_SENSOR_PIN     36  // Analog light sensor
#define BUZZER_PIN           37  // Audio output
#define LED_PIN              38  // Status LED
#define SPARE_PIN_1          39  // Future expansion
#define SPARE_PIN_2          40  // Future expansion

// ============================================================================
// DATA STRUCTURES
// ============================================================================

struct ServoState {
  int centerPos;
  int currentPos;
  int targetPos;
  int minPos;
  int maxPos;
  String name;
};

ServoState servoStates[NUM_SERVOS] = {
  {90, 90, 90, 30, 150, "FL_Shoulder"},  // GPIO2
  {90, 90, 90, 30, 150, "FL_Knee"},     // GPIO4
  {90, 90, 90, 30, 150, "FR_Shoulder"}, // GPIO5
  {90, 90, 90, 30, 150, "FR_Knee"},     // GPIO6
  {90, 90, 90, 30, 150, "RL_Shoulder"}, // GPIO7
  {90, 90, 90, 30, 150, "RL_Knee"},     // GPIO15
  {90, 90, 90, 30, 150, "RR_Shoulder"}, // GPIO16
  {90, 90, 90, 30, 150, "RR_Knee"},     // GPIO17
  {90, 90, 90, 45, 135, "Tail"}         // GPIO23 ← ✅ NEW LINE with correct comma above!
};


struct Personality {
  float happiness;
  float energy;
  float curiosity;
  float friendliness;
  float confidence;
  float playfulness;
  long interactions;
  long uptime;
  bool initialized;
};

Personality dogPersonality = {0.5, 0.8, 0.7, 0.6, 0.5, 0.8, 0, 0, false};

struct BalanceData {
  float pitch;
  float roll;
  float yaw;
  float pitchRate;
  float rollRate;
  bool isBalanced;
  bool fallDetected;
  unsigned long lastBalanceUpdate;
  unsigned long fallStartTime;
};

BalanceData balance = {0, 0, 0, 0, 0, true, false, 0, 0};

struct SensorData {
  float distance;
  bool motionDetected;
  float lightLevel;
  unsigned long lastMotionTime;
  float accelX, accelY, accelZ;
  float gyroX, gyroY, gyroZ;
  float temperature;
};

SensorData currentSensors = {400, false, 0.5, 0, 0, 0, 0, 0, 0, 0, 0};

struct Memory {
  String type;
  String data;
  unsigned long timestamp;
  float emotionalWeight;
};

std::vector<Memory> memories;

// ============================================================================
// SYSTEM VARIABLES
// ============================================================================

enum MovementState {
  IDLE,
  WALKING,
  SITTING,
  PLAYING,
  EXPRESSING_EMOTION,
  BALANCING,
  FALLEN,
  RECOVERING
};

MovementState currentState = IDLE;
unsigned long lastMovementUpdate = 0;
unsigned long bootTime = 0;
const int MOVEMENT_DELAY = 20;

// Balance parameters
#define BALANCE_THRESHOLD_PITCH 15.0
#define BALANCE_THRESHOLD_ROLL  15.0
#define FALL_THRESHOLD_PITCH    45.0
#define FALL_THRESHOLD_ROLL     45.0
#define BALANCE_UPDATE_RATE     50
#define BALANCE_CORRECTION_GAIN 2.0

bool balanceEnabled = true;
bool autoRecovery = true;
bool sdCardWorking = false;

// ============================================================================
// SETUP FUNCTION
// ============================================================================

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  bootTime = millis();
  
  Serial.println("🐕==========================================");
  Serial.println("🐕    NOVA DOG - ESP32 S3 EDITION       ");
  Serial.println("🐕   Complete Self-Balancing Learning    ");
  Serial.println("🐕==========================================");
  Serial.println();
  
  // Initialize hardware with ESP32 S3 pins
  Serial.println("🔧 Initializing ESP32 S3 hardware...");
  initializeHardware();

    // Initialize head servo for ultrasonic scanning
  headServo.attach(HEAD_SERVO_PIN);
  headServo.write(90);  // Center position
  Serial.printf("🔄 Head servo (ultrasonic scan) → GPIO%d\n", HEAD_SERVO_PIN);

  
  // Initialize SD card
  Serial.println("💾 Initializing SD card...");
  initializeSDCard();
  
  // Initialize MPU6050
  Serial.println("⚖️  Initializing MPU6050...");
  initializeMPU6050();
  
  // Load personality and memories
  Serial.println("🧠 Loading personality and memories...");
  loadPersonalityFromStorage();
  loadMemoriesFromSD();
  
  // Create directory structure
  if (sdCardWorking) {
    createDirectoryStructure();
  }
  
  // Move to idle position
  Serial.println("🏠 Moving to idle position...");
  moveToIdlePosition();
  
  // Display startup message
  displayStartupMessage();
  
  // Log boot event
  logEvent("system", "boot", "Nova Dog ESP32 S3 started");
  
  // Ready!
  Serial.println("✅ Nova Dog ESP32 S3 ready with full learning!");
  printCommands();
  
  // Happy startup sequence
  delay(1000);
  wagTail();
  playHappySound();
  
  Serial.println("🐕 Ready for commands and learning!");
}

// ============================================================================
// MAIN LOOP
// ============================================================================

void loop() {
  unsigned long currentTime = millis();
  
  // Update sensors including MPU6050
  updateSensors();
  
  // Update balance system
  updateBalance(currentTime);
  
  // Apply balance corrections if enabled
  if (balanceEnabled && currentState != FALLEN) {
    applyBalanceCorrections();
  }
  
  // Update servo movements with smooth interpolation
  updateMovements(currentTime);
  
  // Handle fall detection and recovery
  handleFallDetection();
  
  // Update personality based on experiences
  updatePersonality(currentTime);
  
  // Update OLED display
  updateDisplay();
  
  // Process serial commands
  if (Serial.available()) {
    processSerialCommand();
  }
  
  // Random personality-based behaviors (only when balanced)
  if (balance.isBalanced && random(1000) < dogPersonality.playfulness * 2) {
    performRandomBehavior();
  }
  
  // React to environmental changes
  reactToEnvironment();
  
  // Periodic comprehensive status update
  static unsigned long lastStatusUpdate = 0;
  if (currentTime - lastStatusUpdate > 300000) { // Every 5 minutes
    Serial.println("💝 Automatic status update:");
    printStatus();
    lastStatusUpdate = currentTime;
  }

  blinkEyesRandomly();
  
  delay(20); // 50Hz main loop
}

// ============================================================================
// HARDWARE INITIALIZATION
// ============================================================================

void initializeHardware() {
  // Initialize I2C with ESP32 S3 pins
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  Serial.printf("📡 I2C initialized: SDA=%d, SCL=%d\n", I2C_SDA_PIN, I2C_SCL_PIN);
  
  // Initialize servos on ESP32 S3 pins
  Serial.println("⚙️  Initializing 8 servos on ESP32 S3...");
  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(servoStates[i].centerPos);
    Serial.printf("   Servo %d (%s) → GPIO%d\n", i+1, servoStates[i].name.c_str(), servoPins[i]);
    delay(100);
  }
  
  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("⚠️ OLED not found on I2C");
  } else {
    Serial.println("📺 OLED display initialized");
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("Nova Dog ESP32 S3");
    display.println("Initializing...");
    display.display();
  }
  
  // Initialize sensor pins
  pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
  pinMode(ULTRASONIC_ECHO_PIN, INPUT);
  pinMode(PIR_SENSOR_PIN, INPUT);
  pinMode(LIGHT_SENSOR_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  pinMode(LEFT_EYE_LED_PIN, OUTPUT);
pinMode(RIGHT_EYE_LED_PIN, OUTPUT);
digitalWrite(LEFT_EYE_LED_PIN, LOW);
digitalWrite(RIGHT_EYE_LED_PIN, LOW);

  
  // Test LED
  digitalWrite(LED_PIN, HIGH);
  delay(200);
  digitalWrite(LED_PIN, LOW);
  
  // Initialize EEPROM
  EEPROM.begin(512);
  
  Serial.println("✅ ESP32 S3 hardware initialization complete");
  Serial.println("📊 Pin assignments:");
  Serial.printf("   Servos: ");
  for (int i = 0; i < NUM_SERVOS; i++) {
    Serial.printf("GPIO%d ", servoPins[i]);
  }
  Serial.println();
  Serial.printf("   SD Card: CS=GPIO%d, SCK=GPIO%d, MOSI=GPIO%d, MISO=GPIO%d\n", 
                SD_CS_PIN, SD_SCK_PIN, SD_MOSI_PIN, SD_MISO_PIN);
  Serial.printf("   I2C: SDA=GPIO%d, SCL=GPIO%d\n", I2C_SDA_PIN, I2C_SCL_PIN);
  Serial.printf("   Sensors: Trig=GPIO%d, Echo=GPIO%d, PIR=GPIO%d, Light=GPIO%d\n", 
                ULTRASONIC_TRIG_PIN, ULTRASONIC_ECHO_PIN, PIR_SENSOR_PIN, LIGHT_SENSOR_PIN);
}

void initializeSDCard() {
  // Initialize SPI with ESP32 S3 specific pins
  SPI.begin(SD_SCK_PIN, SD_MISO_PIN, SD_MOSI_PIN, SD_CS_PIN);
  
  if (!SD.begin(SD_CS_PIN)) {
    Serial.println("❌ SD Card initialization failed!");
    Serial.println("   Continuing with EEPROM-only storage...");
    sdCardWorking = false;
    return;
  }
  
  Serial.println("💾 SD Card initialized successfully on ESP32 S3");
  sdCardWorking = true;
  
  // Test SD card functionality
  File testFile = SD.open("/test.txt", FILE_WRITE);
  if (testFile) {
    testFile.println("Nova Dog ESP32 S3 SD Test");
    testFile.close();
    SD.remove("/test.txt");
    Serial.println("📝 SD Card write test passed");
  } else {
    Serial.println("⚠️ SD Card write test failed");
    sdCardWorking = false;
  }
}

void initializeMPU6050() {
  if (mpu.begin() != 0) {
    Serial.println("❌ MPU6050 initialization failed!");
    Serial.println("   Check I2C connections (SDA=GPIO9, SCL=GPIO10)");
    Serial.println("   Continuing without balance control...");
    balanceEnabled = false;
    return;
  }
  
  Serial.println("⚖️  MPU6050 connected successfully on I2C");
  Serial.println("📐 Calibrating gyroscope... (keep robot still)");
  
  // Visual calibration indicator
  for (int i = 0; i < 10; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    digitalWrite(LED_PIN, LOW);
    delay(100);
  }
  
  mpu.calcOffsets();
  Serial.println("✅ MPU6050 calibration complete");
  
  // Test initial readings
  mpu.update();
  Serial.printf("📊 Initial readings - Pitch: %.1f°, Roll: %.1f°, Yaw: %.1f°\n", 
                mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
  
  balanceEnabled = true;
  Serial.println("🎯 Self-balancing system ACTIVE");
}

// ============================================================================
// SD CARD & LEARNING SYSTEM
// ============================================================================

void createDirectoryStructure() {
  if (!sdCardWorking) return;
  
  Serial.println("📁 Creating directory structure...");
  
  // Create main directories
  SD.mkdir("/logs");
  SD.mkdir("/personality");
  SD.mkdir("/memories");
  SD.mkdir("/behaviors");
  SD.mkdir("/interactions");
  SD.mkdir("/balance");
  SD.mkdir("/learning");
  SD.mkdir("/esp32s3");
  
  // Create dated log directory
  time_t now = millis() / 1000;
  struct tm* timeinfo = localtime(&now);
  
  char dateStr[32];
  strftime(dateStr, sizeof(dateStr), "/logs/%Y-%m-%d", timeinfo);
  SD.mkdir(dateStr);
  
  Serial.println("✅ Directory structure created for ESP32 S3");
}

void logEvent(String category, String action, String details) {
  // Log to serial
  Serial.printf("📝 LOG: [%s] %s - %s\n", category.c_str(), action.c_str(), details.c_str());
  
  if (!sdCardWorking) return;
  
  DynamicJsonDocument doc(1024);
  doc["timestamp"] = millis();
  doc["uptime"] = (millis() - bootTime) / 1000;
  doc["category"] = category;
  doc["action"] = action;
  doc["details"] = details;
  doc["hardware"] = "ESP32_S3";
  doc["personality"]["happiness"] = dogPersonality.happiness;
  doc["personality"]["confidence"] = dogPersonality.confidence;
  doc["personality"]["energy"] = dogPersonality.energy;
  doc["balance"]["pitch"] = balance.pitch;
  doc["balance"]["roll"] = balance.roll;
  doc["balance"]["isBalanced"] = balance.isBalanced;
  
  // Write to daily log file
  time_t now = millis() / 1000;
  struct tm* timeinfo = localtime(&now);
  
  char filename[64];
  strftime(filename, sizeof(filename), "/logs/%Y-%m-%d/events.json", timeinfo);
  
  File file = SD.open(filename, FILE_APPEND);
  if (file) {
    serializeJson(doc, file);
    file.println();
    file.close();
  }
}

void savePersonalityToSD() {
  if (!sdCardWorking) {
    savePersonalityToEEPROM();
    return;
  }
  
  DynamicJsonDocument doc(1024);
  doc["hardware"] = "ESP32_S3";
  doc["version"] = "3.1";
  doc["happiness"] = dogPersonality.happiness;
  doc["energy"] = dogPersonality.energy;
  doc["curiosity"] = dogPersonality.curiosity;
  doc["friendliness"] = dogPersonality.friendliness;
  doc["confidence"] = dogPersonality.confidence;
  doc["playfulness"] = dogPersonality.playfulness;
  doc["interactions"] = dogPersonality.interactions;
  doc["uptime"] = dogPersonality.uptime;
  doc["last_save"] = millis();
  doc["balance_enabled"] = balanceEnabled;
  doc["servo_pins"] = "[2,4,5,6,7,15,16,17]";
  
  File file = SD.open("/personality/current.json", FILE_WRITE);
  if (file) {
    serializeJson(doc, file);
    file.close();
    Serial.println("💾 Personality saved to SD card (ESP32 S3)");
  }
  
  // Also save to EEPROM as backup
  savePersonalityToEEPROM();
}

void loadPersonalityFromStorage() {
  bool loadedFromSD = false;
  
  if (sdCardWorking) {
    File file = SD.open("/personality/current.json");
    if (file) {
      DynamicJsonDocument doc(1024);
      DeserializationError error = deserializeJson(doc, file);
      
      if (!error) {
        dogPersonality.happiness = doc["happiness"] | 0.5;
        dogPersonality.energy = doc["energy"] | 0.8;
        dogPersonality.curiosity = doc["curiosity"] | 0.7;
        dogPersonality.friendliness = doc["friendliness"] | 0.6;
        dogPersonality.confidence = doc["confidence"] | 0.5;
        dogPersonality.playfulness = doc["playfulness"] | 0.8;
        dogPersonality.interactions = doc["interactions"] | 0;
        dogPersonality.uptime = doc["uptime"] | 0;
        
        loadedFromSD = true;
        Serial.println("📂 Personality loaded from SD card");
        Serial.printf("   Previous interactions: %lu\n", dogPersonality.interactions);
        
        String hardware = doc["hardware"] | "Unknown";
        if (hardware == "ESP32_S3") {
          Serial.println("✅ ESP32 S3 configuration detected");
        }
      }
      file.close();
    }
  }
  
  if (!loadedFromSD) {
    loadPersonalityFromEEPROM();
  }
}

void saveMemoryToSD(Memory memory) {
  if (!sdCardWorking) return;
  
  char filename[64];
  sprintf(filename, "/memories/%lu.json", memory.timestamp);
  
  DynamicJsonDocument doc(512);
  doc["type"] = memory.type;
  doc["data"] = memory.data;
  doc["timestamp"] = memory.timestamp;
  doc["emotional_weight"] = memory.emotionalWeight;
  doc["uptime"] = (millis() - bootTime) / 1000;
  doc["hardware"] = "ESP32_S3";
  
  File file = SD.open(filename, FILE_WRITE);
  if (file) {
    serializeJson(doc, file);
    file.close();
  }
}

void loadMemoriesFromSD() {
  if (!sdCardWorking) return;
  
  File dir = SD.open("/memories");
  if (!dir) {
    Serial.println("📂 No memories directory found");
    return;
  }
  
  memories.clear();
  int loadedCount = 0;
  
  while (true) {
    File entry = dir.openNextFile();
    if (!entry) break;
    
    if (!entry.isDirectory() && String(entry.name()).endsWith(".json")) {
      DynamicJsonDocument doc(512);
      DeserializationError error = deserializeJson(doc, entry);
      
      if (!error) {
        Memory memory;
        memory.type = doc["type"].as<String>();
        memory.data = doc["data"].as<String>();
        memory.timestamp = doc["timestamp"];
        memory.emotionalWeight = doc["emotional_weight"] | 0.5;
        
        memories.push_back(memory);
        loadedCount++;
        
        if (loadedCount >= 100) break; // Limit memory count
      }
    }
    entry.close();
  }
  dir.close();
  
  Serial.printf("🧠 Loaded %d memories from SD card\n", loadedCount);
}

// ============================================================================
// BALANCE SYSTEM
// ============================================================================

void updateBalance(unsigned long currentTime) {
  if (!balanceEnabled) return;
  
  if (currentTime - balance.lastBalanceUpdate < (1000 / BALANCE_UPDATE_RATE)) {
    return;
  }
  
  mpu.update();
  
  float newPitch = mpu.getAngleX();
  float newRoll = mpu.getAngleY();
  float newYaw = mpu.getAngleZ();
  
  balance.pitchRate = (newPitch - balance.pitch) * BALANCE_UPDATE_RATE;
  balance.rollRate = (newRoll - balance.roll) * BALANCE_UPDATE_RATE;
  
  balance.pitch = newPitch;
  balance.roll = newRoll;
  balance.yaw = newYaw;
  
  currentSensors.accelX = mpu.getAccX();
  currentSensors.accelY = mpu.getAccY();
  currentSensors.accelZ = mpu.getAccZ();
  currentSensors.gyroX = mpu.getGyroX();
  currentSensors.gyroY = mpu.getGyroY();
  currentSensors.gyroZ = mpu.getGyroZ();
  currentSensors.temperature = mpu.getTemp();
  
  balance.isBalanced = (abs(balance.pitch) < BALANCE_THRESHOLD_PITCH) && 
                     (abs(balance.roll) < BALANCE_THRESHOLD_ROLL);
  
  if (abs(balance.pitch) > FALL_THRESHOLD_PITCH || abs(balance.roll) > FALL_THRESHOLD_ROLL) {
    if (!balance.fallDetected) {
      balance.fallDetected = true;
      balance.fallStartTime = currentTime;
      Serial.println("🚨 FALL DETECTED!");
      logEvent("balance", "fall_detected", "Fall threshold exceeded");
    }
  } else {
    balance.fallDetected = false;
  }
  
  balance.lastBalanceUpdate = currentTime;
  
  static unsigned long lastDebug = 0;
  if (currentTime - lastDebug > 2000) {
    Serial.printf("⚖️  Balance: Pitch=%.1f° Roll=%.1f° %s\n", 
                  balance.pitch, balance.roll, 
                  balance.isBalanced ? "✅BALANCED" : "⚠️UNBALANCED");
    lastDebug = currentTime;
  }
}

void applyBalanceCorrections() {
  if (!balance.isBalanced) {
    currentState = BALANCING;
    
    float pitchCorrection = balance.pitch * BALANCE_CORRECTION_GAIN;
    float rollCorrection = balance.roll * BALANCE_CORRECTION_GAIN;
    
    if (abs(balance.pitch) > 5.0) {
      int frontCorrection = constrain(pitchCorrection, -20, 20);
      int rearCorrection = constrain(-pitchCorrection, -20, 20);
      
      setServoPosition(FRONT_LEFT_KNEE, servoStates[FRONT_LEFT_KNEE].centerPos + frontCorrection);
      setServoPosition(FRONT_RIGHT_KNEE, servoStates[FRONT_RIGHT_KNEE].centerPos + frontCorrection);
      setServoPosition(REAR_LEFT_KNEE, servoStates[REAR_LEFT_KNEE].centerPos + rearCorrection);
      setServoPosition(REAR_RIGHT_KNEE, servoStates[REAR_RIGHT_KNEE].centerPos + rearCorrection);
    }
    
    if (abs(balance.roll) > 5.0) {
      int leftCorrection = constrain(rollCorrection, -20, 20);
      int rightCorrection = constrain(-rollCorrection, -20, 20);
      
      setServoPosition(FRONT_LEFT_KNEE, servoStates[FRONT_LEFT_KNEE].targetPos + leftCorrection);
      setServoPosition(REAR_LEFT_KNEE, servoStates[REAR_LEFT_KNEE].targetPos + leftCorrection);
      setServoPosition(FRONT_RIGHT_KNEE, servoStates[FRONT_RIGHT_KNEE].targetPos + rightCorrection);
      setServoPosition(REAR_RIGHT_KNEE, servoStates[REAR_RIGHT_KNEE].targetPos + rightCorrection);
    }
  } else if (currentState == BALANCING) {
    currentState = IDLE;
  }
}

void handleFallDetection() {
  if (balance.fallDetected && currentState != FALLEN && currentState != RECOVERING) {
    Serial.println("🆘 Initiating fall response...");
    currentState = FALLEN;
    
    for (int i = 0; i < NUM_SERVOS; i++) {
      setServoPosition(i, servoStates[i].centerPos);
    }
    
    playDistressSound();
    dogPersonality.confidence = max(0.1, dogPersonality.confidence - 0.1);
    
    logEvent("balance", "fall_response", "Emergency fall response activated");
    
    if (autoRecovery) {
      delay(1000);
      attemptRecovery();
    }
  }
}

void attemptRecovery() {
  Serial.println("🔄 Attempting recovery...");
  currentState = RECOVERING;
  
  for (int attempt = 0; attempt < 3; attempt++) {
    Serial.printf("Recovery attempt %d/3\n", attempt + 1);
    
    moveToIdlePosition();
    delay(1000);
    
    mpu.update();
    if (abs(mpu.getAngleX()) < BALANCE_THRESHOLD_PITCH && 
        abs(mpu.getAngleY()) < BALANCE_THRESHOLD_ROLL) {
      Serial.println("✅ Recovery successful!");
      currentState = IDLE;
      balance.fallDetected = false;
      dogPersonality.confidence = min(1.0, dogPersonality.confidence + 0.05);
      playHappySound();
      logEvent("balance", "recovery_success", "Recovery completed successfully");
      return;
    }
    delay(500);
  }
  
  Serial.println("❌ Recovery failed - manual intervention required");
  playDistressSound();
  logEvent("balance", "recovery_failed", "Recovery attempts unsuccessful");
}

// ============================================================================
// SERVO CONTROL SYSTEM
// ============================================================================

void setServoPosition(int servoNum, int angle) {
  if (servoNum < 0 || servoNum >= NUM_SERVOS) return;
  
  angle = constrain(angle, servoStates[servoNum].minPos, servoStates[servoNum].maxPos);
  servoStates[servoNum].targetPos = angle;
}

void moveToIdlePosition() {
  Serial.println("🏠 Moving to idle position");
  for (int i = 0; i < NUM_SERVOS; i++) {
    setServoPosition(i, servoStates[i].centerPos);
  }
  currentState = IDLE;
}

void updateMovements(unsigned long currentTime) {
  if (currentTime - lastMovementUpdate < MOVEMENT_DELAY) return;
  
  for (int i = 0; i < NUM_SERVOS; i++) {
    if (servoStates[i].currentPos != servoStates[i].targetPos) {
      int diff = servoStates[i].targetPos - servoStates[i].currentPos;
      int step = (diff > 0) ? min(2, diff) : max(-2, diff);
      servoStates[i].currentPos += step;
      servos[i].write(servoStates[i].currentPos);
    }
  }
  
  lastMovementUpdate = currentTime;
}

// ============================================================================
// SENSOR READING SYSTEM
// ============================================================================

void updateSensors() {
  currentSensors.distance = readUltrasonicDistance();
  
  bool currentMotion = digitalRead(PIR_SENSOR_PIN);
  if (currentMotion && !currentSensors.motionDetected) {
    currentSensors.lastMotionTime = millis();
  }
  currentSensors.motionDetected = currentMotion;
  
  int lightReading = analogRead(LIGHT_SENSOR_PIN);
  currentSensors.lightLevel = lightReading / 4095.0;
}

float readUltrasonicDistance() {
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  
  long duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH, 30000);
  
  if (duration == 0) return 400;
  
  float distance = duration * 0.034 / 2;
  return constrain(distance, 2, 400);
}


// ============================================================================
// AUDIO FEEDBACK SYSTEM
// ============================================================================

void playHappySound() {
  for (int i = 0; i < 3; i++) {
    tone(BUZZER_PIN, 800 + i * 200, 120);
    delay(180);
    noTone(BUZZER_PIN);
  }
}

void playPlayfulSound() {
  tone(BUZZER_PIN, 500, 100);
  delay(120);
  tone(BUZZER_PIN, 700, 100);
  delay(120);
  noTone(BUZZER_PIN);
}

void playCuriousSound() {
  tone(BUZZER_PIN, 600, 150);
  delay(80);
  tone(BUZZER_PIN, 800, 150);
  delay(80);
  noTone(BUZZER_PIN);
}

void playDistressSound() {
  for (int i = 0; i < 5; i++) {
    tone(BUZZER_PIN, 300, 100);
    delay(150);
    tone(BUZZER_PIN, 500, 100);
    delay(150);
    noTone(BUZZER_PIN);
  }
}

void playBalanceAlert() {
  tone(BUZZER_PIN, 1000, 50);
  delay(100);
  tone(BUZZER_PIN, 800, 50);
  delay(100);
  noTone(BUZZER_PIN);
}

// ============================================================================
// EMOTIONAL EXPRESSIONS & BEHAVIORS
// ============================================================================
void blinkEyesRandomly() {
  static unsigned long lastBlink = 0;
  static bool isBlinking = false;
  unsigned long now = millis();

  if (!isBlinking && random(1000) < 5) {  // ~0.5% chance per loop
    digitalWrite(LEFT_EYE_LED_PIN, HIGH);
    digitalWrite(RIGHT_EYE_LED_PIN, HIGH);
    lastBlink = now;
    isBlinking = true;
  }

  if (isBlinking && (now - lastBlink > random(100, 300))) {
    digitalWrite(LEFT_EYE_LED_PIN, LOW);
    digitalWrite(RIGHT_EYE_LED_PIN, LOW);
    isBlinking = false;
  }
}



void wagTail() {
  if (!balance.isBalanced) {
    Serial.println("⚠️ Cannot wag tail – not balanced");
    playBalanceAlert();
    return;
  }

  Serial.println("🐕 Wagging tail!");
  currentState = EXPRESSING_EMOTION;

  const int wagLeft  = 60;    // adjust for your linkage
  const int wagRight = 120;
  const int centre   = 90;

  for (int i = 0; i < 3; i++) {               // three wags
    setServoPosition(TAIL_SERVO, wagLeft);
    delay(200);
    setServoPosition(TAIL_SERVO, wagRight);
    delay(200);
  }
  setServoPosition(TAIL_SERVO, centre);

  dogPersonality.happiness = min(1.0, dogPersonality.happiness + 0.1);
  playHappySound();

  Memory m;
  m.type            = "emotion";
  m.data            = "tail_wag";
  m.timestamp       = millis();
  m.emotionalWeight = 0.8;
  memories.push_back(m);
  saveMemoryToSD(m);

  logEvent("emotion", "happy", "Tail wag – happiness ↑");
  currentState = IDLE;
}

void expressEmotion(String emotion) {
  if (!balance.isBalanced && emotion != "recovery") {
    Serial.println("⚠️ Cannot express emotion - not balanced");
    playBalanceAlert();
    logEvent("emotion", "blocked", "Emotion blocked due to imbalance: " + emotion);
    return;
  }
  
  Serial.println("😊 Expressing: " + emotion);
  currentState = EXPRESSING_EMOTION;
  
  if (emotion == "happy") {
    wagTail();
  } else if (emotion == "curious") {
    setServoPosition(FRONT_LEFT_SHOULDER, 60);
    setServoPosition(FRONT_RIGHT_SHOULDER, 120);
    delay(1200);
    setServoPosition(FRONT_LEFT_SHOULDER, 90);
    setServoPosition(FRONT_RIGHT_SHOULDER, 90);
    dogPersonality.curiosity = min(1.0, dogPersonality.curiosity + 0.05);
    playCuriousSound();
    
    Memory curiousMemory;
    curiousMemory.type = "emotion";
    curiousMemory.data = "head_tilt_curious";
    curiousMemory.timestamp = millis();
    curiousMemory.emotionalWeight = 0.6;
    memories.push_back(curiousMemory);
    saveMemoryToSD(curiousMemory);
    
    logEvent("emotion", "curious", "Head tilt - curiosity increased");
    
  } else if (emotion == "playful") {
    performPlayBow();
    dogPersonality.playfulness = min(1.0, dogPersonality.playfulness + 0.05);
    
  } else if (emotion == "recovery") {
    playHappySound();
    delay(500);
    wagTail();
    logEvent("emotion", "recovery", "Celebrating successful recovery");
  }
  
  digitalWrite(LED_PIN, HIGH);
  delay(300);
  digitalWrite(LED_PIN, LOW);
  
  currentState = IDLE;
}

void performPlayBow() {
  Serial.println("🎾 Play bow!");
  
  setServoPosition(FRONT_LEFT_KNEE, 45);
  setServoPosition(FRONT_RIGHT_KNEE, 45);
  delay(1000);
  
  setServoPosition(FRONT_LEFT_KNEE, 90);
  setServoPosition(FRONT_RIGHT_KNEE, 90);
  
  playPlayfulSound();
  
  Memory playMemory;
  playMemory.type = "emotion";
  playMemory.data = "play_bow";
  playMemory.timestamp = millis();
  playMemory.emotionalWeight = 0.9;
  memories.push_back(playMemory);
  saveMemoryToSD(playMemory);
  
  logEvent("emotion", "playful", "Play bow performed");
}

void performSit() {
  if (!balance.isBalanced) {
    Serial.println("⚠️ Cannot sit - not balanced");
    playBalanceAlert();
    logEvent("movement", "sit_blocked", "Sit command blocked - not balanced");
    return;
  }
  
  Serial.println("🪑 Sitting down");
  
  for (int angle = 90; angle >= 45; angle -= 5) {
    setServoPosition(REAR_LEFT_KNEE, angle);
    setServoPosition(REAR_RIGHT_KNEE, angle);
    delay(50);
  }
  
  currentState = SITTING;
  logEvent("movement", "sit", "Successfully sat down");
}

void performWalk() {
  if (!balance.isBalanced) {
    Serial.println("⚠️ Cannot walk - not balanced");
    playBalanceAlert();
    logEvent("movement", "walk_blocked", "Walk command blocked - not balanced");
    return;
  }
  
  Serial.println("🚶 Walking with balance control!");
  currentState = WALKING;
  
  for (int cycle = 0; cycle < 2; cycle++) {
    mpu.update();
    if (abs(mpu.getAngleX()) > BALANCE_THRESHOLD_PITCH || 
        abs(mpu.getAngleY()) > BALANCE_THRESHOLD_ROLL) {
      Serial.println("⚠️ Balance lost during walk - stopping");
      moveToIdlePosition();
      logEvent("movement", "walk_interrupted", "Walk stopped due to balance loss");
      return;
    }
    
    // Walking gait phases
    setServoPosition(FRONT_LEFT_KNEE, 120);    
    setServoPosition(REAR_RIGHT_KNEE, 120);    
    delay(300);
    
    setServoPosition(FRONT_LEFT_SHOULDER, 110); 
    setServoPosition(REAR_RIGHT_SHOULDER, 110); 
    delay(200);
    
    setServoPosition(FRONT_LEFT_KNEE, 90);     
    setServoPosition(REAR_RIGHT_KNEE, 90);     
    delay(200);
    
    setServoPosition(FRONT_RIGHT_KNEE, 120);   
    setServoPosition(REAR_LEFT_KNEE, 120);     
    delay(300);
    
    setServoPosition(FRONT_RIGHT_SHOULDER, 110); 
    setServoPosition(REAR_LEFT_SHOULDER, 110);   
    delay(200);
    
    setServoPosition(FRONT_RIGHT_KNEE, 90);    
    setServoPosition(REAR_LEFT_KNEE, 90);      
    delay(200);
  }
  
  moveToIdlePosition();
  
  Memory walkMemory;
  walkMemory.type = "movement";
  walkMemory.data = "successful_walk";
  walkMemory.timestamp = millis();
  walkMemory.emotionalWeight = 0.7;
  memories.push_back(walkMemory);
  saveMemoryToSD(walkMemory);
  
  dogPersonality.confidence = min(1.0, dogPersonality.confidence + 0.02);
  
  Serial.println("✅ Walk completed successfully");
  logEvent("movement", "walk_success", "Walk completed with balance maintained");
}

// ============================================================================
// PERSONALITY & LEARNING SYSTEM
// ============================================================================

void savePersonalityToEEPROM() {
  dogPersonality.initialized = true;
  EEPROM.put(0, dogPersonality);
  EEPROM.commit();
  Serial.println("💾 Personality saved to EEPROM (backup)");
}

void loadPersonalityFromEEPROM() {
  Personality loadedPersonality;
  EEPROM.get(0, loadedPersonality);
  
  if (loadedPersonality.initialized) {
    dogPersonality = loadedPersonality;
    Serial.println("📂 Personality loaded from EEPROM");
    Serial.printf("   Previous interactions: %lu\n", dogPersonality.interactions);
    Serial.printf("   Confidence level: %.2f\n", dogPersonality.confidence);
  } else {
    Serial.println("🆕 New personality created");
    savePersonalityToEEPROM();
  }
}

void updatePersonality(unsigned long currentTime) {
  static unsigned long lastPersonalityUpdate = 0;
  
  if (currentTime - lastPersonalityUpdate < 30000) return;
  
  if (balance.isBalanced) {
    dogPersonality.confidence = min(1.0, dogPersonality.confidence + 0.001);
  } else {
    dogPersonality.confidence = max(0.1, dogPersonality.confidence - 0.002);
  }
  
  if (balance.fallDetected) {
    dogPersonality.energy = max(0.2, dogPersonality.energy - 0.05);
    dogPersonality.confidence = max(0.1, dogPersonality.confidence - 0.1);
  }
  
  if (dogPersonality.interactions > 25) {
    dogPersonality.friendliness = min(1.0, dogPersonality.friendliness + 0.001);
  }
  
  dogPersonality.energy = max(0.3, dogPersonality.energy - 0.0005);
  dogPersonality.uptime = (currentTime - bootTime) / 60000;
  
  static int saveCounter = 0;
  if (++saveCounter >= 10) {
    if (sdCardWorking) {
      savePersonalityToSD();
    } else {
      savePersonalityToEEPROM();
    }
    saveCounter = 0;
  }
  
  lastPersonalityUpdate = currentTime;
}

void performRandomBehavior() {
  if (!balance.isBalanced) return;
  
  String behaviors[] = {"happy", "curious"};
  String randomBehavior = behaviors[random(2)];
  
  if (dogPersonality.playfulness > 0.8 && random(100) < 30) {
    randomBehavior = "playful";
  }
  
  expressEmotion(randomBehavior);
  logEvent("behavior", "random", "Performed: " + randomBehavior);
}


float scanForObstacle() {
  float distances[3];

  // Move head left
  headServo.write(45);
  delay(300);
  distances[0] = readUltrasonicDistance();

  // Move head center
  headServo.write(90);
  delay(300);
  distances[1] = readUltrasonicDistance();

  // Move head right
  headServo.write(135);
  delay(300);
  distances[2] = readUltrasonicDistance();

  // Return to center
  headServo.write(90);

  Serial.printf("🔍 Scan: L=%.1f C=%.1f R=%.1f\n", distances[0], distances[1], distances[2]);

  float minDist = min(distances[0], min(distances[1], distances[2]));
  return minDist;
}


float scanForObstacle();  // Forward declaration for ultrasonic scan function

void reactToEnvironment() {
if (balance.isBalanced && random(100) < 15) {
  float scan = scanForObstacle();

  if (scan < 20) {
    expressEmotion("happy");
    logEvent("interaction", "scan", "Close object detected <20cm");

    Memory m;
    m.type = "scan";
    m.data = "happy_detect_close";
    m.timestamp = millis();
    m.emotionalWeight = 0.7;
    memories.push_back(m);
    saveMemoryToSD(m);

  } else if (scan < 50) {
    expressEmotion("curious");
    logEvent("interaction", "scan", "Object detected <50cm");

    Memory m;
    m.type = "scan";
    m.data = "curious_detect_near";
    m.timestamp = millis();
    m.emotionalWeight = 0.5;
    memories.push_back(m);
    saveMemoryToSD(m);
  }
}

  
  if (currentSensors.motionDetected && random(100) < 15) {
    if (balance.isBalanced) {
      expressEmotion("curious");
      logEvent("interaction", "motion", "Motion detected");
    } else {
      playBalanceAlert();
      logEvent("system", "motion_while_unbalanced", "Motion detected but unbalanced");
    }
  }
  
  if (!balance.isBalanced && currentState != BALANCING && 
      currentState != FALLEN && currentState != RECOVERING) {
    static unsigned long lastBalanceAlert = 0;
    if (millis() - lastBalanceAlert > 5000) {
      playBalanceAlert();
      logEvent("balance", "imbalance_reaction", "Reacting to loss of balance");
      lastBalanceAlert = millis();
    }
  }
}

// ============================================================================
// COMMAND PROCESSING SYSTEM
// ============================================================================

void processSerialCommand() {
  String command = Serial.readStringUntil('\n');
  command.trim();
  command.toLowerCase();
  
  Serial.println("📞 Command: " + command);
  dogPersonality.interactions++;
  
  Memory commandMemory;
  commandMemory.type = "command";
  commandMemory.data = command;
  commandMemory.timestamp = millis();
  commandMemory.emotionalWeight = 0.5;
  
  if (command == "sit") {
    performSit();
    commandMemory.emotionalWeight = 0.7;
  } else if (command == "play") {
    expressEmotion("playful");
    commandMemory.emotionalWeight = 0.9;
  } else if (command == "happy") {
    expressEmotion("happy");
    commandMemory.emotionalWeight = 0.8;
  } else if (command == "curious") {
    expressEmotion("curious");
    commandMemory.emotionalWeight = 0.6;
  } else if (command == "walk") {
    performWalk();
    commandMemory.emotionalWeight = 0.8;
  } else if (command == "reset") {
    moveToIdlePosition();
    commandMemory.emotionalWeight = 0.3;
  } else if (command == "status") {
    printStatus();
    commandMemory.emotionalWeight = 0.2;
  } else if (command == "balance") {
    printBalanceStatus();
    commandMemory.emotionalWeight = 0.2;
  } else if (command == "calibrate") {
    calibrateMPU6050();
    commandMemory.emotionalWeight = 0.4;
  } else if (command == "recovery") {
    attemptRecovery();
    commandMemory.emotionalWeight = 0.6;
  } else if (command == "balance_on") {
    balanceEnabled = true;
    Serial.println("✅ Balance control ENABLED");
    logEvent("system", "balance_enabled", "Balance control manually enabled");
    commandMemory.emotionalWeight = 0.5;
  } else if (command == "balance_off") {
    balanceEnabled = false;
    Serial.println("⚠️ Balance control DISABLED");
    logEvent("system", "balance_disabled", "Balance control manually disabled");
    commandMemory.emotionalWeight = 0.3;
  } else if (command == "save") {
    if (sdCardWorking) {
      savePersonalityToSD();
    } else {
      savePersonalityToEEPROM();
    }
    Serial.println("💾 Data saved manually");
    commandMemory.emotionalWeight = 0.3;
  } else if (command == "memories") {
    printMemories();
    commandMemory.emotionalWeight = 0.2;
  } else if (command == "pins") {
    printPinAssignments();
    commandMemory.emotionalWeight = 0.1;
  } else if (command == "factory_reset") {
    factoryReset();
    commandMemory.emotionalWeight = 0.1;
  } else if (command == "help" || command == "commands") {
    printCommands();
    commandMemory.emotionalWeight = 0.1;
  } else {
    Serial.println("❓ Unknown command. Type 'help' for command list.");
    commandMemory.emotionalWeight = 0.1;
  }
  
  memories.push_back(commandMemory);
  saveMemoryToSD(commandMemory);
  
  logEvent("interaction", "command", "Executed: " + command);
}

void printCommands() {
  Serial.println("\n🎮 ===== NOVA DOG ESP32 S3 COMMANDS =====");
  Serial.println("   🐕 Basic Actions:");
  Serial.println("   • sit        - Sit down");
  Serial.println("   • walk       - Walk forward");
  Serial.println("   • play       - Play bow");
  Serial.println("   • happy      - Tail wag");
  Serial.println("   • curious    - Head tilt");
  Serial.println("   • reset      - Return to idle");
  Serial.println("   ");
  Serial.println("   ⚖️  Balance & Status:");
  Serial.println("   • status     - Show full status");
  Serial.println("   • balance    - Show balance info");
  Serial.println("   • calibrate  - Recalibrate IMU");
  Serial.println("   • recovery   - Attempt fall recovery");
  Serial.println("   • balance_on/off - Enable/disable balance");
  Serial.println("   ");
  Serial.println("   🧠 Learning & Memory:");
  Serial.println("   • memories   - Show recent memories");
  Serial.println("   • save       - Save data manually");
  Serial.println("   ");
  Serial.println("   🔧 System:");
  Serial.println("   • pins       - Show ESP32 S3 pin assignments");
  Serial.println("   • factory_reset - Reset all data");
  Serial.println("   • help       - Show this menu");
  Serial.println("========================================\n");
}

void printPinAssignments() {
  Serial.println("\n📡 ===== ESP32 S3 PIN ASSIGNMENTS =====");
  Serial.println("   8 Servos:");
  for (int i = 0; i < NUM_SERVOS; i++) {
    Serial.printf("   • %s → GPIO%d\n", servoStates[i].name.c_str(), servoPins[i]);
   // Serial.printf("   • Tail            → GPIO%d\n", TAIL_SERVO_PIN);

  }Serial.printf("   • Tail            → GPIO%d\n", TAIL_SERVO);
  Serial.println("   ");
  Serial.printf("   SD Card SPI:\n");
  Serial.printf("   • CS   → GPIO%d\n", SD_CS_PIN);
  Serial.printf("   • SCK  → GPIO%d\n", SD_SCK_PIN);
  Serial.printf("   • MOSI → GPIO%d\n", SD_MOSI_PIN);
  Serial.printf("   • MISO → GPIO%d\n", SD_MISO_PIN);
  Serial.println("   ");
  Serial.printf("   I2C Bus:\n");
  Serial.printf("   • SDA  → GPIO%d (MPU6050 & OLED)\n", I2C_SDA_PIN);
  Serial.printf("   • SCL  → GPIO%d (MPU6050 & OLED)\n", I2C_SCL_PIN);
  Serial.println("   ");
  Serial.printf("   Sensors:\n");
  Serial.printf("   • Ultrasonic Trig → GPIO%d\n", ULTRASONIC_TRIG_PIN);
  Serial.printf("   • Ultrasonic Echo → GPIO%d\n", ULTRASONIC_ECHO_PIN);
  Serial.printf("   • PIR Motion      → GPIO%d\n", PIR_SENSOR_PIN);
  Serial.printf("   • Light Sensor    → GPIO%d\n", LIGHT_SENSOR_PIN);
  Serial.printf("   • Buzzer          → GPIO%d\n", BUZZER_PIN);
  Serial.printf("   • Status LED      → GPIO%d\n", LED_PIN);
  Serial.println("=====================================\n");
}

void calibrateMPU6050() {
  if (!balanceEnabled) {
    Serial.println("❌ Balance system not available");
    return;
  }
  
  Serial.println("🔄 Recalibrating MPU6050...");
  Serial.println("   Keep robot completely still!");
  
  for (int i = 0; i < 10; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    digitalWrite(LED_PIN, LOW);
    delay(100);
  }
  
  mpu.calcOffsets();
  Serial.println("✅ Calibration complete");
  logEvent("system", "calibration", "MPU6050 recalibrated");
}

void factoryReset() {
  Serial.println("🔄 Factory reset - clearing all data");
  
  dogPersonality = {0.5, 0.8, 0.7, 0.6, 0.5, 0.8, 0, 0, false};
  memories.clear();
  
  if (sdCardWorking) {
    savePersonalityToSD();
  } else {
    savePersonalityToEEPROM();
  }
  
  moveToIdlePosition();
  
  Serial.println("✅ Factory reset complete!");
  logEvent("system", "factory_reset", "All data reset to defaults");
}

void printStatus() {
  Serial.println("\n🐕======= NOVA DOG ESP32 S3 STATUS =======");
  Serial.printf("⚡ Hardware: ESP32 S3 with %d servos\n", NUM_SERVOS);
  Serial.printf("💾 Storage: %s\n", sdCardWorking ? "SD Card + EEPROM" : "EEPROM only");
  Serial.printf("🎮 Interactions: %lu\n", dogPersonality.interactions);
  Serial.printf("⏱️ Uptime: %lu minutes\n", dogPersonality.uptime);
  Serial.printf("🧠 Memories: %d stored\n", memories.size());
  Serial.println("   ");
  Serial.println("🧠 Personality:");
  Serial.printf("   😊 Happiness: %.2f\n", dogPersonality.happiness);
  Serial.printf("   🔋 Energy: %.2f\n", dogPersonality.energy);
  Serial.printf("   🤔 Curiosity: %.2f\n", dogPersonality.curiosity);
  Serial.printf("   🤝 Friendliness: %.2f\n", dogPersonality.friendliness);
  Serial.printf("   💪 Confidence: %.2f\n", dogPersonality.confidence);
  Serial.printf("   🎾 Playfulness: %.2f\n", dogPersonality.playfulness);
  Serial.println("   ");
  Serial.println("📊 Sensors:");
  Serial.printf("   📏 Distance: %.1f cm\n", currentSensors.distance);
  Serial.printf("   👋 Motion: %s\n", currentSensors.motionDetected ? "Detected" : "None");
  Serial.printf("   💡 Light: %.2f\n", currentSensors.lightLevel);
  Serial.printf("   🌡️ Temperature: %.1f°C\n", currentSensors.temperature);
  Serial.println("   ");
  printBalanceStatus();
  Serial.println("=======================================\n");
}

void printBalanceStatus() {
  Serial.println("⚖️  Balance Status:");
  if (!balanceEnabled) {
    Serial.println("   ❌ Balance system DISABLED");
    return;
  }
  
  Serial.printf("   📐 Pitch: %.1f° (%.1f°/s)\n", balance.pitch, balance.pitchRate);
  Serial.printf("   📐 Roll: %.1f° (%.1f°/s)\n", balance.roll, balance.rollRate);
  Serial.printf("   📐 Yaw: %.1f°\n", balance.yaw);
  Serial.printf("   ⚖️  Balanced: %s\n", balance.isBalanced ? "✅ YES" : "⚠️ NO");
  Serial.printf("   🚨 Fall Detected: %s\n", balance.fallDetected ? "⚠️ YES" : "✅ NO");
  Serial.printf("   🎯 State: %s\n", getStateString().c_str());
}

void printMemories() {
  Serial.println("\n🧠 ===== RECENT MEMORIES =====");
  
  if (memories.empty()) {
    Serial.println("   No memories stored yet");
    Serial.println("=============================\n");
    return;
  }
  
  int startIdx = max(0, (int)memories.size() - 10);
  for (int i = startIdx; i < memories.size(); i++) {
    Serial.printf("%d. [%s] %s (%.1f)\n", 
                  i+1, 
                  memories[i].type.c_str(),
                  memories[i].data.c_str(),
                  memories[i].emotionalWeight);
  }
  Serial.println("=============================\n");
}

// ============================================================================
// DISPLAY SYSTEM
// ============================================================================

void updateDisplay() {
  static unsigned long lastDisplayUpdate = 0;
  if (millis() - lastDisplayUpdate < 1000) return;
  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  
  display.println("Nova ESP32 S3");
  display.printf("State: %s\n", getStateString().c_str());
  display.printf("Happy:%.1f Conf:%.1f\n", dogPersonality.happiness, dogPersonality.confidence);
  display.printf("Pitch:%.0f Roll:%.0f\n", balance.pitch, balance.roll);
  display.printf("Balance: %s\n", balance.isBalanced ? "OK" : "BAD");
  display.printf("Mem:%d Cmd:%lu\n", memories.size(), dogPersonality.interactions);
  display.printf("SD:%s I2C:%d,%d", sdCardWorking ? "OK" : "NO", I2C_SDA_PIN, I2C_SCL_PIN);
  
  display.display();
  lastDisplayUpdate = millis();
}

String getStateString() {
  switch (currentState) {
    case IDLE: return "Idle";
    case WALKING: return "Walking";
    case SITTING: return "Sitting";
    case PLAYING: return "Playing";
    case EXPRESSING_EMOTION: return "Emoting";
    case BALANCING: return "Balancing";
    case FALLEN: return "Fallen";
    case RECOVERING: return "Recovery";
    default: return "Unknown";
  }
}

void displayStartupMessage() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("NOVA DOG");
  display.println("ESP32 S3 Edition");
  display.println("");
  display.printf("Servos: %d\n", NUM_SERVOS);
  display.printf("Balance: %s\n", balanceEnabled ? "ON" : "OFF");
  display.printf("Storage: %s\n", sdCardWorking ? "SD" : "EEPROM");
  display.println("Learning: Active");
  display.println("");
  display.println("Ready to learn!");
  display.display();
  delay(3000);
}