Week 12/13

#include <Wire.h> 
#include <Adafruit_MPU6050.h> 
#include <Adafruit_Sensor.h> 
#include <SPI.h> 
#include <Adafruit_GFX.h> 
#include <Adafruit_SSD1306.h> 
#define SCREEN_WIDTH 128 // OLED display width, in pixels 
#define SCREEN_HEIGHT 64 // OLED display height, in pixels 
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins) 
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin) 
#define SCREEN_ADDRESS 0x3D 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); 
// specific pins 
const int sdaPin = 6; 
const int sclPin = 7; 
const int ledPin = 0; 
const int motorAIN2 = 26; 
const int motorAIN1 = 27; 
const int motorBIN2 = 28; 
const int motorBIN1 = 29; 
Adafruit_MPU6050 mpu; void setup() 
{ 
	Serial.begin(9600); 
	while (!Serial); // Wait for Serial Monitor 
	pinMode(motorAIN1, OUTPUT); 
	pinMode(motorAIN2, OUTPUT); 
	pinMode(motorBIN1, OUTPUT); 
	pinMode(motorBIN2, OUTPUT); 
	pinMode(ledPin, OUTPUT); 
	digitalWrite(ledPin, HIGH); 
	Serial.println("--- MPU6050 Hardware Check ---"); 
	// 1. Initialize I2C 
	Wire.setSDA(sdaPin); 
	Wire.setSCL(sclPin); 
	Wire.begin(); 
	display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 
	display.clearDisplay(); display.display(); 
	// 2. Run a Quick Scan to see if the chip is even on the bus 
	Serial.println("Scanning I2C bus..."); 
	byte count = 0; 
	for (byte i = 1; i < 127; i++) {
		Wire.beginTransmission(i); 
		if (Wire.endTransmission() == 0) { 
			Serial.print("Found device at 0x"); 
			Serial.println(i, HEX); count++; 
		} 
	} 
	if (count == 0) { 
		Serial.println("ERROR: No I2C devices found at all. Check wiring/power."); 
	} 
	// 3. Try to start the MPU6050 (checking both common addresses) 
	Serial.println("Attempting to connect to MPU6050..."); 
	if (!mpu.begin(0x68)) { 
		// Default address 
		if (!mpu.begin(0x69)) { 
			// Alternate address 
			Serial.println("FAILED: MPU6050 not responding at 0x68 or 0x69."); 
			Serial.println("Double check: 3V3, GND, SDA(GPIO6), SCL(GPIO7)"); 
			return; 
		} 
	} 
	Serial.println("SUCCESS: MPU6050 Connected!"); 
	// Configure sensor settings 
	mpu.setAccelerometerRange(MPU6050_RANGE_8_G); 
	mpu.setGyroRange(MPU6050_RANGE_500_DEG); 
	mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); 
} 
void loop() { 
	sensors_event_t a, g, temp; 
	mpu.getEvent(&a, &g, &temp); 
	// --- Serial Output --- 
	Serial.print("Accel X: "); 
	Serial.print(a.acceleration.x); 
	Serial.print(", Y: "); 
	Serial.print(a.acceleration.y); 
	Serial.print(", Z: "); 
	Serial.print(a.acceleration.z); 
	Serial.println(" m/s^2"); 
	// --- OLED Display Output --- 
	display.clearDisplay(); 
	// Header Style 
	display.setTextSize(1); 
	display.setTextColor(SSD1306_WHITE); 
	display.setCursor(0, 0); 
	display.println("MPU6050 ACCEL (m/s2)"); 
	display.drawFastHLine(0, 10, 128, SSD1306_WHITE); 
	// Decorative line // Data Style 
	display.setTextSize(2); 
	display.setCursor(0, 18); 
	display.print("X: "); 
	display.println(a.acceleration.x, 3); 
	// 1 decimal place 
	display.setCursor(0, 34); 
	display.print("Y: "); 
	display.println(a.acceleration.y, 3); 
	display.setCursor(0, 50); 
	display.print("Z: "); 
	display.println(a.acceleration.z, 3); 
	display.display(); 
	// Run Motors digitalWrite(motorAIN1, HIGH); 
	digitalWrite(motorAIN2, LOW); 
	digitalWrite(motorBIN1, HIGH); 
	digitalWrite(motorBIN2, LOW); 
	delay(200); 
	// Slightly faster refresh for smoother reading 
	Serial.println("Reading MPU6050..."); 
	mpu.getEvent(&a, &g, &temp); 
	Serial.println("Driving Motors..."); // If you don't see this, the code is stuck above 
}

#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);

// --- Motor Pins (L298N) ---
const int motorAIN1 = 27; const int motorAIN2 = 26; // Left Motor Dir
const int motorBIN1 = 29; const int motorBIN2 = 28; // Right Motor Dir

// --- MPU6050 ---
const int MPU_ADDR = 0x69;
int16_t AcX, AcZ, GyY;

// --- Sensor Fusion ---
float pitch = 0;       // Complementary filter output
float gyroAngle = 0;   // Gyroscope integrated angle

// --- PID Constants ---
float Kp = -30.0;
float Ki = 0.0;
float Kd = -1.0;
float targetAngle = 0.0;

// --- PID Variables ---
float error, lastError, cumError, rateError;
unsigned long lastTime;

// ─────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  delay(2000);

  // --- Motor Pin Setup ---
  pinMode(motorAIN1, OUTPUT); pinMode(motorAIN2, OUTPUT);
  pinMode(motorBIN1, OUTPUT); pinMode(motorBIN2, OUTPUT);

  // --- Init OLED first (so we can show errors) ---
  Wire.setSDA(6); Wire.setSCL(7);
  Wire.begin();
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) for (;;);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.display();

  // --- I2C Bus Recovery ---
  // Release I2C pins back to GPIO to manually recover the bus
  Wire.end();
  pinMode(6, OUTPUT); pinMode(7, OUTPUT);
  digitalWrite(6, HIGH); digitalWrite(7, HIGH);
  delay(10);

  // Clock out any stuck slave (up to 9 pulses until SDA goes HIGH)
  pinMode(7, OUTPUT);
  for (int i = 0; i < 9; i++) {
    if (digitalRead(6) == HIGH) break;
    digitalWrite(7, LOW);  delayMicroseconds(5);
    digitalWrite(7, HIGH); delayMicroseconds(5);
  }

  // Send a manual STOP condition
  pinMode(6, OUTPUT);
  digitalWrite(7, LOW); delayMicroseconds(5);
  digitalWrite(6, LOW); delayMicroseconds(5);
  digitalWrite(7, HIGH); delayMicroseconds(5);
  digitalWrite(6, HIGH); delayMicroseconds(5);
  delay(10);

  // Re-init I2C properly
  Wire.setSDA(6); Wire.setSCL(7);
  Wire.begin();
  delay(100); // Give MPU6050 time to stabilize

  // --- Wake MPU6050 with retry ---
  bool mpuOK = false;
  for (int attempt = 0; attempt < 5; attempt++) {
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x6B);
    Wire.write(0x00); // Clear sleep bit
    if (Wire.endTransmission(true) == 0) {
      mpuOK = true;
      break;
    }
    delay(50);
  }

  if (!mpuOK) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("MPU6050 FAIL!");
    display.setCursor(0, 12);
    display.print("Check wiring");
    display.setCursor(0, 24);
    display.print("or reset again");
    display.display();
    while (true); // Halt
  }

  // Show ready message
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(20, 25);
  display.print("MPU6050 Ready!");
  display.display();
  delay(1000);

  lastTime = millis();
}

// ─────────────────────────────────────────
void loop() {
  // 1. Read MPU6050 Data
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);

  AcX = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();               // Skip AcY
  AcZ = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();               // Skip Temp
  Wire.read(); Wire.read();               // Skip GyX
  GyY = Wire.read() << 8 | Wire.read();  // Gyro Y (pitch rate)
  Wire.read(); Wire.read();               // Skip GyZ

  // 2. Complementary Filter for Pitch
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // Accelerometer angle
  float accelAngle = atan2(AcX, AcZ) * 180.0 / M_PI;
  if (accelAngle < 180 && accelAngle > 0) accelAngle = 180 - accelAngle;
  else accelAngle = -1 * accelAngle - 180;

  // Gyroscope rate -> integrate into angle
  float gyroRate = GyY / 131.0; // 131 LSB per °/s at ±250°/s default
  gyroAngle = pitch + gyroRate * dt;

  // Complementary filter: gyro short-term + accel long-term drift correction
  pitch = 0.98 * gyroAngle + 0.02 * accelAngle;

  // 3. PID Calculation
  error = targetAngle - pitch;
  cumError += error * dt;
  rateError = (error - lastError) / dt;

  float motorPower = (Kp * error) + (Ki * cumError) + (Kd * rateError);

  lastError = error;

  // 4. Motor Execution & Safety
  if (abs(pitch) > 40) {  // Robot fell over — stop motors
    controlMotors(0);
    cumError = 0;          // Reset integral to prevent windup
  } else {
    controlMotors(motorPower);
  }

  // 5. OLED update (throttled — every 10 loops)
  static int oledTimer = 0;
  if (oledTimer++ > 10) {
    drawFace(pitch);
    oledTimer = 0;
  }
}

// ─────────────────────────────────────────
void controlMotors(float power) {
  int pwmValue = constrain(abs(power), 0, 255);

  if (power > 0) {  // Tilt Forward -> Move Forward
    analogWrite(motorAIN1, pwmValue); digitalWrite(motorAIN2, LOW);
    digitalWrite(motorBIN1, LOW);     analogWrite(motorBIN2, pwmValue);
  } else {          // Tilt Backward -> Move Backward
    digitalWrite(motorAIN1, LOW); analogWrite(motorAIN2, pwmValue);
    analogWrite(motorBIN1, pwmValue); digitalWrite(motorBIN2, LOW);
  }
}

void drawFace(float currentPitch) {
  static unsigned long lastBlink = 0;
  static bool eyesOpen = true;

  // Blink logic: open 3000ms, closed 200ms
  unsigned long now = millis();
  if (eyesOpen && now - lastBlink > 2000) {
    eyesOpen = false;
    lastBlink = now;
  } else if (!eyesOpen && now - lastBlink > 200) {
    eyesOpen = true;
    lastBlink = now;
  }

  display.clearDisplay();

  // Eyes
  if (eyesOpen) {
    display.fillCircle(40, 25, 6, SSD1306_WHITE); // Left open
    display.fillCircle(88, 25, 6, SSD1306_WHITE); // Right open
  } else {
    display.drawLine(34, 25, 46, 25, SSD1306_WHITE); // Left closed
    display.drawLine(82, 25, 94, 25, SSD1306_WHITE); // Right closed
  }

  // Mouth (center Y shifts with pitch — frown vs smile)
  int mouthCenterY = 48 + (int)constrain(currentPitch, -12, 12);
  display.drawLine(40, 48, 64, mouthCenterY, SSD1306_WHITE);
  display.drawLine(64, mouthCenterY, 88, 48, SSD1306_WHITE);

  // Angle readout
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Ang: "); display.print(currentPitch, 1);

  display.display();
}

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>
#include <Arduino.h>
#include "esp32-hal-ledc.h"

// OLED settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDR 0x3C

//MOTORS
// Motor A
#define INA1 2
#define INA2 3

// Motor B
#define INB1 4
#define INB2 5

//debug settings
//#define display_on
#define Serial_on
#define MOTORS
#define PID

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
MPU6050 mpu(0x69);

// I2C pins
#define SDA_PIN 6
#define SCL_PIN 7


#define PWM_FREQ 1000
#define PWM_RESOLUTION 8

#define CH_A1 0
#define CH_A2 1
#define CH_B1 2
#define CH_B2 3

// Complementary filter variables
float angleX = 0;
float angleY = 0;
float pitch = 0;

//Calibration variables 
float gyroX_offset = 0;
float gyroY_offset = 0;
float gyroZ_offset = 0;

float accX_offset = 0;
float accY_offset = 0;
float accZ_offset = 0;

//PID VARS               //////////////////////////////////////
float Kp = 20;
float Ki = 0.5;
float Kd = -2;

float setpoint = -3; // upright

float error = 0;
float previous_error = 0;
float integral = 0;
float derivative = 0;

float output = 0;
//////////////////////////////////////////////

int16_t ax, ay, az;
int16_t gx, gy, gz;

unsigned long lastTime = 0;

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

  // Setup PWM channels
  ledcAttach(INA1, PWM_FREQ, PWM_RESOLUTION);
  ledcAttach(INA2, PWM_FREQ, PWM_RESOLUTION);
  ledcAttach(INB1, PWM_FREQ, PWM_RESOLUTION);
  ledcAttach(INB2, PWM_FREQ, PWM_RESOLUTION);



  Wire.setClock(400000);
  Wire.begin(SDA_PIN, SCL_PIN);

  // OLED init
  // if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
  //   Serial.println("OLED not found");
  //   while (1);
  // }

  // display.clearDisplay();
  // display.setTextSize(1);
  // display.setTextColor(WHITE);

  // MPU init
  Serial.println("Initializing MPU6050...");
  mpu.initialize();

  // if (!mpu.testConnection()) {
  //   Serial.println("MPU6050 connection failed");

  //   display.setCursor(0, 0);
  //   display.println("MPU FAIL");
  //   display.display();
  //   while (1);
  // }

  // display.setCursor(0, 0);
  // display.println("MPU OK");
  // display.display();

  // //Calibaration phase
  // // --- Gyro calibration ---
  // display.clearDisplay();
  // display.setCursor(0, 0);
  // display.println("Calibrating...");
  // display.display();

  delay(1000); // let sensor stabilize

  long gx_sum = 0;
  long gy_sum = 0;
  long gz_sum = 0;

  int samples = 1000;

  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    gx_sum += gx;
    gy_sum += gy;
    gz_sum += gz;

    delay(2); // small delay
  }

  // Calculate average offset
  gyroX_offset = gx_sum / (float)samples;
  gyroY_offset = gy_sum / (float)samples;
  gyroZ_offset = gz_sum / (float)samples;

  // Debug
  Serial.println("Gyro offsets:");
  Serial.print(gyroX_offset); Serial.print(" ");
  Serial.print(gyroY_offset); Serial.print(" ");
  Serial.println(gyroZ_offset);

  long ax_sum = 0;
  long ay_sum = 0;
  long az_sum = 0;

  //int samples = 1000;

  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    ax_sum += ax;
    ay_sum += ay;
    az_sum += az;

    delay(2);
  }

  // Calculate offsets
  accX_offset = ax_sum / (float)samples;
  accY_offset = ay_sum / (float)samples;

  // IMPORTANT: Z should be 1g (~16384)
  accZ_offset = (az_sum / (float)samples) - 16384.0;

  Serial.println("Accel offsets:");
  Serial.print(accX_offset); Serial.print(" ");
  Serial.print(accY_offset); Serial.print(" ");
  Serial.println(accZ_offset);
  // display.clearDisplay();
  // display.setCursor(0, 0);
  // display.println("Calibration OK");
  // display.display();

  delay(1000);

  delay(2000);

  lastTime = micros();
}

void loop() {
  

  // Read raw data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Time delta
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0;
  lastTime = now;
  if (dt <= 0 || dt > 0.1) return; 

  // --- Accelerometer angles (degrees) ---
  //float accAngleX = atan2(ay, az) * 180 / PI;
  float denomX = sqrt(ay * ay + az * az);
  if (denomX < 0.001) denomX = 0.001;

  float accAngleY = atan2(-ax, denomX) * 180 / PI;
  //float accAngleY = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;

  // --- Gyro (convert to deg/sec) ---
  float gyroX = (gx - gyroX_offset) / 131.0;
  float gyroY = (gy - gyroY_offset) / 131.0;

  ax = ax - accX_offset;
  ay = ay - accY_offset;
  az = az - accZ_offset;

  // --- Complementary filter ---
  float alpha = 0.995;
  // if(abs(pitch) < 1)
  //   accAngleY = 0;

  //angleX = alpha * (angleX + gyroX * dt) + (1 - alpha) * accAngleX;
  angleY = alpha * (angleY + (gyroY) * dt) + (1 - alpha) * accAngleY;
  
  // if(angleY < 0)
  //   pitch = 180 + angleY;
  // else
  //   pitch = angleY - 180; 
  // --- Display ---
  if (isnan(angleX) || isnan(angleY)) {
    Serial.println("NaN detected! Resetting...");
    angleX = 0;
    angleY = 0;
    return;
  }
  #ifdef PID
  /// PID LOOP
  error = setpoint - angleY;

  // Integral (with simple limiting)
  integral += error * dt;
  integral = constrain(integral, -100, 100);

  // Derivative
  derivative = -gyroY;//(error - previous_error) / dt;

  // PID output
  output = Kp * error + Ki * integral + Kd * derivative;

  previous_error = error;
  if(abs(angleY) > 45)
    output = 0;
  
  
  if (abs(output) < 100 && output != 0) {
    output = (output > 0) ? 100 : -100;
  }
  #endif
  // output += 10;
  // delay(1000);
  #ifdef MOTORS

  int motorSpeed = constrain(abs(output), 0, 250);

  if (output > 0 ) {
    // forward
    ledcWrite(INA1, motorSpeed);
    ledcWrite(INA2, 0);
    ledcWrite(INB1, motorSpeed);
    ledcWrite(INB2, 0);
  } else {
    // backward
    ledcWrite(INA1, 0);
    ledcWrite(INA2, motorSpeed);
    ledcWrite(INB1, 0);
    ledcWrite(INB2, motorSpeed);
  }
  #endif


  #ifdef display_on
  display.clearDisplay();

  display.setCursor(0, 0);
  display.println("MPU6050 Angle");

  // display.setCursor(0, 15);
  // display.print("Angle X: ");
  // display.println(angleX, 2);

  display.print("Angle Y: ");
  display.println(pitch, 2);

  display.display();
  #endif
  #ifdef Serial_on
  Serial.print("output: ");
  Serial.print(output, 2);
  Serial.print(" Angle  Y: ");
  Serial.print(angleY);
  Serial.print(" Kp: ");
  Serial.print(Kp * error);
  Serial.print(" Kd ");
  Serial.print(Kd * derivative);
  Serial.print(" Ki ");
  Serial.println(Ki * integral);
  #endif
  //delay(1);
}

#include "esp_camera.h"
#include "Arduino.h"
#include <WiFi.h>
#include <WebServer.h>

// ─── WiFi credentials ───────────────────────────────────────────
#define WIFI_SSID   "iPhone"
#define WIFI_PASS   "Manuela26"

// ─── XIAO ESP32-S3 Sense pinout ─────────────────────────────────
#define CAM_PIN_PWDN  -1
#define CAM_PIN_RESET -1
#define CAM_PIN_XCLK  10
#define CAM_PIN_SIOD  40
#define CAM_PIN_SIOC  39
#define CAM_PIN_D7    48
#define CAM_PIN_D6    11
#define CAM_PIN_D5    12
#define CAM_PIN_D4    14
#define CAM_PIN_D3    16
#define CAM_PIN_D2    18
#define CAM_PIN_D1    17
#define CAM_PIN_D0    15
#define CAM_PIN_VSYNC 38
#define CAM_PIN_HREF  47
#define CAM_PIN_PCLK  13

#define SIGNAL_PIN     43
#define SMILE_HOLD_MS  800   // how long GPIO stays HIGH after smile
#define COOLDOWN_MS   2500   // min time between two smile triggers
#define SMILE_FRAMES     4   // consecutive frames needed to confirm

// ── Relative detection thresholds ───────────────────────────────
// Mouth must be this much brighter than forehead to count as smile
#define RELATIVE_THRESH  22 /// 22
// Local contrast in mouth zone (teeth vs lips boundary)
#define CONTRAST_THRESH  100  ////50
 
static camera_config_t camera_config = {
  .pin_pwdn      = CAM_PIN_PWDN,  .pin_reset   = CAM_PIN_RESET,
  .pin_xclk      = CAM_PIN_XCLK,
  .pin_sscb_sda  = CAM_PIN_SIOD,  .pin_sscb_scl = CAM_PIN_SIOC,
  .pin_d7=CAM_PIN_D7, .pin_d6=CAM_PIN_D6,
  .pin_d5=CAM_PIN_D5, .pin_d4=CAM_PIN_D4,
  .pin_d3=CAM_PIN_D3, .pin_d2=CAM_PIN_D2,
  .pin_d1=CAM_PIN_D1, .pin_d0=CAM_PIN_D0,
  .pin_vsync=CAM_PIN_VSYNC, .pin_href=CAM_PIN_HREF, .pin_pclk=CAM_PIN_PCLK,
  .xclk_freq_hz  = 10000000,
  .ledc_timer    = LEDC_TIMER_0,
  .ledc_channel  = LEDC_CHANNEL_0,
  .pixel_format  = PIXFORMAT_GRAYSCALE,
  .frame_size    = FRAMESIZE_QVGA,
  .jpeg_quality  = 12,
  .fb_count      = 2,
  .fb_location   = CAMERA_FB_IN_PSRAM,
  .grab_mode     = CAMERA_GRAB_LATEST,
};

WebServer server(80);

int      currentMean        = 0;
int      currentContrast    = 0;
int      currentRelative    = 0;
uint32_t smileHoldUntil     = 0;
uint32_t smileCooldownUntil = 0;
int      consecutiveSmiles  = 0;
uint32_t totalSmiles        = 0;
uint32_t frameCount         = 0;
bool     lastSmileState     = false;
bool     newSmileFlag       = false;

// ────────────────────────────────────────────────────────────────
// RELATIVE smile detection
// Compares mouth zone brightness against forehead zone.
// If the room gets brighter, BOTH zones rise equally → relative
// stays the same → no false trigger.
// Only teeth showing (mouth brighter than forehead) fires a smile.
// ────────────────────────────────────────────────────────────────
bool detectSmile(uint8_t* buf, int w, int h) {

  int colS = w / 4;
  int colE = (w * 3) / 4;

  int foreS = h / 10;
  int foreE = h / 4;

  int mouthS = (h * 62) / 100;
  int mouthE = (h * 85) / 100;

  long sumF=0, cntF=0;
  long sumM=0, cntM=0;
  long globalSum=0;

  int maxM=0, minM=255;

  // --- Global brightness ---
  for (int i=0; i < w*h; i++) globalSum += buf[i];
  int globalMean = globalSum / (w*h);

  // --- Forehead ---
  for (int y=foreS; y < foreE; y++)
    for (int x=colS; x < colE; x++) {
      uint8_t p = buf[y*w+x];
      sumF += p; cntF++;
    }

  // --- Mouth ---
  for (int y=mouthS; y < mouthE; y++)
    for (int x=colS; x < colE; x++) {
      uint8_t p = buf[y*w+x];
      sumM += p; cntM++;

      if (p > maxM) maxM = p;
      if (p < minM) minM = p;
    }

  if (!cntF || !cntM) return false;

  int meanFore  = sumF / cntF;
  int meanMouth = sumM / cntM;

  // --- Normalize against global brightness ---
  int relative  = (meanMouth - meanFore) - (globalMean / 12);

  int contrast  = maxM - minM;

  // --- Face presence check ---
  if (meanFore < 40 || meanFore > 200) return false;

  // --- Dynamic thresholds ---
  int dynamicRelThresh = max(15, meanMouth / 10);
  int dynamicContThresh = max(60, contrast / 2);

  // --- Smooth values (low-pass filter) ---
  static float smoothRel = 0;
  static float smoothCont = 0;

  smoothRel  = 0.7 * smoothRel  + 0.3 * relative;
  smoothCont = 0.7 * smoothCont + 0.3 * contrast;

  currentMean     = meanMouth;
  currentContrast = (int)smoothCont;
  currentRelative = (int)smoothRel;

  bool isSmile = (smoothRel > dynamicRelThresh && smoothCont > dynamicContThresh);

  Serial.printf("Fore:%3d Mouth:%3d Rel:%+3d Cont:%3d ThrR:%d %s\n",
    meanFore, meanMouth,
    (int)smoothRel, (int)smoothCont,
    dynamicRelThresh,
    isSmile ? "<<< SMILE" : "");

  return isSmile;
}

// ─── Web page ───────────────────────────────────────────────────
const char INDEX_HTML[] PROGMEM = R"rawhtml(
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>Smile Detector</title>
<link href="https://fonts.googleapis.com/css2?family=DM+Mono:wght@400;500&family=Syne:wght@400;700;800&display=swap" rel="stylesheet">
>style>>
*,*::before,*::after{box-sizing:border-box;margin:0;padding:0}
:root{
  --bg:#0a0a0f;--surface:#12121a;--border:#1e1e2e;--muted:#3a3a50;
  --text:#e8e8f0;--subtle:#7070a0;--smile:#c8f060;--neutral:#4060f0;--accent:#f060c0;
}
body{background:var(--bg);color:var(--text);font-family:'Syne',sans-serif;min-height:100vh;display:grid;grid-template-rows:auto 1fr auto;}
body::before{content:'';position:fixed;inset:0;background-image:linear-gradient(var(--border) 1px,transparent 1px),linear-gradient(90deg,var(--border) 1px,transparent 1px);background-size:40px 40px;opacity:0.4;pointer-events:none;z-index:0;}
header{position:relative;z-index:1;padding:1.4rem 2rem;border-bottom:1px solid var(--border);display:flex;align-items:center;justify-content:space-between;}
.grandma-panel { display: flex; justify-content: space-between; position: fixed; top: 150px;width: 100%; }
.grandma-panel img { width: 20%; border-radius: 50%; }
.logo{font-size:1rem;font-weight:800;letter-spacing:0.15em;text-transform:uppercase;color:var(--smile);}
.logo span{color:var(--subtle);font-weight:400;}
.pill{font-family:'DM Mono',monospace;font-size:0.65rem;padding:0.25rem 0.7rem;border-radius:99px;border:1px solid var(--muted);color:var(--subtle);letter-spacing:0.1em;transition:all .3s;}
.pill.online{border-color:var(--smile);color:var(--smile);}
main{position:relative;z-index:1;display:grid;grid-template-columns:1fr 290px;gap:1.2rem;padding:1.2rem 2rem;max-width:1050px;width:100%;margin:0 auto;}
.left{display:flex;flex-direction:column;gap:1.2rem;}
.face{background:var(--surface);border:1px solid var(--border);border-radius:1.4rem;padding:2rem;display:flex;flex-direction:column;align-items:center;gap:1rem;transition:border-color .3s,box-shadow .3s;position:relative;overflow:hidden;}
.face.smiling{border-color:var(--smile);box-shadow:0 0 50px -10px rgba(200,240,96,.3);}
.face::after{content:'';position:absolute;inset:0;background:radial-gradient(ellipse at 50% 0%,rgba(200,240,96,.07) 0%,transparent 70%);opacity:0;transition:opacity .4s;pointer-events:none;}
.face.smiling::after{opacity:1;}
.emoji{font-size:5.5rem;line-height:1;user-select:none;transition:transform .3s cubic-bezier(.34,1.56,.64,1),filter .3s;}
.face.smiling .emoji{transform:scale(1.15);filter:drop-shadow(0 0 18px rgba(200,240,96,.5));animation:pop .45s cubic-bezier(.34,1.56,.64,1);}
@keyframes pop{0%{transform:scale(1)}50%{transform:scale(1.22)}100%{transform:scale(1.15)}}
.slabel{font-size:1.3rem;font-weight:800;letter-spacing:.06em;text-transform:uppercase;color:var(--subtle);transition:color .3s;}
.face.smiling .slabel{color:var(--smile);}
.sigbadge{display:flex;align-items:center;gap:.5rem;font-family:'DM Mono',monospace;font-size:.68rem;color:var(--subtle);padding:.35rem .9rem;border:1px solid var(--muted);border-radius:99px;transition:all .3s;}
.sigbadge.active{color:var(--smile);border-color:var(--smile);background:rgba(200,240,96,.06);}
.dot{width:6px;height:6px;border-radius:50%;background:var(--muted);transition:background .3s;}
.sigbadge.active .dot{background:var(--smile);animation:blink 1s ease-in-out infinite;}
@keyframes blink{0%,100%{opacity:1;transform:scale(1)}50%{opacity:.3;transform:scale(.5)}}
.chart-card{background:var(--surface);border:1px solid var(--border);border-radius:1.3rem;padding:1.2rem 1.3rem;}
.ctitle{font-family:'DM Mono',monospace;font-size:.65rem;letter-spacing:.15em;text-transform:uppercase;color:var(--subtle);margin-bottom:.7rem;}
canvas{width:100%!important;height:90px!important;display:block;}
.tl{font-family:'DM Mono',monospace;font-size:.6rem;color:var(--accent);margin-top:.35rem;opacity:.7;}
.right{display:flex;flex-direction:column;gap:.9rem;}
.sc{background:var(--surface);border:1px solid var(--border);border-radius:1rem;padding:1rem 1.2rem;}
.slb{font-family:'DM Mono',monospace;font-size:.6rem;letter-spacing:.15em;text-transform:uppercase;color:var(--subtle);margin-bottom:.25rem;}
.sv{font-size:1.8rem;font-weight:800;line-height:1;}
.su{font-family:'DM Mono',monospace;font-size:.65rem;color:var(--subtle);margin-top:.15rem;}
.meter{height:3px;background:var(--muted);border-radius:2px;overflow:hidden;margin-top:.5rem;}
.mf{height:100%;border-radius:2px;background:var(--neutral);transition:width .3s,background .3s;}
.mf.hi{background:var(--smile);}
/* Relative meter: negative=blue, positive=green */
.rel-bar-wrap{height:6px;background:var(--muted);border-radius:3px;overflow:hidden;margin-top:.5rem;position:relative;}
.rel-bar{position:absolute;height:100%;border-radius:3px;transition:all .2s;}
.log-card{background:var(--surface);border:1px solid var(--border);border-radius:1rem;padding:1rem 1.2rem;flex:1;display:flex;flex-direction:column;}
.le{font-family:'DM Mono',monospace;font-size:.65rem;line-height:1.9;color:var(--subtle);overflow-y:auto;max-height:300px;}
.lr{display:flex;gap:.6rem;}
.lt{color:var(--muted);flex-shrink:0;}
.es{color:var(--smile);}
footer{position:relative;z-index:1;padding:.8rem 2rem;border-top:1px solid var(--border);display:flex;justify-content:space-between;font-family:'DM Mono',monospace;font-size:.6rem;color:var(--muted);}
@media(max-width:700px){main{grid-template-columns:1fr;padding:1rem;}header,footer{padding:1rem;}}
</style>
</head>
<body>
<header>
  <div class="logo">Smile<span>Detector</span></div>
  <div class="pill" id="pill">CONNECTING...</div>
</header>
<div class="grandma-panel">
 <img src="https://fabacademy.org/2026/labs/dilijan/students/mariam-daghbashyan/images/week_12/Manuela_SmileDetection.jpg" style="transform: scaleX(-1);" class="img-fluid d-flex align-center">
  <img src="https://fabacademy.org/2026/labs/dilijan/students/mariam-daghbashyan/images/week_12/Manuela_SmileDetection.jpg" class="img-fluid d-flex align-center"> 
</div>
<main>
  <div class="left">
    <div class="face" id="face">
      <div class="emoji" id="emoji">😐</div>
      <div class="slabel" id="slabel">No smile</div>
      <div class="sigbadge" id="sig"><div class="dot"></div><span>GPIO 43 → ESP32-C3</span></div>
    </div>
    <div class="chart-card">
      <div class="ctitle">Relative brightness (mouth − forehead) — last 60 frames</div>
      <canvas id="chart"></canvas>
      <div class="tl">── smile threshold: +22</div>
    </div>
  </div>
  <div class="right">
    <div class="sc">
      <div class="slb">Relative brightness</div>
      <div class="sv" id="v-rel">—</div>
      <div class="su">mouth minus forehead</div>
      <div class="rel-bar-wrap"><div class="rel-bar" id="rel-bar"></div></div>
    </div>
    <div class="sc">
      <div class="slb">Contrast</div>
      <div class="sv" id="v-con">—</div>
      <div class="su">max − min in mouth zone</div>
      <div class="meter"><div class="mf" id="m-con"></div></div>
    </div>
    <div class="sc">
      <div class="slb">Total smiles</div>
      <div class="sv" id="v-total">0</div>
      <div class="su">confirmed this session</div>
    </div>
    <div class="sc">
      <div class="slb">Frames analysed</div>
      <div class="sv" id="v-frames">0</div>
      <div class="su">since boot</div>
    </div>
    <div class="log-card">
      <div class="ctitle">Event log</div>
      <div class="le" id="log"></div>
    </div>
  </div>
</main>
<footer>
  <span>XIAO ESP32-S3 Sense</span>
  <span id="fps">— fps</span>
  <span>Self-balancing robot</span>
</footer>
<script>
const REL_THRESH = 22;
let hist = new Array(60).fill(0);
let lastT = Date.now();
const canvas = document.getElementById('chart');
const ctx = canvas.getContext('2d');

function drawChart() {
  const dpr=devicePixelRatio||1, W=canvas.offsetWidth*dpr, H=90*dpr;
  canvas.width=W; canvas.height=H; ctx.clearRect(0,0,W,H);
  // zero line
  const zy=H/2;
  ctx.strokeStyle='rgba(112,112,160,0.35)';ctx.lineWidth=dpr;
  ctx.beginPath();ctx.moveTo(0,zy);ctx.lineTo(W,zy);ctx.stroke();
  // threshold line
  const range=80, ty=H/2-(REL_THRESH/range)*(H/2);
  ctx.strokeStyle='rgba(240,96,192,0.5)';
  ctx.setLineDash([4*dpr,4*dpr]);
  ctx.beginPath();ctx.moveTo(0,ty);ctx.lineTo(W,ty);ctx.stroke();
  ctx.setLineDash([]);
  // bars
  const bw=W/hist.length;
  hist.forEach((v,i)=>{
    const clamped=Math.max(-range,Math.min(range,v));
    const barH=Math.abs(clamped/range)*(H/2);
    const y=clamped>=0?H/2-barH:H/2;
    ctx.fillStyle=clamped>REL_THRESH?`rgba(200,240,96,${0.5+0.4*(clamped/range)})`:`rgba(64,96,240,${0.3+0.3*(Math.abs(clamped)/range)})`;
    ctx.fillRect(i*bw+1,y,bw-2,barH);
  });
}

async function poll() {
  try {
    const d=await fetch('/data',{cache:'no-store'}).then(r=>r.json());
    const now=Date.now();
    document.getElementById('fps').textContent=Math.round(1000/Math.max(1,now-lastT))+' fps';
    lastT=now;
    const s=d.smile;
    document.getElementById('face').classList.toggle('smiling',s);
    document.getElementById('emoji').textContent=s?'😄':'😐';
    document.getElementById('slabel').textContent=s?'Smile detected!':'No smile';
    document.getElementById('sig').classList.toggle('active',s);
    document.getElementById('v-rel').textContent=(d.relative>=0?'+':'')+d.relative;
    document.getElementById('v-rel').style.color=d.relative>REL_THRESH?'var(--smile)':d.relative>0?'var(--text)':'var(--subtle)';
    document.getElementById('v-con').textContent=d.contrast;
    document.getElementById('v-total').textContent=d.total;
    document.getElementById('v-frames').textContent=d.frames;
    // relative bar: centre=0, right=positive, left=negative
    const rb=document.getElementById('rel-bar');
    const pct=Math.min(50,Math.abs(d.relative)/80*50);
    if(d.relative>=0){rb.style.left='50%';rb.style.width=pct+'%';rb.style.background=d.relative>REL_THRESH?'var(--smile)':'var(--neutral)';}
    else{rb.style.left=(50-pct)+'%';rb.style.width=pct+'%';rb.style.background='var(--accent)';}
    const cm=document.getElementById('m-con');
    cm.style.width=Math.min(100,d.contrast/255*100)+'%';
    cm.classList.toggle('hi',d.contrast>50);
    hist.push(d.relative);
    if(hist.length>60)hist.shift();
    drawChart();
    if(d.new_smile)addLog('Smile confirmed — GPIO 43 HIGH',true);
    document.getElementById('pill').textContent='ONLINE';
    document.getElementById('pill').classList.add('online');
  } catch(e){
    document.getElementById('pill').textContent='OFFLINE';
    document.getElementById('pill').classList.remove('online');
  }
  setTimeout(poll,50);
}

function addLog(msg,smile){
  const el=document.createElement('div');el.className='lr';
  const t=new Date().toTimeString().slice(0,8);
  el.innerHTML=`<span class="lt">${t}</span><span class="${smile?'es':''}">${msg}</span>`;
  const lg=document.getElementById('log');lg.prepend(el);
  while(lg.children.length>50)lg.removeChild(lg.lastChild);
}
addLog('Dashboard connected',false);
poll();
window.addEventListener('resize',drawChart);
</script>
</body>
</html>
)rawhtml";

void handleRoot() { server.send(200,"text/html",INDEX_HTML); }

void handleData() {
  bool smile=(millis()set_brightness(s,1); s->set_contrast(s,1); s->set_saturation(s,-1);
    s->set_whitebal(s,1);   s->set_awb_gain(s,1);
    s->set_exposure_ctrl(s,1); s->set_aec2(s,1);
    s->set_gainceiling(s,GAINCEILING_4X);
  }

  Serial.printf("Connecting to %s",WIFI_SSID);
  WiFi.begin(WIFI_SSID,WIFI_PASS);
  for(int i=0;i<40&&WiFi.status()!=WL_CONNECTED;i++){delay(500);Serial.print(".");}
  if(WiFi.status()==WL_CONNECTED)
    Serial.printf("\n>>> Open: http://%s\n",WiFi.localIP().toString().c_str());
  else
    Serial.println("\nWiFi failed — running headless");

  server.on("/",    handleRoot);
  server.on("/data",handleData);
  server.begin();
  Serial.println("Ready! Watch Rel value — smile when >+22");
}

void loop() {
  server.handleClient();
  digitalWrite(SIGNAL_PIN,millis()buf,fb->width,fb->height);
  esp_camera_fb_return(fb);

  if(smileNow){
    if(++consecutiveSmiles>=SMILE_FRAMES){
      if(millis()>smileCooldownUntil){
        Serial.println(">>> SMILE CONFIRMED");
        smileHoldUntil     =millis()+SMILE_HOLD_MS;
        smileCooldownUntil =millis()+COOLDOWN_MS;
        totalSmiles++;
        newSmileFlag=true;
      }
      consecutiveSmiles=0;
    }
  } else {
    consecutiveSmiles=0;
  }
}

import cv2
import os
import time

# ===== CONFIG =====
people = ["Mariam", "Hrach"]   # <-- persons --> #
dataset_path = "dataset"
images_per_person = 100
img_size = 320

# Load face detector
face_cascade = cv2.CascadeClassifier(
    cv2.data.haarcascades + "haarcascade_frontalface_default.xml"
)

# Create dataset folders
for person in people:
    path = os.path.join(dataset_path, person)
    os.makedirs(path, exist_ok=True)

# Start webcam
cap = cv2.VideoCapture(0)

if not cap.isOpened():
    print("Error: Cannot access camera")
    exit()

print("\nPress 's' to start capturing for each person.")
print("Press 'q' to quit.\n")

for person in people:
    print(f"\n➡️ Get ready for: {person}")
    input("Press ENTER when ready...")

    count = 0
    save_path = os.path.join(dataset_path, person)

    while count < images_per_person:
        ret, frame = cap.read()
        if not ret:
            break

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        faces = face_cascade.detectMultiScale(
            gray,
            scaleFactor=1.2,
            minNeighbors=5,
            minSize=(50, 50)
        )

        for (x, y, w, h) in faces:
            face = frame[y:y+h, x:x+w]

            # Resize to 160x160
            face_resized = cv2.resize(face, (img_size, img_size))

            filename = os.path.join(save_path, f"{person}-{count}.jpg")
            cv2.imwrite(filename, face_resized)

            count += 1

            # Draw rectangle
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0,255,0), 2)

            print(f"{person}: {count}/{images_per_person}")

            time.sleep(0.15)  # small delay

            break  # only first face

        cv2.imshow("Dataset Generator", frame)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    print(f"✅ Done for {person}")

cap.release()
cv2.destroyAllWindows()

print("\n🎉 Dataset collection complete!")