CyberPet Cube Companion

// ============================================================
//  CyberPet V4 — main.cpp
//  Orchestrator only. No business logic lives here.
//
//  Board : ESP32-S3-DevKitM-1 N16R8 (16 MB flash, 8 MB Octal PSRAM)
//  Power : 2x WLY102050 1000mAh LiPo in PARALLEL -> 3.7V / 2000mAh
//          -> CD42 IP5306 boost -> 5V rail -> ESP32-S3 5V pin
//          PARALLEL = 3.7V always. NEVER assume 7.4V series.
//
//  GPIO 33-37: RESERVED for Octal PSRAM. Do not wire or write.
//
//  Feature modules:
//    Config.h              -- pins, constants, enums, shared state
//    Sensors.h/.cpp        -- APDS9960, MPU6050, TTP223, Piezo, IP5306
//    Display_Face.h/.cpp   -- LovyanGFX sprite engine, 20 animations
//    Audio_AI.h/.cpp       -- INMP441 RX, MAX98357A TX, acoustic events
//    Kinematics.h/.cpp     -- eased servo control, jaw sync, shiver
//    MachineLearning.h/.cpp -- NVS, Echo Drift, personality, habits
//    WiFi_Cloud.h/.cpp     -- Wi-Fi, NTP, OWM weather, cloud AI bridge
// ============================================================

#include <Arduino.h>
#include <Wire.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "Config.h"
#include "Sensors.h"
#include "Display_Face.h"
#include "Audio_AI.h"
#include "Kinematics.h"
#include "MachineLearning.h"
#include "WiFi_Cloud.h"

#define I2C_SDA 8
#define I2C_SCL 9
#define PRIVACY_LED 18

// Pin constants (I2C_SDA, I2C_SCL, PRIVACY_LED) come from Config.h.
// The #defines below are redundant with Config.h — remove to avoid redefinition.
// (kept as comments for cross-reference during hardware assembly)
// I2C_SDA = 8, I2C_SCL = 9, PRIVACY_LED = 18

// ─────────────────────────────────────────────────────────────
//  SHARED GLOBAL STATE DEFINITIONS
//  (declared extern in Config.h; defined once here)
// ─────────────────────────────────────────────────────────────

// Forward declarations
static Mood vibeToMood(float v);
static void taskPrivacyMonitor(void* pv);
static void taskMoodReactor(void* pv);
static void taskWeatherReactor(void* pv);
static void taskBatteryDrama(void* pv);

volatile float            g_vibeScore       = 50.0f;
volatile Mood             g_currentMood     = MOOD_CONTENT;
volatile bool             g_privacyMode     = false;
volatile bool             g_audioPlaying    = false;
volatile uint8_t          g_batteryPct      = 100;
volatile WeatherCondition g_weather         = WX_CLEAR;
volatile float            g_tempC           = 20.0f;
volatile PowerMode        g_powerMode       = POWER_UPS;   // UPS assumed at boot
volatile bool             g_dramaModeActive = false;        // set by Sensors_Task
SemaphoreHandle_t         g_moodMutex       = nullptr;
SemaphoreHandle_t         g_servoMutex      = nullptr;

// ─────────────────────────────────────────────────────────────
//  MOOD MAPPING  (vibe score -> Mood enum)
// ─────────────────────────────────────────────────────────────
static Mood vibeToMood(float v) {
  if (v >= 90.0f) return MOOD_ECSTATIC;
  if (v >= 75.0f) return MOOD_HAPPY;
  if (v >= 65.0f) return MOOD_AFFECTIONATE;
  if (v >= 55.0f) return MOOD_CONTENT;
  if (v >= 45.0f) return MOOD_EAGER;
  if (v >= 35.0f) return MOOD_WONDER;
  if (v >= 25.0f) return MOOD_SAD;
  if (v >= 15.0f) return MOOD_LONELY;
  if (v >= 10.0f) return MOOD_ANXIOUS;
  if (v >=  5.0f) return MOOD_PANIC;
  return MOOD_REGRETFUL;
}

// ─────────────────────────────────────────────────────────────
//  PRIVACY PIN MONITOR TASK
//  Physical switch cuts 3.3V to mic + camera; GPIO18 goes LOW.
//  This task detects that and sets g_privacyMode accordingly.
// ─────────────────────────────────────────────────────────────
static void taskPrivacyMonitor(void* pv) {
  bool lastState = false;
  while (true) {
    bool currentState = (digitalRead(PRIVACY_LED) == LOW);
    if (currentState != lastState) {
      lastState     = currentState;
      g_privacyMode = currentState;
      WiFi_SetPrivacyMode(currentState);
      if (currentState) {
        Serial.println("[PRIV] Privacy mode ON -- mic + camera power cut");
        Display_ShowPrivacyMode();
      } else {
        Serial.println("[PRIV] Privacy mode OFF");
        Display_SetMood(g_currentMood);
      }
    }
    vTaskDelay(pdMS_TO_TICKS(200));
  }
}

// ─────────────────────────────────────────────────────────────
//  MOOD REACTOR TASK
//  Reads sensor events every 100 ms and applies mood hierarchy.
//  Priority order (highest first):
//    1. MPU6050 drop/shake/upside-down  (dizzy overrides all)
//    2. Piezo impact                    (angry/scared)
//    3. Audio events                    (loud/whisper/rhythm)
//    4. Touch events                    (happy, spam -> annoyance)
//    5. APDS9960 gesture / proximity
//    6. Baseline vibe drift from ML
//
//  DRAMA MODE GATE: when g_dramaModeActive is true, high-energy
//  Kinematics calls (sweeps, wobbles, snaps) are skipped. The
//  robot still reacts emotionally (face animations, tones) but
//  conserves power by not running servos unnecessarily.
// ─────────────────────────────────────────────────────────────
static void taskMoodReactor(void* pv) {
  SensorEvents ev;
  Mood         lastMood = MOOD_CONTENT;

  while (true) {
    Sensors_GetEvents(ev);
    Sensors_ClearEvents();

    // Drama gate: block high-energy servo moves in POWER_DRAMA
    bool servoOk = !g_dramaModeActive;

    // ── Priority 1: Motion extremes ───────────────────────────
    if (ev.dropped) {
      if (servoOk) Kinematics_Wobble(6, 20);
      Display_SetAnim(ANIM_SPIRAL_DIZZY);
      vTaskDelay(pdMS_TO_TICKS(800));
    } else if (ev.shake) {
      if (servoOk) Kinematics_Wobble(8, 25);
      Display_SetAnim(ANIM_SPIRAL_DIZZY);
      vTaskDelay(pdMS_TO_TICKS(600));
    } else if (ev.upsideDown) {
      Display_SetAnim(ANIM_PANIC);
      if (servoOk) Kinematics_MoveTo(SERVO_CENTER, SERVO_TILT_MAX, 6, 10);
    }

    // ── Priority 2: Desk impact ───────────────────────────────
    else if (ev.piezoHit) {
      if (ev.rhythmicClap) {
        Display_SetAnim(ANIM_EAGER);
        if (servoOk) {
          Kinematics_MoveTo(SERVO_CENTER - 20, SERVO_CENTER, 4, 8);
          vTaskDelay(pdMS_TO_TICKS(200));
          Kinematics_MoveTo(SERVO_CENTER + 20, SERVO_CENTER, 4, 8);
          vTaskDelay(pdMS_TO_TICKS(200));
          Kinematics_Center();
        }
      } else {
        Display_SetAnim(ANIM_SQUINT_ANGRY);
        if (servoOk) {
          Kinematics_HeadSnap(SERVO_CENTER, SERVO_TILT_MIN + 10);
          vTaskDelay(pdMS_TO_TICKS(400));
          Kinematics_Center();
        }
      }
      ML_LogAudioInteraction(AUD_EVT_LOUD);
    }

    // ── Priority 3: Audio events ──────────────────────────────
    else {
      AudioEvent aev = Audio_PollEvent();
      switch (aev) {
        case AUD_EVT_LOUD:
          Display_SetAnim(ANIM_PANIC);
          if (servoOk) Kinematics_HeadSnap(SERVO_CENTER, SERVO_TILT_MIN + 15);
          ML_LogAudioInteraction(AUD_EVT_LOUD);
          break;
        case AUD_EVT_SUSTAINED:
          Display_SetAnim(ANIM_SQUINT_ANGRY);
          break;
        case AUD_EVT_SILENCE:
          Display_SetAnim(ANIM_WONDER);
          if (servoOk) {
            Kinematics_MoveTo(SERVO_CENTER - 15, SERVO_CENTER, 8, 12);
            vTaskDelay(pdMS_TO_TICKS(300));
            Kinematics_Center();
          }
          break;
        case AUD_EVT_WHISPER:
          Display_SetAnim(ANIM_AFFECTIONATE);
          if (servoOk) Kinematics_MoveTo(SERVO_CENTER, SERVO_CENTER + 10, 10, 14);
          ML_LogAudioInteraction(AUD_EVT_WHISPER);
          break;
        case AUD_EVT_RHYTHM:
          Display_SetAnim(ANIM_ECSTATIC);
          if (servoOk) Kinematics_Wobble(4, 12);
          ML_LogAudioInteraction(AUD_EVT_RHYTHM);
          break;
        case AUD_EVT_WAKEWORD:
          Display_SetAnim(ANIM_EAGER);
          if (servoOk) Kinematics_Center();
          break;
        default: break;
      }

      // ── Priority 4: Touch ─────────────────────────────────
      if (ev.touched) {
        if (ev.touchSpam) {
          Display_SetAnim(ANIM_SQUINT_ANGRY);
        } else {
          Display_SetAnim(ANIM_AFFECTIONATE);
          if (servoOk) {
            Kinematics_MoveTo(SERVO_CENTER, SERVO_CENTER + 8, 10, 14);
            vTaskDelay(pdMS_TO_TICKS(500));
            Kinematics_Center();
          }
        }
        ML_LogPet();
      }

      // ── Priority 5: Gesture ───────────────────────────────
      if (ev.gesture != 0 && servoOk) {
        int curPan  = Kinematics_GetPan();
        int curTilt = Kinematics_GetTilt();
        switch (ev.gesture) {
          case 0x01:
            Kinematics_MoveTo(curPan,
              constrain(curTilt + 15, SERVO_TILT_MIN, SERVO_TILT_MAX),
              8, SERVO_EASE_STEP_MS);
            break;
          case 0x02:
            Kinematics_MoveTo(curPan,
              constrain(curTilt - 15, SERVO_TILT_MIN, SERVO_TILT_MAX),
              8, SERVO_EASE_STEP_MS);
            break;
          case 0x03:
            Kinematics_MoveTo(
              constrain(curPan - 15, SERVO_PAN_MIN, SERVO_PAN_MAX),
              curTilt, 8, SERVO_EASE_STEP_MS);
            break;
          case 0x04:
            Kinematics_MoveTo(
              constrain(curPan + 15, SERVO_PAN_MIN, SERVO_PAN_MAX),
              curTilt, 8, SERVO_EASE_STEP_MS);
            break;
          default: break;
        }
      }

      if (ev.proximity > 180 && !g_dramaModeActive) {
        Display_SetAnim(ANIM_EAGER);
      }
    }

    // ── Mood sync to vibe score ───────────────────────────────
    float vs = 50.0f;
    if (xSemaphoreTake(g_moodMutex, 5)) {
      vs = g_vibeScore;
      xSemaphoreGive(g_moodMutex);
    }
    Mood newMood = vibeToMood(vs);
    if (newMood != lastMood) {
      lastMood = newMood;
      if (xSemaphoreTake(g_moodMutex, 5)) {
        g_currentMood = newMood;
        xSemaphoreGive(g_moodMutex);
      }
      Display_SetMood(newMood);
    }

    // ── Sleep debt brightness debuff ──────────────────────────
    uint8_t debt = ML_GetSleepDebt();
    if (debt > 0) {
      Display_SetBrightness((uint8_t)(200 - debt * 20));
    }

    // ── Accountability (online only) ──────────────────────────
    if (WiFi_IsConnected()) {
      if (ML_IsIgnoredTooLong()) {
        Display_SetAnim(ANIM_LONELY);
        Serial.println("[MOOD] Emergency check-in: ignored > 48h");
      } else if (ML_ShouldGuiltTrip()) {
        Display_SetAnim(ANIM_SAD);
      } else if (ML_ShouldCelebrate()) {
        Display_SetAnim(ANIM_ECSTATIC);
        Audio_PlayTone(880, 300);
        vTaskDelay(pdMS_TO_TICKS(100));
        Audio_PlayTone(1046, 300);
      }
    }

    // ── Ghost Mode (~1 in 200 ticks) ─────────────────────────
    if (random(200) == 42 && !g_privacyMode) {
      Display_ShowGhostMode();
      vTaskDelay(pdMS_TO_TICKS(900));
    }

    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// ─────────────────────────────────────────────────────────────
//  WEATHER REACTION TASK  (2-minute poll)
// ─────────────────────────────────────────────────────────────
static void taskWeatherReactor(void* pv) {
  WeatherCondition lastWx = WX_CLEAR;
  while (true) {
    WeatherCondition wx = g_weather;
    if (wx != lastWx) {
      lastWx = wx;
      Display_ShowWeatherOverlay(wx, g_tempC);
      switch (wx) {
        case WX_COLD:
          Display_SetAnim(ANIM_SHIVER);
          Kinematics_Shiver(2500);
          break;
        case WX_HOT:
          Display_ShowWeatherOverlay(WX_HOT, g_tempC);
          break;
        case WX_RAIN:
          Display_SetAnim(ANIM_SAD);
          break;
        case WX_SNOW:
          Display_SetAnim(ANIM_WONDER);
          break;
        default: break;
      }
    }
    vTaskDelay(pdMS_TO_TICKS(120000));
  }
}

// ─────────────────────────────────────────────────────────────
//  BATTERY DRAMA TASK  (15-second poll)
//
//  Reads g_powerMode (written by Sensors_Task every 30 s) and
//  drives the three power-awareness states:
//
//  POWER_UPS      — "Infinite Desk Power" — USB-C plugged in.
//                   IP5306 charging flag = true. All features OK.
//                   Robot knows it has stable power; no warnings.
//
//  POWER_BATTERY  — Running on the 2× WLY102050 parallel pack.
//                   IP5306 charging flag = false, pct > 15%.
//                   Light status display only; all features still
//                   enabled. Vibe debuff at pct < 30% (mild worry).
//
//  POWER_DRAMA    — Battery mode, pct <= 15%. Survival mode.
//                   Robot refuses high-energy tasks (servo sweeps,
//                   cloud AI, camera capture) via g_dramaModeActive.
//                   Triggers: SAD animation, low-descending tones,
//                   battery-warning display, "point at USB-C" nod.
//                   Blinks sluggishly (Display_SetBrightness reduced).
//                   Exits ONLY when IP5306 charging flag returns true
//                   (robot plugged back in) — then plays recovery chime.
//
//  PowerMode transitions are hysteresis-handled inside Sensors_Task.
//  This task only reacts to the current g_powerMode value.
// ─────────────────────────────────────────────────────────────
static void taskBatteryDrama(void* pv) {
  PowerMode lastMode      = POWER_UPS;
  bool      dramaAnnounced = false;
  uint32_t  dramaTickMs   = 0;  // timestamp of last drama reminder

  while (true) {
    PowerMode mode = g_powerMode;
    uint8_t   pct  = g_batteryPct;

    if (mode != lastMode) {
      // ── Mode transition handler ─────────────────────────────
      switch (mode) {

        case POWER_UPS:
          // Just plugged back in
          Serial.println("[BAT] USB-C connected — UPS / Desk Power mode");
          if (lastMode == POWER_DRAMA) {
            // Recovery: play a relieved ascending chime
            Display_SetMood(g_currentMood);
            Display_SetBrightness(200);
            Audio_PlayTone(330, 100);
            vTaskDelay(pdMS_TO_TICKS(60));
            Audio_PlayTone(523, 100);
            vTaskDelay(pdMS_TO_TICKS(60));
            Audio_PlayTone(784, 200);
            Serial.println("[BAT] Survival mode cleared — recovered");
          }
          dramaAnnounced = false;
          break;

        case POWER_BATTERY:
          // Just unplugged, or leaving drama after top-up
          Serial.printf("[BAT] Battery mode — %d%% remaining\n", pct);
          // Brief status update on display; no alarm
          Display_ShowBatteryWarning(pct);
          vTaskDelay(pdMS_TO_TICKS(3000));
          Display_SetMood(g_currentMood);
          dramaAnnounced = false;
          break;

        case POWER_DRAMA:
          // Entering drama mode for the first time
          if (!dramaAnnounced) {
            dramaAnnounced = true;
            dramaTickMs    = millis();
            Serial.printf("[BAT] DRAMA MODE — %d%% — survival engaged\n", pct);

            // ── Visual: sad + dim ─────────────────────────────
            Display_SetAnim(ANIM_SAD);
            Display_SetBrightness(70);
            Display_ShowBatteryWarning(pct);

            // ── Audio: descending sigh tones ──────────────────
            // High-energy task gate is already set by g_dramaModeActive;
            // Audio_PlayTone is explicitly allowed in drama (low power).
            Audio_PlayTone(440, 350);
            vTaskDelay(pdMS_TO_TICKS(120));
            Audio_PlayTone(370, 400);
            vTaskDelay(pdMS_TO_TICKS(120));
            Audio_PlayTone(294, 600);

            // ── Kinematic: tilt down toward USB-C port ────────
            // Robot "points" chin down toward its own base where
            // the USB-C port is located.
            if (!g_dramaModeActive) {  // race-condition guard
              Kinematics_MoveTo(SERVO_CENTER, SERVO_TILT_MIN + 8, 12, 14);
              vTaskDelay(pdMS_TO_TICKS(1200));
              Kinematics_Center();
            }
          }
          break;
      }
      lastMode = mode;
    }

    // ── Periodic drama reminder every 3 minutes ───────────────
    // While in drama mode: robot continues to blink sluggishly and
    // sigh periodically until plugged in. Reminder fires every 3 min.
    if (mode == POWER_DRAMA &&
        (millis() - dramaTickMs) > 180000UL) {
      dramaTickMs = millis();
      Serial.printf("[BAT] Still in drama — %d%% remaining\n", pct);
      Display_ShowBatteryWarning(pct);

      // Sluggish blink: dim, wait, re-dim
      Display_SetBrightness(40);
      vTaskDelay(pdMS_TO_TICKS(300));
      Display_SetBrightness(70);

      // Short sigh tone (low power: single tone only)
      Audio_PlayTone(294, 500);
    }

    // ── Battery-mode periodic display update (every 5 min) ────
    if (mode == POWER_BATTERY) {
      static uint32_t lastBattDisplay = 0;
      if ((millis() - lastBattDisplay) > 300000UL) {
        lastBattDisplay = millis();
        Display_ShowBatteryWarning(pct);
        vTaskDelay(pdMS_TO_TICKS(2500));
        Display_SetMood(g_currentMood);
      }
    }

    vTaskDelay(pdMS_TO_TICKS(15000));  // check every 15 s
  }
}

// ─────────────────────────────────────────────────────────────
//  SETUP
// ─────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  delay(400);
  Serial.println("=================================");
  Serial.println("     CyberPet V4  Booting");
  Serial.println("  ESP32-S3-DevKitM-1  N16R8");
  Serial.printf( "  Display: %s\n", DISP_LABEL);
  Serial.println("  2x WLY102050 // 2000mAh PARALLEL");
  Serial.println("=================================");

  // Shared semaphores
  g_moodMutex  = xSemaphoreCreateMutex();
  g_servoMutex = xSemaphoreCreateMutex();
  configASSERT(g_moodMutex);
  configASSERT(g_servoMutex);

  // I2C bus
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(I2C_FREQ);
  Serial.println("[I2C] Bus ready at 400 kHz");

  // Privacy LED pin (input -- physical switch drives it)
  pinMode(PRIVACY_LED, INPUT_PULLUP);

  // Module init
  Display_Init();
  Display_ShowBootSplash();

  ML_Init();
  Sensors_Init();
  Audio_Init();
  WiFi_Init();

  // Seed vibe from ML learned baseline
  if (xSemaphoreTake(g_moodMutex, portMAX_DELAY)) {
    g_vibeScore = ML_GetVibeBaseline();
    xSemaphoreGive(g_moodMutex);
  }

  Serial.printf("[ML]  Personality: %s  Baseline vibe: %.1f\n",
                ML_GetPersonalityTag(), ML_GetVibeBaseline());

  // Startup chime (C5-E5-G5)
  Audio_PlayTone(523, 120);
  vTaskDelay(pdMS_TO_TICKS(40));
  Audio_PlayTone(659, 120);
  vTaskDelay(pdMS_TO_TICKS(40));
  Audio_PlayTone(784, 180);

  // Launch FreeRTOS tasks
  xTaskCreatePinnedToCore(Kinematics_Task,    "Kinematics", STACK_KINEMATICS,
                          NULL, PRI_KINEMATICS, NULL, CORE_KINEMATICS);
  xTaskCreatePinnedToCore(Sensors_Task,       "Sensors",    STACK_SENSORS,
                          NULL, PRI_SENSORS,    NULL, CORE_SENSORS);
  xTaskCreatePinnedToCore(Audio_Task,         "Audio",      STACK_AUDIO,
                          NULL, PRI_AUDIO,      NULL, CORE_AUDIO);
  xTaskCreatePinnedToCore(Display_Task,       "Display",    STACK_DISPLAY,
                          NULL, PRI_DISPLAY,    NULL, CORE_DISPLAY);
  xTaskCreatePinnedToCore(WiFi_Task,          "WiFi",       STACK_WIFI,
                          NULL, PRI_WIFI,       NULL, CORE_WIFI);
  xTaskCreatePinnedToCore(ML_Task,            "ML",         STACK_ML,
                          NULL, PRI_ML,         NULL, CORE_ML);
  xTaskCreatePinnedToCore(taskMoodReactor,    "MoodReactor",5120,
                          NULL, 2,              NULL, CORE_DISPLAY);
  xTaskCreatePinnedToCore(taskWeatherReactor, "Weather",    3072,
                          NULL, 1,              NULL, CORE_DISPLAY);
  xTaskCreatePinnedToCore(taskBatteryDrama,   "Battery",    2048,
                          NULL, 1,              NULL, CORE_DISPLAY);
  xTaskCreatePinnedToCore(taskPrivacyMonitor, "Privacy",    2048,
                          NULL, 1,              NULL, CORE_DISPLAY);

  Serial.println("[MAIN] All tasks launched.");
}

// ─────────────────────────────────────────────────────────────
//  LOOP  -- idle. All work is in FreeRTOS tasks.
// ─────────────────────────────────────────────────────────────
void loop() {
  // PSRAM health check once at first loop pass
  static bool psramChecked = false;
  if (!psramChecked) {
    psramChecked = true;
    Serial.printf("[MEM] Free PSRAM : %zu KB\n",
                  heap_caps_get_free_size(MALLOC_CAP_SPIRAM) / 1024);
    Serial.printf("[MEM] Free heap  : %zu KB\n",
                  heap_caps_get_free_size(MALLOC_CAP_INTERNAL) / 1024);
  }
  vTaskDelay(pdMS_TO_TICKS(1000));
}