#include "esp_camera.h"
#include <WiFi.h>
#include <WiFiUdp.h>
#include <ESP32Servo.h>

// === НАСТРОЙКА РЕЖИМА ЗАГЛУШКИ ===
const bool USE_MOCK_IF_NO_CAMERA = true; // true - слать черный экран, false - ничего не слать

Servo myServo;
const int servoPin = 13; // ПРИОРИТЕТ: Пин жестко закреплен за сервоприводом

// !!! УНИКАЛЬНЫЙ ID ДЛЯ КАЖДОЙ ПЛАТЫ
const uint8_t DEVICE_ID = 1;

// Конфигурация пинов (пример для Seeed Studio XIAO ESP32S3 Sense)
#define PWDN_GPIO_NUM     -1
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM     10
#define SIOD_GPIO_NUM     40
#define SIOC_GPIO_NUM     39
#define Y9_GPIO_NUM       48
#define Y8_GPIO_NUM       47
#define Y7_GPIO_NUM       18 // ИЗМЕНЕНО: Отключаем этот пин для камеры, отдаем под Серво
#define Y6_GPIO_NUM       14
#define Y5_GPIO_NUM       12
#define Y4_GPIO_NUM       11
#define Y3_GPIO_NUM       17
#define Y2_GPIO_NUM       16
#define VSYNC_GPIO_NUM    41
#define HREF_GPIO_NUM     38
#define PCLK_GPIO_NUM     37

const char* ssid = "RT-WiFi-1712";
const char* password = "yt3xihY2Ci";

const char* serverIP = "192.168.0.10"; 
const int serverPort = 9999;       
const int localPort = 8888;        



WiFiUDP udpVideo; 
WiFiUDP udpCmd;   
camera_config_t config;

// Флаг успешной инициализации камеры
bool isCameraInitialized = false;

// Моковый минимальный валидный JPEG черного цвета 
const uint8_t mockBlackJpeg[] = {
  0xFF, 0xD8, 0xFF, 0xE0, 0x00, 0x10, 0x4A, 0x46, 0x49, 0x46, 0x00, 0x01, 0x01, 0x01, 0x00, 0x60,
  0x00, 0x60, 0x00, 0x00, 0xFF, 0xDB, 0x00, 0x43, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0x00, 0x0B, 0x08, 0x00, 0x08,
  0x00, 0x08, 0x01, 0x01, 0x11, 0x00, 0xFF, 0xC4, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0xFF, 0xC4, 0x00, 0x14,
  0x10, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0xFF, 0xDA, 0x00, 0x08, 0x01, 0x01, 0x00, 0x00, 0x3F, 0x00, 0x00, 0x3F, 0xFF, 0xD9
};
const size_t mockSize = sizeof(mockBlackJpeg);

void setup() {
  Serial.begin(115200);
  
  // Инициализируем сервопривод в первую очередь
  myServo.attach(servoPin, 500, 2500); 
  Serial.println("Сервопривод инициализирован на пине 13");

  // Настройка конфигурации камеры
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM; // Передаст -1, библиотека не тронет 13 пин
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.frame_size = FRAMESIZE_QVGA; 
  config.pixel_format = PIXFORMAT_JPEG;
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_count = 2;
  
  // Проверяем статус инициализации камеры
  esp_err_t err = esp_camera_init(&config);
  if (err == ESP_OK) {
    isCameraInitialized = true;
    Serial.println("Камера успешно инициализирована!");
  } else {
    isCameraInitialized = false;
    Serial.printf("Камера не найдена (код 0x%x). Режим мок-данных: %s\n", err, USE_MOCK_IF_NO_CAMERA ? "ВКЛ" : "ВЫКЛ");
  }

  // Подключение к Wi-Fi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  Serial.println("Wi-Fi подключен!");

  // Запуск прослушивания порта для команд
  udpCmd.begin(localPort);
  Serial.print("Ожидание команд на порту: ");
  Serial.println(localPort);
}

void checkIncomingCommands() {
  int packetSize = udpCmd.parsePacket();
  if (packetSize == 5) {
    uint8_t packetBuffer[5];
    int len = udpCmd.read(packetBuffer, 5);
    if (len == 5) {
      uint8_t steer = packetBuffer[0];
      uint8_t throttle = packetBuffer[1];
      uint8_t brake = packetBuffer[2];
      uint8_t gear = packetBuffer[3];
      uint8_t flags = packetBuffer[4];

      // ИСПРАВЛЕНО: Инвертирован диапазон (180, 0 вместо 0, 180) для исправления направления
      int servoAngle = map(steer, 0, 200, 180, 0);
      myServo.write(servoAngle);

      Serial.printf("Got control packet: Steer=%d (Angle=%d), Throttle=%d%%, Brake=%d%%, Gear=%d, Flags=0x%02X\n",
                    steer, servoAngle, throttle, brake, gear, flags);
    }
  } else if (packetSize > 0) {
    char garbage[255];
    udpCmd.read(garbage, sizeof(garbage));
  }
}

void loop() {
  checkIncomingCommands();

  uint8_t * buf = nullptr;
  size_t bufferSize = 0;
  camera_fb_t * fb = nullptr;

  // Пытаемся получить кадр, если камера запущена
  if (isCameraInitialized) {
    fb = esp_camera_fb_get();
    if (fb) {
      bufferSize = fb->len;
      buf = fb->buf;
    }
  }

  // Если кадра с камеры нет
  if (!buf) {
    // Если отправка заглушки отключена — выходим из цикла, продолжая слушать команды
    if (!USE_MOCK_IF_NO_CAMERA) {
      delay(10); 
      return;
    }
    // Если включена — подставляем моковый черный кадр
    bufferSize = mockSize;
    buf = (uint8_t *)mockBlackJpeg;
  }
  
  // Нарезка и отправка по UDP (оригинальная логика)
  size_t chunkSize = 1023; 
  uint8_t packetBuffer[1024];

  for (size_t i = 0; i < bufferSize; i += chunkSize) {
    size_t sendSize = chunkSize;
    if (i + chunkSize > bufferSize) {
      sendSize = bufferSize - i;
    }
    
    packetBuffer[0] = DEVICE_ID;
    memcpy(packetBuffer + 1, buf + i, sendSize);
    
    udpVideo.beginPacket(serverIP, serverPort);
    udpVideo.write(packetBuffer, sendSize + 1); 
    udpVideo.endPacket();
  }

  // Возвращаем буфер только если он реальный
  if (fb) {
    esp_camera_fb_return(fb);
  }

  delay(10); 
}