// =============================================================================
//  PULCINO — firmware per Seeed XIAO ESP32S3 Sense
//  Mini robot bipede open source: 6 servo MG90S, IMU MPU6050, camera, mic, speaker.
//
//  Contratto (pin, API, formati): SPEC.md nella radice del repo.
//  Configurazione dell'utente: config.h (Wi-Fi, token, variante PCA9685, calibrazione).
//
//  Moduli:
//   servos     uscita PWM verso i servo (ESP32Servo oppure PCA9685)
//   imu        MPU6050 + filtro complementare
//   poses      pose a keyframe (stand / rest / wave)
//   gait_cpg   camminata a oscillatori da gait.json (LittleFS)
//   policy_mlp rete neurale da policy.h (se valida)
//   motion     sceglie il generatore, transizioni morbide, watchdog 1 s
//   camera     OV2640/OV3660 in PSRAM
//   audio      mic PDM -> WS, WS/beep -> speaker I2S
//   web        Wi-Fi/AP/mDNS, HTTP porta 80 e 81, WebSocket /ws e /audio
// =============================================================================
#include <Wire.h>
#include "config.h"
#include "servos.h"
#include "imu.h"
#include "gait_cpg.h"
#include "policy_mlp.h"
#include "motion.h"
#include "camera.h"
#include "audio.h"
#include "web.h"

void setup() {
  Serial.begin(115200);
  delay(300);
  Serial.printf("\n=== Pulcino firmware %s (%s) ===\n", FW_VERSION,
                USE_PCA9685 ? "PCA9685" : "servo diretti");

  // 1) Camera PRIMA dei servo: occupa LEDC timer 0 per il suo clock XCLK
  if (cameraBegin()) Serial.println("[camera] ok");

  // 2) I²C (IMU e, se presente, PCA9685) a 400 kHz
  Wire.begin(PIN_SDA, PIN_SCL, 400000);

  // 3) Servo: vanno subito in piedi (90° + offset) per non "cadere" all'avvio
  servosBegin();

  // 4) IMU: calibra il giroscopio -> tenere il robot fermo per ~1 s all'accensione
  if (imuBegin()) Serial.println("[imu] MPU6050 ok");
  else Serial.println("[imu] MPU6050 NON trovato: controlla SDA=D4, SCL=D5, VCC 3,3 V");

  // 5) Camminata (LittleFS) e politica
  gaitBegin();
  Serial.printf("[policy] %s\n", policyValid() ? "rete valida" : "segnaposto -> uso il CPG");

  // 6) Movimento
  motionBegin();

  // 7) Audio (mic su I2S0, speaker su I2S1)
  audioBegin();

  // 8) Rete e server web
  netBegin();
  webBegin();

  audioHello();   // "ciao!": pronto
  eyesSet(true, true);
}

void loop() {
  // Il ciclo di controllo a 50 Hz gira qui (core 1); rete, audio e camera
  // lavorano in task separati sul core 0.
  motionUpdate();
  vTaskDelay(1);
}