#include <Arduino.h>
#include <ESP32Servo.h>
#include <DFRobotDFPlayerMini.h>
#include <HardwareSerial.h>
#include "driver/i2s.h"
#include "esp_task_wdt.h"

#include <gerandum-project-1_inferencing.h>  // Edge Impulse модель

// ==== Пины ====
#define SERVO1_PIN      12
#define SERVO2_PIN      13
#define SERVO_WHEEL_L   18
#define SERVO_WHEEL_R   19
#define DF_RX           16
#define DF_TX           17
#define TRIG_PIN         4
#define ECHO_PIN         5
#define I2S_WS          14
#define I2S_SD          32
#define I2S_SCK         15

#define I2S_PORT        I2S_NUM_0
#define SAMPLE_RATE     16000
#define BUFFER_SIZE     EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE

Servo servo1, servo2, servoWheelL, servoWheelR;
HardwareSerial dfSerial(2);
DFRobotDFPlayerMini dfplayer;
float ei_buffer[BUFFER_SIZE];
ei_impulse_result_t result;
bool stop_requested = false;

int get_signal_data(size_t offset, size_t length, float *out_ptr) {
  memcpy(out_ptr, ei_buffer + offset, length * sizeof(float));
  return 0;
}

signal_t ei_signal = {
  .get_data = get_signal_data,
  .total_length = BUFFER_SIZE
};

// Расстояние (HC-SR04)
long measure_distance_cm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  return duration * 0.034 / 2;
}

// I2S микрофон
void i2s_init() {
  i2s_config_t config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_I2S_MSB,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 8,
    .dma_buf_len = 64,
    .use_apll = false,
    .tx_desc_auto_clear = false,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pins = {
    .bck_io_num = I2S_SCK,
    .ws_io_num = I2S_WS,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num = I2S_SD
  };

  i2s_driver_install(I2S_PORT, &config, 0, NULL);
  i2s_set_pin(I2S_PORT, &pins);
  i2s_zero_dma_buffer(I2S_PORT);
}

bool microphone_inference_record() {
  int16_t sample_raw[BUFFER_SIZE];
  size_t bytes_read;
  esp_err_t r = i2s_read(I2S_PORT, sample_raw, sizeof(sample_raw), &bytes_read, portMAX_DELAY);
  if (r != ESP_OK || bytes_read != sizeof(sample_raw)) {
    Serial.println("Ошибка чтения I2S");
    return false;
  }
  for (int i = 0; i < BUFFER_SIZE; i++) {
    ei_buffer[i] = (float)sample_raw[i] / 32768.0f;
  }
  Serial.println("✓ Аудио считано");
  return true;
}

// === Команды ===

void action_stop() {
  Serial.println("→ СТОП");
  stop_requested = true;
  servo1.write(45);
  servo2.write(135);
  servoWheelL.write(90);
  servoWheelR.write(90);
  dfplayer.stop();
}

void action_hello() {
  Serial.println("→ ПРИВЕТ");
  stop_requested = false;
  dfplayer.play(1);
  servo1.write(38);
  for (int i = 30; i <= 60; i++) {
    if (stop_requested) return action_stop();
    esp_task_wdt_reset();  // сброс WDT
    servo2.write(i);
    delay(20);
  }
  delay(200);
  servo1.write(45);
  servo2.write(135);
}

void action_walk() {
  Serial.println("→ ГУЛЯТЬ");
  stop_requested = false;
  long distance = measure_distance_cm();
  esp_task_wdt_reset();
  if (distance < 20) {
    Serial.println("‼ Препятствие!");
    servoWheelL.write(60);
    servoWheelR.write(120);
    delay(800);
    esp_task_wdt_reset();
    servoWheelL.write(90);
    servoWheelR.write(90);
    delay(300);
    return;
  }
  servo1.write(30);
  servoWheelL.write(100);
  delay(1000);
  esp_task_wdt_reset();
  servo1.write(0);
  servoWheelL.write(90);
  delay(300);
  servo2.write(30);
  servoWheelR.write(80);
  delay(1000);
  esp_task_wdt_reset();
  servo2.write(0);
  servoWheelR.write(90);
}

void action_music() {
  Serial.println("→ МУЗЫКА");
  stop_requested = false;
  for (int i = 0; i < 3; i++) {
    if (stop_requested) return action_stop();
    esp_task_wdt_reset();
    servo1.write(60);
    servo2.write(60);
    delay(200);
    servo1.write(45);
    servo2.write(135);
    delay(200);
  }
  dfplayer.loopFolder(1);
}

// === Setup ===

void setup() {
  Serial.begin(115200);
  Serial.println("== НАЧАЛО SETUP ==");

  esp_task_wdt_config_t config = {
    .timeout_ms = 10000,
    .idle_core_mask = (1 << portNUM_PROCESSORS) - 1,
    .trigger_panic = true
  };

  esp_err_t err = esp_task_wdt_init(&config);
  if (err == ESP_ERR_INVALID_STATE) {
    Serial.println("⚠ WDT уже инициализирован — пропускаем");
  } else if (err != ESP_OK) {
    Serial.println("Ошибка инициализации WDT");
    while(1);
  }

  if (esp_task_wdt_add(NULL) != ESP_OK) {
    Serial.println("Ошибка добавления задачи в WDT");
    while(1);
  }

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  dfSerial.begin(9600, SERIAL_8N1, DF_RX, DF_TX);
  if (!dfplayer.begin(dfSerial)) Serial.println("DFPlayer не найден");
  else dfplayer.volume(20);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servoWheelL.attach(SERVO_WHEEL_L);
  servoWheelR.attach(SERVO_WHEEL_R);
  servo1.write(45);
  servo2.write(135);
  servoWheelL.write(90);
  servoWheelR.write(90);
  i2s_init();

  Serial.println("Готов к работе");
}

// === Цикл ===

void loop() {
  esp_task_wdt_reset();  // Сброс в начале цикла

  if (!microphone_inference_record()) return;

  if (run_classifier(&ei_signal, &result, false) != EI_IMPULSE_OK) {
    Serial.println("Ошибка классификатора");
    return;
  }

  for (size_t i = 0; i < EI_CLASSIFIER_LABEL_COUNT; i++) {
    float conf = result.classification[i].value;
    const char* label = result.classification[i].label;
    if (conf > 0.85f) {
      Serial.printf("Команда: %s (%.2f)\n", label, conf);
      if (strcmp(label, "privet") == 0) action_hello();
      else if (strcmp(label, "gulat") == 0) action_walk();
      else if (strcmp(label, "musik") == 0) action_music();
      else if (strcmp(label, "stop") == 0) action_stop();
    }
  }

  delay(100);
}