/*
   main.c

   Odradek Simulator on ESP32 using ESP-IDF & FreeRTOS
  · This guy is connected to https://wokwi.com/projects/431219132727744513 (BBpod)
  thanks to MQTT.
   ---------------------------------------------------
   - Reads two HC-SR04 ultrasonic sensors (front and bottom)
   - Samples an analog noise sensor (microphone) on ADC1_CH6 -> Tried with custom chip, but i'm using a slider potentiometer
   - Drives a buzzer via PWM to emulate “lup-dup” heartbeat sounds
     (lower-pitched “lup”)
   - Controls two servos:
         • claw‐servo: locks or opens the claw
         • radar‐servo: sweeps when no EV, locks onto direction when EV detected
   - Controls a stepper motor for chase indication
   - Updates a NeoPixel LED ring via RMT with blinking white/blue/yellow/orange/red
   - Manages an 7‐state Finite‐State Machine (FSM):
         STATE_DISCONNECTED
         STATE_CONNECTED_IDLE
         STATE_EV_FAR
         STATE_EV_CLOSE
         STATE_EV_NEAR_CAUTIOUS
         STATE_EV_NEAR_ALERT
         STATE_BB_STRESSED
   - Uses mutexes and queues for safe data sharing
   - Publishes its state every 5 s to MQTT topic "odradek/state"
   - Listens for commands ("receptor", "relax", "stress") on MQTT topic "odradek/cmd"

   MQTT broker via HiveMQ WebSocket demo (still working without this steps, but you can see how it works)
   -------------------------------------
   1) Open in browser:
        https://www.hivemq.com/demos/websocket-client/
   2) Click “Connect”
   3) Subscribe to topic:
        odradek/state
   4) Publish on topic:
        odradek/cmd
      with payload “receptor” or “relax”
   5) Watch incoming state messages on “odradek/state”
*/

#include <stdio.h>
#include <string.h>
#include <stdbool.h>

// FreeRTOS
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

// Drivers
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "driver/rmt_tx.h"
#include "esp_adc/adc_oneshot.h"
#include "sdkconfig.h"

// ESP-IDF
#include "esp_log.h"
#include "esp_timer.h"
#include "esp_rom_sys.h"

// NeoPixel (WS2812) via LED strip API
#include "led_strip.h"
#include "led_strip_rmt.h"
#include "led_strip_spi.h"
#include "led_strip_types.h"
#include "led_strip_interface.h"

// MQTT
#include "nvs_flash.h"
#include "esp_netif.h"
#include "esp_event.h"
#include "esp_wifi.h"
#include "mqtt_client.h"
#include "freertos/event_groups.h"

// 1) API
#define TAG LEDSTRIP_API_TAG
#include "led_strip_api.c"
#undef TAG
// 2) RMT device
#define TAG LEDSTRIP_RMT_DEV_TAG
#include "led_strip_rmt_dev.c"
#undef TAG
// 3) RMT encoder
#define TAG LEDSTRIP_RMT_ENCODER_TAG
#include "led_strip_rmt_encoder.c"
#undef TAG
// 4) SPI device (not used here, but included for completeness)
#define TAG LEDSTRIP_SPI_DEV_TAG
#include "led_strip_spi_dev.c"
#undef TAG

//==============================================================================
// Pin and Peripheral Channel Definitions
//==============================================================================
#define SOUND_SENSOR_ADC_CH    ADC_CHANNEL_6      // Noise sensor input (GPIO34)
#define BUZZER_GPIO            GPIO_NUM_27        // Buzzer PWM output
#define TRIG_FRONT_PIN         GPIO_NUM_5         // Front ultrasonic trigger
#define ECHO_FRONT_PIN         GPIO_NUM_18        // Front ultrasonic echo
#define TRIG_BOTTOM_PIN        GPIO_NUM_4         // Bottom ultrasonic trigger
#define ECHO_BOTTOM_PIN        GPIO_NUM_15        // Bottom ultrasonic echo
#define SERVO_CLAW_GPIO        GPIO_NUM_21        // Claw servo PWM output
#define SERVO_RADAR_GPIO       GPIO_NUM_22        // Radar servo PWM output
#define SERVO_MIN_US           1000               // 1 ms pulse
#define SERVO_MAX_US           2000               // 2 ms pulse
#define STEPPER_STEP_GPIO      GPIO_NUM_32        // Stepper driver STEP pin
#define STEPPER_DIR_GPIO       GPIO_NUM_33        // Stepper driver DIR pin
#define NEOPIXEL_GPIO          GPIO_NUM_13        // NeoPixel data line
#define NEOPIXEL_RMT_CHANNEL   RMT_CHANNEL_0      // RMT channel for NeoPixels
#define NUM_PIXELS             16                 // Number of NeoPixel LEDs
#define MAX_CMD_LEN            128                // Max length for MQTT commands

//==============================================================================
// Network and MQTT Configuration
//==============================================================================
#define WIFI_SSID              "Wokwi-GUEST"                // Wi-Fi SSID
#define WIFI_PASS              ""                            // Wi-Fi password

#define MQTT_BROKER_URI        "mqtt://broker.hivemq.com:1883/mqtt" // MQTT broker URI
#define MQTT_USER              ""                            // MQTT username (optional)
#define MQTT_PASSWORD          ""                            // MQTT password (optional)

// MQTT topics
#define MQTT_TOPIC_STATE       "odradek/state"             // Topic for publishing state
#define MQTT_TOPIC_CMD         "odradek/cmd"               // Topic for receiving commands

#define MQTT_CONNECTED_BIT     BIT0                         // Event bit for MQTT connection

#define KEEP_ALIVE_TIMEOUT_MS  (5  * 1000)                    // every how many times without messages we consider disconnected (in ms)
#define STRESS_TIMEOUT_MS      (10 * 1000)                    // every how much without “stress” we consider recovered (in ms)

//==============================================================================
// Thresholds and Timing Constants
//==============================================================================
#define TH_DIST_FAR            80.0f    // cm: threshold for “far” distance
#define TH_DIST_CLOSE          30.0f    // cm: threshold for “close” distance
#define TH_DIST_RECEPTOR       15.0f    // cm: threshold for receptor detection
#define TH_DIST_FLOOR          20.0f    // cm: threshold for crouch detection
#define TH_NOISE_ALERT         200      // ADC level for panic threshold

//==============================================================================
// Other Configuration
//==============================================================================
#define RMT_CLK_HZ             (10 * 1000 * 1000)         // RMT clock frequency
#define ADC_ATTENUATION        ADC_ATTEN_DB_12            // ADC attenuation setting
#define ADC_BITWIDTH           ADC_BITWIDTH_DEFAULT       // ADC resolution

//==============================================================================
// Logging Configuration (required for Wokwi)
//==============================================================================
// Enable verbose logging on Wokwi simulator
#define CONFIG_LOG_DEFAULT_LEVEL_VERBOSE 1  // Set default log level to VERBOSE
#define CONFIG_LOG_MAXIMUM_LEVEL         5  // Allow up to maximum log verbosity

//==============================================================================
// Finite-State Machine States
//==============================================================================
typedef enum {
  STATE_DISCONNECTED = 0,
  STATE_CONNECTED_IDLE,
  STATE_EV_FAR,
  STATE_EV_CLOSE,
  STATE_EV_NEAR_CAUTIOUS,
  STATE_EV_NEAR_ALERT,
  STATE_BB_STRESSED
} odradek_state_t;

//==============================================================================
// Shared Context Structure
//==============================================================================
typedef struct {
  // --- Core sensor & state data ---
  odradek_state_t          current_state;      // Active FSM state
  float                    front_distance_cm;  // Last front ultrasonic reading
  float                    bottom_distance_cm; // Last bottom ultrasonic reading
  bool                     crouched;           // bottom < TH_DIST_FLOOR
  uint8_t                  noise_level;        // Latest ADC noise sample

  // --- Panic & receptor logic ---
  bool                     receptor_detected;  // Set when “receptor” cmd received
  int64_t                  stress_start_us;    // Timestamp when panic began

  // --- RTOS primitives ---
  QueueHandle_t            noise_queue;        // Samples from noise task
  SemaphoreHandle_t        ctx_mutex;          // Guards access to this struct

  // --- MQTT comms handles ---
  esp_mqtt_client_handle_t mqtt_client;        // MQTT client instance
  EventGroupHandle_t       mqtt_event_group;   // Signals MQTT_CONNECTED_BIT
  int64_t                 last_receptor_us;    // us from last "receptor"
  int64_t                 last_stress_us;      // us from last "stress"
} odradek_ctx_t;

//==============================================================================
// Globals
//==============================================================================
static odradek_ctx_t ctx;           // Shared context instance
static const char*    TAG = "ODRADEK";

//==============================================================================
// Human-readable state names
//==============================================================================
static const char* state_names[] = {
  "DISCONNECTED",
  "CONNECTED_IDLE",
  "EV_FAR",
  "EV_CLOSE",
  "EV_NEAR_CAUTIOUS",
  "EV_NEAR_ALERT",
  "BB_STRESSED"
};

//==============================================================================
// begin_monitor: initialize shared context and RTOS primitives
//==============================================================================
void begin_monitor(void) {
  // --- Initialize context fields ---
  ctx.current_state      = STATE_DISCONNECTED;
  ctx.front_distance_cm  = -1.0f;
  ctx.bottom_distance_cm = -1.0f;
  ctx.crouched           = false;
  ctx.noise_level        = 0;
  ctx.receptor_detected  = false;
  ctx.stress_start_us    = 0;
  ctx.last_receptor_us   = 0;
  ctx.last_stress_us     = 0;


  // --- Create RTOS primitives ---
  ctx.ctx_mutex          = xSemaphoreCreateMutex();
  configASSERT(ctx.ctx_mutex);

  ctx.noise_queue        = xQueueCreate(8, sizeof(uint8_t));
  configASSERT(ctx.noise_queue);

  ctx.mqtt_event_group   = xEventGroupCreate();
  configASSERT(ctx.mqtt_event_group);
}


//==============================================================================
// Task: Noise Sensor (every 100 ms)
//  - Reads 12-bit ADC, converts to 8-bit
//  - Updates ctx.noise_level and pushes sample into ctx.noise_queue
//==============================================================================
static void task_noise_sensor(void *pv) {
  // Initialize ADC oneshot unit
  adc_oneshot_unit_init_cfg_t unit_cfg = {
    .unit_id = ADC_UNIT_1
  };
  adc_oneshot_unit_handle_t adc_handle;
  ESP_ERROR_CHECK(adc_oneshot_new_unit(&unit_cfg, &adc_handle));

  // Configure channel attenuation and bitwidth
  adc_oneshot_chan_cfg_t chan_cfg = {
    .atten    = ADC_ATTENUATION,
    .bitwidth = ADC_BITWIDTH
  };
  ESP_ERROR_CHECK(adc_oneshot_config_channel(adc_handle,
                  SOUND_SENSOR_ADC_CH,
                  &chan_cfg));

  TickType_t last_wake = xTaskGetTickCount();
  for (;;) {
    int raw;
    ESP_ERROR_CHECK(adc_oneshot_read(adc_handle,
                                     SOUND_SENSOR_ADC_CH,
                                     &raw));
    uint8_t level = raw >> 4;  // 0–4095 -> 0–255

    // Update shared context
    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    ctx.noise_level = level;
    xSemaphoreGive(ctx.ctx_mutex);

    // Push to queue for FSM
    xQueueSend(ctx.noise_queue, &level, 0);

    // Wait until next cycle
    vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(100));
  }
}

//==============================================================================
// Helper: send a 10 µs trigger and measure echo pulse -> distance in cm
//==============================================================================
static float read_distance_cm(gpio_num_t trig_pin, gpio_num_t echo_pin) {
  const int64_t TIMEOUT_US = 30000;
  // Trigger pulse
  gpio_set_level(trig_pin, 1);
  esp_rom_delay_us(10);
  gpio_set_level(trig_pin, 0);

  // Wait for echo start
  int64_t start = esp_timer_get_time();
  while (!gpio_get_level(echo_pin)) {
    if (esp_timer_get_time() - start > TIMEOUT_US) {
      return -1.0f;
    }
    taskYIELD();
  }
  int64_t t0 = esp_timer_get_time();

  // Wait for echo end
  while (gpio_get_level(echo_pin)) {
    if (esp_timer_get_time() - t0 > TIMEOUT_US) {
      return -1.0f;
    }
    taskYIELD();
  }
  int64_t t1 = esp_timer_get_time();

  // Convert time-of-flight to cm: (343 m/s * dt *100 cm/m)/2
  double dt = (t1 - t0) * 1e-6;
  return (float)(343.0 * dt * 100.0 / 2.0);
}

//==============================================================================
// Task: Front Distance Sensor (every 200 ms)
//==============================================================================
static void task_distance_front(void *pv) {
  // Configure pins
  gpio_reset_pin(TRIG_FRONT_PIN);
  gpio_set_direction(TRIG_FRONT_PIN, GPIO_MODE_OUTPUT);
  gpio_reset_pin(ECHO_FRONT_PIN);
  gpio_set_direction(ECHO_FRONT_PIN, GPIO_MODE_INPUT);
  gpio_set_pull_mode(ECHO_FRONT_PIN, GPIO_PULLDOWN_ONLY);

  TickType_t last_wake = xTaskGetTickCount();
  const TickType_t period = pdMS_TO_TICKS(200);

  for (;;) {
    float d = read_distance_cm(TRIG_FRONT_PIN, ECHO_FRONT_PIN);

    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    ctx.front_distance_cm = d;
    xSemaphoreGive(ctx.ctx_mutex);

    vTaskDelayUntil(&last_wake, period);
  }
}

//==============================================================================
// Task: Bottom (Crouch) Sensor (every 200 ms)
//==============================================================================
static void task_distance_bottom(void *pv) {
  // Configure pins
  gpio_reset_pin(TRIG_BOTTOM_PIN);
  gpio_set_direction(TRIG_BOTTOM_PIN, GPIO_MODE_OUTPUT);
  gpio_reset_pin(ECHO_BOTTOM_PIN);
  gpio_set_direction(ECHO_BOTTOM_PIN, GPIO_MODE_INPUT);
  gpio_set_pull_mode(ECHO_BOTTOM_PIN, GPIO_PULLDOWN_ONLY);

  TickType_t last_wake = xTaskGetTickCount();
  const TickType_t period = pdMS_TO_TICKS(200);

  for (;;) {
    float d = read_distance_cm(TRIG_BOTTOM_PIN, ECHO_BOTTOM_PIN);

    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    ctx.bottom_distance_cm = d;
    // if < floor threshold -> crouched
    ctx.crouched = (d >= 0 && d < TH_DIST_FLOOR);
    xSemaphoreGive(ctx.ctx_mutex);

    vTaskDelayUntil(&last_wake, period);
  }
}

//==============================================================================
// TASK: STATE MANAGER (100ms)
//==============================================================================

//==============================================================================
// Inline helper to update FSM state with mutex protection & logging
//==============================================================================
static inline void update_state(odradek_state_t new_state) {
  xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
  ctx.current_state = new_state;
  xSemaphoreGive(ctx.ctx_mutex);
  //ESP_LOGI(TAG, "FSM -> %s", state_names[new_state]);
}

//==============================================================================
// task_state_manager
//  - Runs every 100 ms
//  - Checks MQTT connectivity -> STATE_DISCONNECTED
//  - Reads latest noise sample from queue
//  - Incorporates “receptor” command flag + proximity + noise -> next state
//==============================================================================
static void task_state_manager(void* pv) {
    const TickType_t period            = pdMS_TO_TICKS(100);
    TickType_t       last_wake         = xTaskGetTickCount();
    uint8_t          new_noise;

    for (;;) {
        int64_t now = esp_timer_get_time();

        // Snapshot connectivity and timing
        xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
          bool in_stressed      = (ctx.current_state == STATE_BB_STRESSED);
          bool mqtt_connected   = (xEventGroupGetBits(ctx.mqtt_event_group) & MQTT_CONNECTED_BIT);
          int64_t delta_stress_ms = (now - ctx.last_stress_us) / 1000;
          int64_t delta_receptor_ms = (now - ctx.last_receptor_us) / 1000;
        xSemaphoreGive(ctx.ctx_mutex);

        // 1) If already in BB_STRESSED, only exit on receptor or timeout
        if (in_stressed) {
            // a) if MQTT disconnected, drop to CONNECTED_IDLE
            if (!mqtt_connected) {
                update_state(STATE_CONNECTED_IDLE);
            }
            // b) if a newer “receptor” keep-alive arrived, exit stress immediately
            else if (ctx.last_receptor_us > ctx.last_stress_us) {
                update_state(STATE_CONNECTED_IDLE);
            }
            // c) or if no “stress” keep-alive for STRESS_TIMEOUT_MS
            else if (delta_stress_ms > STRESS_TIMEOUT_MS) {
                update_state(STATE_CONNECTED_IDLE);
            }
            vTaskDelayUntil(&last_wake, period);
            continue;
        }

        // 2) If MQTT disconnected -> DISCONNECTED
        if (!mqtt_connected) {
            update_state(STATE_DISCONNECTED);
            vTaskDelayUntil(&last_wake, period);
            continue;
        }

        // 3) Handle first handshake or receptor timeout -> CONNECTED_IDLE
        if (ctx.last_receptor_us == 0 || delta_receptor_ms > KEEP_ALIVE_TIMEOUT_MS) {
            update_state(STATE_CONNECTED_IDLE);
            vTaskDelayUntil(&last_wake, period);
            continue;
        }

        // 4) Read latest noise sample (non-blocking)
        if (xQueueReceive(ctx.noise_queue, &new_noise, 0) == pdPASS) {
            xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
              ctx.noise_level = new_noise;
            xSemaphoreGive(ctx.ctx_mutex);
        }

        // 5) Snapshot sensor readings
        xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
          float dist   = ctx.front_distance_cm;
          uint8_t noise = ctx.noise_level;
          bool crouch  = ctx.crouched;
        xSemaphoreGive(ctx.ctx_mutex);

        // 6) Determine next state by priority
        odradek_state_t next;
        if (dist < TH_DIST_CLOSE && noise > TH_NOISE_ALERT && !crouch) {
            next = STATE_EV_NEAR_ALERT;
        } else if (dist < TH_DIST_CLOSE) {
            next = STATE_EV_NEAR_CAUTIOUS;
        } else if (dist < TH_DIST_FAR) {
            next = STATE_EV_CLOSE;
        } else {
            next = STATE_EV_FAR;
        }

        // 7) Commit new state
        update_state(next);
        vTaskDelayUntil(&last_wake, period);
    }
}

//==============================================================================
// Task: Visual Feedback (NeoPixel LED ring)
//  - Runs continuously, blinking or breathing per FSM state
//==============================================================================
static void task_visual_feedback(void *pv) {
  // 1) Configure LED strip over RMT
  led_strip_config_t strip_cfg = {
    .strip_gpio_num         = NEOPIXEL_GPIO,
    .max_leds               = NUM_PIXELS,
    .led_model              = LED_MODEL_WS2812,
    .color_component_format = LED_STRIP_COLOR_COMPONENT_FMT_GRB,
    .flags.invert_out       = false,
  };
  led_strip_rmt_config_t rmt_cfg = {
    .clk_src           = RMT_CLK_SRC_DEFAULT,
    .resolution_hz     = RMT_CLK_HZ,
    .mem_block_symbols = 0,
    .flags.with_dma    = false,
  };
  led_strip_handle_t strip;
  ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_cfg, &rmt_cfg, &strip));

  // 2) Define blink intervals (ms) and RGB colors for each state
  static const uint32_t blink_intervals[7] = {
    1000,   // STATE_DISCONNECTED: slow blink
    1000,   // STATE_CONNECTED_IDLE: same (or treat as breathing elsewhere)
    1000,   // STATE_EV_FAR
    750,    // STATE_EV_CLOSE
    500,    // STATE_EV_NEAR_CAUTIOUS
    250,    // STATE_EV_NEAR_ALERT
    125     // STATE_BB_STRESSED: very fast blink
  };
  static const uint8_t colors[7][3] = {
    { 50,   0,   0 },  // DISCONNECTED (dim red)
    {100,   0,   0 },  // CONNECTED_IDLE (bright red)
    {  0,   0, 255 },  // EV_FAR (blue)
    {  0,   0, 200 },  // EV_CLOSE (darker blue)
    {255, 255,   0 },  // EV_NEAR_CAUTIOUS (yellow)
    {255, 128,   0 },  // EV_NEAR_ALERT (orange)
    {255,   0,   0 }   // BB_STRESSED (red)
  };

  for (;;) {
    // Snapshot current state
    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    odradek_state_t st = ctx.current_state;
    xSemaphoreGive(ctx.ctx_mutex);

    // Disconnected: LEDS shut off
    if (st == STATE_DISCONNECTED) {
      for (int i = 0; i < NUM_PIXELS; i++)
        led_strip_set_pixel(strip, i, 0, 0, 0);
      ESP_ERROR_CHECK(led_strip_refresh(strip));
      vTaskDelay(pdMS_TO_TICKS(500));
      continue;
    }

    // Connected Idle: “breathing”
    if (st == STATE_CONNECTED_IDLE) {
      // r va de 20 -> 100 y vuelve, g=b=0
      static int8_t dir = 1;
      static uint8_t bri = 20;
      bri += dir;
      if (bri >= 100 || bri <= 20) dir = -dir;
      for (int i = 0; i < NUM_PIXELS; i++)
        led_strip_set_pixel(strip, i, bri, 0, 0);
      ESP_ERROR_CHECK(led_strip_refresh(strip));
      vTaskDelay(pdMS_TO_TICKS(20));
      continue;
    }

    // Other states just blink
    uint32_t interval = blink_intervals[st];
    uint8_t r = colors[st][0],
            g = colors[st][1],
            b = colors[st][2];

    // LEDs ON
    for (int i = 0; i < NUM_PIXELS; i++) {
      led_strip_set_pixel(strip, i, r, g, b);
    }
    ESP_ERROR_CHECK(led_strip_refresh(strip));
    vTaskDelay(pdMS_TO_TICKS(interval / 2));

    // LEDs OFF
    for (int i = 0; i < NUM_PIXELS; i++) {
      led_strip_set_pixel(strip, i, 0, 0, 0);
    }
    ESP_ERROR_CHECK(led_strip_refresh(strip));
    vTaskDelay(pdMS_TO_TICKS(interval / 2));
  }
}

//==============================================================================
// Task: Acoustic Feedback (“lup–dup” heartbeat)
//  - Uses PWM to generate two-tone beat per state
//==============================================================================
static void task_acoustic_alert(void *pv) {
  // 1) Configure LEDC timer for PWM (will reconfigure freq dynamically)
  ledc_timer_config_t tcfg = {
    .speed_mode      = LEDC_HIGH_SPEED_MODE,
    .timer_num       = LEDC_TIMER_0,
    .duty_resolution = LEDC_TIMER_8_BIT,
    .freq_hz         = 1000,           // placeholder, will update
    .clk_cfg         = LEDC_AUTO_CLK
  };
  ESP_ERROR_CHECK(ledc_timer_config(&tcfg));

  // 2) Configure channel for buzzer
  ledc_channel_config_t ccfg = {
    .gpio_num   = BUZZER_GPIO,
    .speed_mode = LEDC_HIGH_SPEED_MODE,
    .channel    = LEDC_CHANNEL_0,
    .intr_type  = LEDC_INTR_DISABLE,
    .timer_sel  = LEDC_TIMER_0,
    .duty       = 0,
    .hpoint     = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&ccfg));

  // 3) Define beat intervals (ms) for each state
  static const uint32_t beat_intervals[8] = {
    1500,  // DISCONNECTED / IDLE
    1500,  // CONNECTED_IDLE
    1000,  // EV_FAR
    750,   // EV_CLOSE
    500,   // EV_NEAR_CAUTIOUS
    300,   // EV_NEAR_ALERT
    250,   // RECEPTOR_PRESENT
    150    // BB_STRESSED
  };

  for (;;) {
    // Snapshot state
    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    odradek_state_t st = ctx.current_state;
    xSemaphoreGive(ctx.ctx_mutex);

    // States without heart-beat
    if (st == STATE_DISCONNECTED || st == STATE_CONNECTED_IDLE) {
      ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0);
      ledc_update_duty(ccfg.speed_mode, ccfg.channel);
      vTaskDelay(pdMS_TO_TICKS(100)); // pause to avoid overload
      continue;
    }

    uint32_t interval = beat_intervals[st];
    // “LUP” tone: lower pitched, longer
    tcfg.freq_hz = 800;
    ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
    ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0xFF);
    ledc_update_duty(ccfg.speed_mode, ccfg.channel);
    vTaskDelay(pdMS_TO_TICKS(70));

    // short silence between
    ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0);
    ledc_update_duty(ccfg.speed_mode, ccfg.channel);
    vTaskDelay(pdMS_TO_TICKS(50));

    // “DUP” tone: higher pitched, shorter
    tcfg.freq_hz = 1200;
    ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
    ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0xC0);
    ledc_update_duty(ccfg.speed_mode, ccfg.channel);
    vTaskDelay(pdMS_TO_TICKS(30));

    // silence for the rest of the interval
    ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0);
    ledc_update_duty(ccfg.speed_mode, ccfg.channel);

    if (st == STATE_BB_STRESSED) {
      tcfg.freq_hz = 1000;
      ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
      ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0xFF);
      ledc_update_duty(ccfg.speed_mode, ccfg.channel);
      // no delay, is "dead" until next change
      continue;
    }

    if (interval > 150) {
      vTaskDelay(pdMS_TO_TICKS(interval - 150));
    } else {
      taskYIELD();
    }
  }
}


//==============================================================================
// Task: Mechanism Control (claw-servo, radar-servo, stepper)
//==============================================================================
static void task_mechanism_control(void *pv) {
  // 1) Configure 50 Hz PWM for servos
  ledc_timer_config_t t2 = {
    .speed_mode      = LEDC_LOW_SPEED_MODE,
    .timer_num       = LEDC_TIMER_1,
    .duty_resolution = LEDC_TIMER_16_BIT,
    .freq_hz         = 50,
    .clk_cfg         = LEDC_AUTO_CLK
  };
  ESP_ERROR_CHECK(ledc_timer_config(&t2));

  // Claw‐servo (canal 1)
  ledc_channel_config_t claw = {
    .gpio_num   = SERVO_CLAW_GPIO,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .channel    = LEDC_CHANNEL_1,
    .intr_type  = LEDC_INTR_DISABLE,
    .timer_sel  = LEDC_TIMER_1,
    .duty       = 0,
    .hpoint     = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&claw));

  // Radar‐servo (canal 2)
  ledc_channel_config_t radar = {
    .gpio_num   = SERVO_RADAR_GPIO,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .channel    = LEDC_CHANNEL_2,
    .intr_type  = LEDC_INTR_DISABLE,
    .timer_sel  = LEDC_TIMER_1,
    .duty       = 0,
    .hpoint     = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&radar));

  // 2) Configure stepper
  gpio_set_direction(STEPPER_STEP_GPIO, GPIO_MODE_OUTPUT);
  gpio_set_direction(STEPPER_DIR_GPIO,  GPIO_MODE_OUTPUT);

  // Precompute extremes
  const uint32_t max_duty    = (1u << 16) - 1;
  const uint32_t duty_closed = (SERVO_MIN_US * max_duty) / 20000;
  const uint32_t duty_open   = (SERVO_MAX_US * max_duty) / 20000;

  // Sweep variables for radar
  static uint32_t pos = SERVO_MIN_US;
  static int      dir = 1;
  uint32_t last_swept_duty = 0;

  // Timing for claw‐servo “clap”
  static TickType_t last_clap = 0;
  TickType_t now;

  TickType_t last = xTaskGetTickCount();
  const TickType_t period = pdMS_TO_TICKS(10);

  for (;;) {
    // snapshot estado
    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    odradek_state_t st = ctx.current_state;
    float dist = ctx.front_distance_cm;
    xSemaphoreGive(ctx.ctx_mutex);

    // RADAR-SERVO
    switch (st) {
      case STATE_EV_FAR:
        // barrido
        pos += dir * 5;
        if (pos >= SERVO_MAX_US || pos <= SERVO_MIN_US) {
          dir = -dir;
          pos += dir * 5;
        }
        last_swept_duty = (pos * max_duty) / 20000;
        ledc_set_duty(radar.speed_mode, radar.channel, last_swept_duty);
        break;

      case STATE_EV_CLOSE:
        // fija en última posición de barrido
        ledc_set_duty(radar.speed_mode, radar.channel, last_swept_duty);
        break;

      case STATE_BB_STRESSED:
        // malfunción: barrido rápido
        {
          static bool sd = true;
          static uint32_t sp = SERVO_MIN_US;
          sp += sd ? 20 : -20;
          if (sp >= SERVO_MAX_US || sp <= SERVO_MIN_US) sd = !sd;
          uint32_t du = (sp * max_duty) / 20000;
          ledc_set_duty(radar.speed_mode, radar.channel, du);
        }
        break;

      case STATE_CONNECTED_IDLE:
        // **INMÓVIL** en posición central
        {
          const uint32_t center_us   = (SERVO_MIN_US + SERVO_MAX_US) / 2;
          const uint32_t center_duty = (center_us * max_duty) / 20000;
          ledc_set_duty(radar.speed_mode, radar.channel, center_duty);
        }
        break;

      default:
        // EV_NEAR_CAUTIOUS o EV_NEAR_ALERT: fijo proporcional
        {
          float f = 1.0f - (dist / TH_DIST_FAR);
          f = f < 0 ? 0 : (f > 1 ? 1 : f);
          uint32_t us = SERVO_MIN_US + (uint32_t)((SERVO_MAX_US - SERVO_MIN_US) * f);
          uint32_t du = (us * max_duty) / 20000;
          ledc_set_duty(radar.speed_mode, radar.channel, du);
        }
        break;
    }
    ledc_update_duty(radar.speed_mode, radar.channel);


    // 3) Claw‐servo “clap” con temporizador
    now = xTaskGetTickCount();
    TickType_t clap_interval = 0;

    switch (st) {
      case STATE_EV_FAR:           clap_interval = pdMS_TO_TICKS(1000); break; // 1 Hz
      case STATE_EV_CLOSE:         clap_interval = pdMS_TO_TICKS(500);  break; // 2 Hz
      case STATE_BB_STRESSED:      clap_interval = pdMS_TO_TICKS(200);  break; // 5 Hz
      default:                     clap_interval = 0;                   break;
    }

    static bool claw_open = false;
    if (clap_interval > 0) {
      if (now - last_clap >= clap_interval) {
        claw_open = !claw_open;
        last_clap = now;
      }
      ledc_set_duty(claw.speed_mode, claw.channel,
                    claw_open ? duty_open : duty_closed);
    } else {
      // en otros estados, cerrada
      ledc_set_duty(claw.speed_mode, claw.channel, duty_closed);
    }
    ledc_update_duty(claw.speed_mode, claw.channel);

    // 4) Stepper solo en ALERT
    if (st == STATE_EV_NEAR_ALERT || st == STATE_EV_NEAR_CAUTIOUS) {
      gpio_set_level(STEPPER_DIR_GPIO, 0);
      gpio_set_level(STEPPER_STEP_GPIO, 1);
      esp_rom_delay_us(500);
      gpio_set_level(STEPPER_STEP_GPIO, 0);
    }

    // 5) Esperar siguiente ciclo
    vTaskDelayUntil(&last, period);
  }
}



//==============================================================================
// Wi-Fi Event Handler
//  - Connect on start
//  - Reconnect on disconnect
//  - Start MQTT when IP acquired
//==============================================================================
static void wifi_event_handler(void* arg,
                               esp_event_base_t event_base,
                               int32_t event_id,
                               void* event_data)
{
  if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
    esp_wifi_connect();
  }
  else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
    ESP_LOGW(TAG, "Wi-Fi lost, reconnecting");
    esp_wifi_connect();
  }
  else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
    ESP_LOGI(TAG, "Got IP, starting MQTT");
    esp_mqtt_client_start(ctx.mqtt_client);
  }
}

//==============================================================================
// init_wifi: configure station mode, register event handler
//==============================================================================
static void init_wifi(void)
{
  // Create default STA netif
  esp_netif_create_default_wifi_sta();

  // Register Wi-Fi and IP event handlers
  ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT,
                  ESP_EVENT_ANY_ID,
                  &wifi_event_handler,
                  NULL));
  ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT,
                  IP_EVENT_STA_GOT_IP,
                  &wifi_event_handler,
                  NULL));

  // Init Wi-Fi driver
  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
  ESP_ERROR_CHECK(esp_wifi_init(&cfg));

  // Set station configuration
  wifi_config_t wifi_cfg = {
    .sta = {
      .ssid = WIFI_SSID,
      .password = WIFI_PASS,
      .threshold.authmode = WIFI_AUTH_OPEN,
    },
  };
  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
  ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_cfg));

  // Start Wi-Fi
  ESP_ERROR_CHECK(esp_wifi_start());
  ESP_LOGI(TAG, "Wi-Fi initialized");
}

//==============================================================================
// MQTT Event Handler
//  - On CONNECT: subscribe + set event bit
//  - On DISCONNECT/ERROR: clear flags, reconnect
//  - On DATA: handle “receptor”/“stress”
//==============================================================================
static void mqtt_event_handler(void* handler_args,
                               esp_event_base_t base,
                               int32_t event_id,
                               void* event_data)
{
  esp_mqtt_event_handle_t evt = event_data;
  switch (event_id) {

    case MQTT_EVENT_CONNECTED:
      ESP_LOGE(TAG, "MQTT connected");
      esp_mqtt_client_subscribe(ctx.mqtt_client, MQTT_TOPIC_CMD, 1);
      xEventGroupSetBits(ctx.mqtt_event_group, MQTT_CONNECTED_BIT);
      break;

    case MQTT_EVENT_DISCONNECTED:
    case MQTT_EVENT_ERROR:
      ESP_LOGW(TAG, "MQTT disconnected, resetting state");
      // 1) clear the connection flag so FSM sees it
      xEventGroupClearBits(ctx.mqtt_event_group, MQTT_CONNECTED_BIT);
      // 2) reset all context flags
      xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
      ctx.receptor_detected = false;
      ctx.stress_start_us   = 0;
      ctx.current_state     = STATE_DISCONNECTED;   // <-- DISCONNECTED, not IDLE
      xSemaphoreGive(ctx.ctx_mutex);
      // 3) back-off then reconnect
      vTaskDelay(pdMS_TO_TICKS(500));
      esp_mqtt_client_reconnect(ctx.mqtt_client);
      break;

    case MQTT_EVENT_DATA:
      if (strncmp(evt->topic, MQTT_TOPIC_CMD, evt->topic_len) == 0) {

        char cmd[MAX_CMD_LEN] = {0};
        int len = evt->data_len < MAX_CMD_LEN - 1 ? evt->data_len : MAX_CMD_LEN - 1;
        memcpy(cmd, evt->data, len);

        int64_t now = esp_timer_get_time();

        xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
        if (strcmp(cmd, "receptor") == 0) {
          ctx.last_receptor_us = now;
        } else if (strcmp(cmd, "stress") == 0) {
          // Enter BB_STRESSED and refresh stress
          ctx.current_state    = STATE_BB_STRESSED;
          ctx.last_stress_us   = now;
          ctx.last_receptor_us = 0;
        }
        xSemaphoreGive(ctx.ctx_mutex);
      }
      break;

    default:
      break;
  }
}

//==============================================================================
// init_mqtt: configure and create the MQTT client
//==============================================================================
static void init_mqtt(void)
{
  esp_mqtt_client_config_t cfg = {
    .broker = {
      .address = {
        .uri = MQTT_BROKER_URI,
      }
    },
    .credentials = {
      .username = MQTT_USER,
      .authentication.password = MQTT_PASSWORD
    },
    .network.reconnect_timeout_ms = 5000,
  };

  ctx.mqtt_client = esp_mqtt_client_init(&cfg);
  esp_mqtt_client_register_event(ctx.mqtt_client,
                                 MQTT_EVENT_ANY,
                                 &mqtt_event_handler,
                                 NULL);

}

//==============================================================================
// task_mqtt_publisher: publish JSON state every 5 seconds
//==============================================================================
static void task_mqtt_publisher(void* pv)
{
  // Wait until connected
  xEventGroupWaitBits(ctx.mqtt_event_group,
                      MQTT_CONNECTED_BIT,
                      pdFALSE, pdTRUE,
                      portMAX_DELAY);

  char msg[128];
  for (;;) {
    // Snapshot under mutex
    xSemaphoreTake(ctx.ctx_mutex, portMAX_DELAY);
    int   st   = ctx.current_state;
    float df   = ctx.front_distance_cm;
    float db   = ctx.bottom_distance_cm;
    int   cr   = ctx.crouched ? 1 : 0;
    int   nl   = ctx.noise_level;
    xSemaphoreGive(ctx.ctx_mutex);

    int len = snprintf(msg, sizeof(msg),
                       "{\"state\":%d,"
                       "\"dist_front\":%.1f,"
                       "\"dist_bot\":%.1f,"
                       "\"crouch\":%d,"
                       "\"noise\":%d}",
                       st, df, db, cr, nl);

    esp_mqtt_client_publish(ctx.mqtt_client,
                            MQTT_TOPIC_STATE,
                            msg, len, 1, 0);
    vTaskDelay(pdMS_TO_TICKS(5000));
  }
}


//==============================================================================
// APP ENTRY POINT
//==============================================================================
//==============================================================================
// app_main
//==============================================================================
void app_main(void) {
  // 0) Log config setup
  esp_log_level_set("*", ESP_LOG_VERBOSE);

  //ESP_LOGE(TAG, "Este es un ERROR (siempre visible)");
  //ESP_LOGW(TAG, "Este es un WARNING (debería verse)");
  //ESP_LOGI(TAG, "Este es un mensaje INFORMATIVO");
  //ESP_LOGD(TAG, "Este es DEBUG (solo con nivel Verbose)");
  //ESP_LOGV(TAG, "Este es VERBOSE (máximo detalle)");

  // 1) init NVS, netif, event loop
  esp_err_t ret = nvs_flash_init();
  if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
    ESP_ERROR_CHECK(nvs_flash_erase());
    ret = nvs_flash_init();
  }
  ESP_ERROR_CHECK(ret);
  ESP_ERROR_CHECK(esp_netif_init());
  ESP_ERROR_CHECK(esp_event_loop_create_default());

  // 2) init context & primitives
  begin_monitor();

  // 3) init comms
  init_wifi();
  init_mqtt();

  // 4) spawn tasks
  xTaskCreate(task_noise_sensor,      "Noise",     4096, NULL, 4, NULL);
  xTaskCreate(task_distance_front,   "DistFront", 4096, NULL, 4, NULL);
  xTaskCreate(task_distance_bottom,  "DistBottom", 4096, NULL, 4, NULL);

  xTaskCreate(task_state_manager,    "FSM",       4096, NULL, 3, NULL);

  xTaskCreate(task_acoustic_alert,   "Audio",     4096, NULL, 2, NULL);
  xTaskCreate(task_visual_feedback,  "Visual",    8192, NULL, 5, NULL);
  xTaskCreate(task_mechanism_control, "Mechanism", 8192, NULL, 2, NULL);

  xTaskCreate(task_mqtt_publisher,   "MQTTPub",   4096, NULL, 1, NULL);

  ESP_LOGI(TAG, "All tasks started successfully");
}