/*
 * ESP32-CAM Wi-Fi 자율주행 자동차 펌웨어
 *
 * 차량 구조:
 * - 뒷바퀴: DC 모터 1개 (구동 - 전진/후진/속도)
 * - 앞바퀴: 서보모터 1개 (조향 - 좌/우 회전)
 * - 2륜 구동 + Ackermann 조향 방식
 *
 * 기능:
 * - Wi-Fi 연결 (STA 모드)
 * - MJPEG 영상 스트리밍 (/stream)
 * - UDP 제어 명령 수신 (포트 4210)  + want_distance로 HC-SR04 거리값 동봉
 * - DC 모터 1개 PWM 제어 (L298N A채널 사용)
 * - 서보모터 PWM 제어
 * - HC-SR04 거리 측정: loop()에서 200ms 주기로만 갱신, UDP 콜백은 캐시값만 반환
 *
 * 필요 라이브러리:
 * - ESP32 Camera (보드 매니저에 포함)
 * - AsyncUDP
 * - ESP32Servo
 * - ArduinoJson
 */

#include "esp_camera.h"
#include "esp_http_server.h"
#include "esp_timer.h"
#include <WiFi.h>
#include <AsyncUDP.h>
#include <ESP32Servo.h>
#include <ArduinoJson.h>
#include <driver/ledc.h>

// ===== Wi-Fi 설정 =====
// 노트북에서 만든 모바일 핫스팟에 STA 모드로 접속.
// 핫스팟 이름은 단계 2.5의 ESP32 AP와 동일하게 "Auto_Car_01" 권장.
// 핫스팟이 개방형이면 password = "" 그대로, 비번이 있으면 일치시켜 입력.
const char *ssid = "Auto_Car_01";
const char *password = "12345678";  // 노트북 핫스팟 비밀번호

// ===== 핀 설정 =====
// DC 모터 1개 (뒷바퀴 구동) - L298N A채널 사용
const int MOTOR_PWM_PIN = 12;  // L298N ENA (PWM 속도 제어)
const int MOTOR_DIR1_PIN = 13; // L298N IN1 (방향 제어)
const int MOTOR_DIR2_PIN = 15; // L298N IN2 (방향 제어)

// 서보모터 1개 (앞바퀴 조향)
const int SERVO_PIN = 14; // 서보모터 PWM 신호

// HC-SR04 초음파 거리센서
//   TRIG = GPIO 4 / ECHO = GPIO 2 / VCC = 3V3 / GND
//   ※ pinMode 초기화는 setup() 맨 마지막에 — GPIO2 부팅 strapping 영향 회피
//   ※ ECHO 라인에 10kΩ 풀다운 권장 (부팅 시 strap LOW 보장)
const int TRIG_PIN = 4;
const int ECHO_PIN = 2;

// PWM 설정
const int MOTOR_PWM_CHANNEL = 1; // 카메라는 채널 0 사용, 모터는 채널 1 사용
const int MOTOR_PWM_FREQ = 1000;
const int MOTOR_PWM_RESOLUTION = 8;

// ===== UDP 설정 =====
const int UDP_PORT = 4210;
AsyncUDP udp;

// ===== 서보모터 =====
Servo steeringServo;

// ===== HC-SR04 마지막 측정 거리 (cm). 999면 측정 불가/타임아웃. =====
//   loop()에서 주기적 갱신. UDP 콜백은 캐시값만 즉시 반환 (블로킹 없음).
volatile long lastDistCM = 999;
const unsigned long DIST_SAMPLE_MS = 200;

// ===== 카메라 설정 (AI-Thinker ESP32-CAM) =====
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27
#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22

// ===== HTTP Server =====
#define PART_BOUNDARY "123456789000000000000987654321"
static const char *_STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char *_STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char *_STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

httpd_handle_t camera_httpd = NULL;

// ===== 함수 선언 =====
void setupCamera();
void setupWiFi();
void setupMotors();
void setupUDP();
void startCameraServer();
void setMotorSpeed(int speed);
void setSteeringAngle(int angle);
long readDistanceCM();

void setup()
{
  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println("\n\n=== ESP32-CAM 자율주행 자동차 시작 ===");

  // 카메라 초기화
  setupCamera();

  // Wi-Fi 연결
  setupWiFi();

  // 모터 및 서보 초기화
  setupMotors();

  // UDP 설정
  setupUDP();

  // HTTP 서버 시작
  startCameraServer();

  // HC-SR04 초음파 핀 초기화 (의도적으로 마지막에 — GPIO2 부팅 strapping 회피)
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  digitalWrite(TRIG_PIN, LOW);

  Serial.println("\n=== 초기화 완료 ===");
  Serial.printf("스트리밍 URL: http://%s/stream\n", WiFi.localIP().toString().c_str());
  Serial.printf("상태 확인 URL: http://%s/status\n", WiFi.localIP().toString().c_str());
  Serial.printf("카메라 제어 URL: http://%s/control?var=VAR&val=VAL\n", WiFi.localIP().toString().c_str());
  Serial.printf("UDP 제어 포트: %d\n", UDP_PORT);
  Serial.printf("Free Heap: %d bytes\n", esp_get_free_heap_size());
  Serial.println("\n사용 가능한 제어 명령:");
  Serial.println("- UDP: {\"speed\":-255~255, \"steering\":0~180}");
  Serial.println("       {\"speed\":N,\"steering\":N,\"want_distance\":true}");
  Serial.println("- HTTP: /control?var=framesize&val=8 (QVGA)");
  Serial.println("        /control?var=quality&val=10");
  Serial.println("        /control?var=brightness&val=-2~2");
  Serial.println("================================\n");
}

void loop()
{
  // HC-SR04 거리 측정 (200ms 주기). loop 태스크라 pulseIn 블로킹 OK.
  // UDP 콜백은 lastDistCM 캐시만 읽어 즉시 반환.
  static unsigned long lastSampleMs = 0;
  unsigned long now = millis();
  if (now - lastSampleMs >= DIST_SAMPLE_MS)
  {
    lastSampleMs = now;
    readDistanceCM(); // 결과는 lastDistCM에 저장
  }
  delay(10);
}

// ===== 카메라 초기화 =====
void setupCamera()
{
  Serial.println("카메라 초기화 중...");

  camera_config_t config;
  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;
  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_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;

  // PSRAM이 있으면 더 높은 해상도와 품질 사용
  if (psramFound())
  {
    config.frame_size = FRAMESIZE_QVGA; // 320x240
    config.jpeg_quality = 10;
    config.fb_count = 2;
    config.grab_mode = CAMERA_GRAB_LATEST;
    Serial.println("PSRAM 감지됨 - 고품질 모드");
  }
  else
  {
    config.frame_size = FRAMESIZE_QVGA; // 320x240
    config.jpeg_quality = 12;
    config.fb_count = 1;
    config.fb_location = CAMERA_FB_IN_DRAM;
    Serial.println("PSRAM 없음 - 표준 모드");
  }

  // 카메라 초기화
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK)
  {
    Serial.printf("카메라 초기화 실패: 0x%x\n", err);
    while (1)
      delay(1000);
  }

  // 센서 설정 조정
  sensor_t *s = esp_camera_sensor_get();
  if (s != NULL)
  {
    s->set_framesize(s, FRAMESIZE_QVGA); // 320x240
    s->set_brightness(s, 0);             // -2 to 2
    s->set_contrast(s, 0);               // -2 to 2
    s->set_saturation(s, 0);             // -2 to 2
  }

  Serial.println("카메라 초기화 완료 (320x240)");
}

// ===== Wi-Fi 연결 =====
void setupWiFi()
{
  Serial.print("Wi-Fi 연결 중: ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  WiFi.setSleep(false); // Wi-Fi 절전 모드 비활성화

  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 40)
  {
    delay(500);
    Serial.print(".");
    attempts++;
  }

  if (WiFi.status() == WL_CONNECTED)
  {
    Serial.println("\nWi-Fi 연결 성공");
    Serial.print("IP 주소: ");
    Serial.println(WiFi.localIP());
  }
  else
  {
    Serial.println("\nWi-Fi 연결 실패!");
    while (1)
      delay(1000);
  }
}

// ===== 모터 및 서보 초기화 =====
void setupMotors()
{
  Serial.println("모터 초기화 중...");

  // DC 모터 PWM 설정 (ESP-IDF 저수준 API 사용)
  // 카메라가 TIMER_0, CHANNEL_0 사용하므로 TIMER_1, CHANNEL_1 사용
  ledc_timer_config_t ledc_timer = {
      .speed_mode = LEDC_LOW_SPEED_MODE,
      .duty_resolution = (ledc_timer_bit_t)MOTOR_PWM_RESOLUTION,
      .timer_num = LEDC_TIMER_1, // 카메라와 충돌 방지
      .freq_hz = MOTOR_PWM_FREQ,
      .clk_cfg = LEDC_AUTO_CLK};
  ledc_timer_config(&ledc_timer);

  ledc_channel_config_t ledc_channel = {
      .gpio_num = MOTOR_PWM_PIN,
      .speed_mode = LEDC_LOW_SPEED_MODE,
      .channel = (ledc_channel_t)MOTOR_PWM_CHANNEL,
      .intr_type = LEDC_INTR_DISABLE,
      .timer_sel = LEDC_TIMER_1, // 카메라와 충돌 방지
      .duty = 0,
      .hpoint = 0};
  ledc_channel_config(&ledc_channel);

  // DC 모터 방향 핀 설정
  pinMode(MOTOR_DIR1_PIN, OUTPUT);
  pinMode(MOTOR_DIR2_PIN, OUTPUT);

  // 초기 상태: 정지
  digitalWrite(MOTOR_DIR1_PIN, LOW);
  digitalWrite(MOTOR_DIR2_PIN, LOW);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL, 0);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL);

  // 서보모터 설정
  steeringServo.attach(SERVO_PIN);
  steeringServo.write(90); // 중립 위치

  Serial.println("모터 초기화 완료");
}

// ===== UDP 설정 =====
void setupUDP()
{
  Serial.println("UDP 설정 중...");

  if (udp.listen(UDP_PORT))
  {
    Serial.printf("UDP 리스닝 시작: 포트 %d\n", UDP_PORT);

    udp.onPacket([](AsyncUDPPacket packet)
                 {
      // 수신된 데이터를 문자열로 변환
      String data = "";
      for (int i = 0; i < packet.length(); i++) {
        data += (char)packet.data()[i];
      }

      Serial.printf("UDP 수신: %s (from %s:%d)\n",
                    data.c_str(),
                    packet.remoteIP().toString().c_str(),
                    packet.remotePort());

      // JSON 파싱
      StaticJsonDocument<200> doc;
      DeserializationError error = deserializeJson(doc, data);

      if (error) {
        Serial.print("JSON 파싱 오류: ");
        Serial.println(error.c_str());
        packet.printf("{\"status\":\"error\",\"message\":\"JSON parse error\"}");
        return;
      }

      // 패킷 해석: 제어 / 거리 요청 / 둘 다 / 둘 다 아님
      bool has_control = doc.containsKey("speed") && doc.containsKey("steering");
      bool want_dist   = doc["want_distance"] | false;

      // 1) 제어 (있으면 적용)
      int speed = 0, steering = 90;
      if (has_control) {
        speed = doc["speed"];
        steering = doc["steering"];
        setMotorSpeed(speed);
        setSteeringAngle(steering);
      }

      // 2) 거리 — 캐시값만 읽음 (절대 콜백 안에서 측정하지 않음)
      long dist = (long)lastDistCM;

      // 3) 응답 — 처리한 항목만 키로 포함
      if (has_control && want_dist) {
        Serial.printf("제어+거리 - 속도: %d, 조향: %d, 거리: %ld cm\n", speed, steering, dist);
        packet.printf("{\"status\":\"ok\",\"speed\":%d,\"steering\":%d,\"distance\":%ld}",
                      speed, steering, dist);
      } else if (has_control) {
        Serial.printf("제어 - 속도: %d, 조향: %d\n", speed, steering);
        packet.printf("{\"status\":\"ok\",\"speed\":%d,\"steering\":%d}", speed, steering);
      } else if (want_dist) {
        Serial.printf("거리만 - %ld cm\n", dist);
        packet.printf("{\"status\":\"ok\",\"distance\":%ld}", dist);
      } else {
        packet.printf("{\"status\":\"error\",\"message\":\"need speed/steering or want_distance\"}");
      } });
  }
  else
  {
    Serial.println("UDP 리스닝 실패!");
  }
}

// ===== MJPEG 스트리밍 핸들러 (CameraWebServer 예제 기반) =====
static esp_err_t stream_handler(httpd_req_t *req)
{
  camera_fb_t *fb = NULL;
  esp_err_t res = ESP_OK;
  size_t _jpg_buf_len = 0;
  uint8_t *_jpg_buf = NULL;
  char part_buf[64];

  // FPS 카운터
  static int64_t last_frame = 0;
  static int frame_count = 0;
  static int64_t last_fps_print = 0;

  if (!last_frame)
  {
    last_frame = esp_timer_get_time();
    last_fps_print = last_frame;
  }

  res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
  if (res != ESP_OK)
  {
    return res;
  }

  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
  httpd_resp_set_hdr(req, "X-Framerate", "20");

  Serial.println("스트리밍 클라이언트 연결됨");

  while (true)
  {
    fb = esp_camera_fb_get();
    if (!fb)
    {
      Serial.println("프레임 캡처 실패");
      res = ESP_FAIL;
    }
    else
    {
      if (fb->format != PIXFORMAT_JPEG)
      {
        Serial.println("JPEG 포맷 아님");
        esp_camera_fb_return(fb);
        res = ESP_FAIL;
      }
      else
      {
        _jpg_buf_len = fb->len;
        _jpg_buf = fb->buf;
      }
    }

    if (res == ESP_OK)
    {
      res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
    }
    if (res == ESP_OK)
    {
      size_t hlen = snprintf(part_buf, 64, _STREAM_PART, _jpg_buf_len);
      res = httpd_resp_send_chunk(req, part_buf, hlen);
    }
    if (res == ESP_OK)
    {
      res = httpd_resp_send_chunk(req, (const char *)_jpg_buf, _jpg_buf_len);
    }

    if (fb)
    {
      esp_camera_fb_return(fb);
      fb = NULL;
      _jpg_buf = NULL;
    }

    if (res != ESP_OK)
    {
      break;
    }

    // FPS 계산 및 출력
    int64_t frame_time = esp_timer_get_time();
    frame_count++;

    // 5초마다 FPS 출력
    if ((frame_time - last_fps_print) >= 5000000)
    {
      float fps = frame_count / 5.0;
      Serial.printf("스트리밍 FPS: %.1f\n", fps);
      frame_count = 0;
      last_fps_print = frame_time;
    }

    last_frame = frame_time;
  }

  Serial.println("스트리밍 클라이언트 연결 종료");
  return res;
}

// ===== 루트 핸들러 =====
static esp_err_t index_handler(httpd_req_t *req)
{
  httpd_resp_set_type(req, "text/html");
  String html = "<html><body>";
  html += "<h1>ESP32-CAM 자율주행 자동차</h1>";
  html += "<p><a href=\"/stream\">영상 스트리밍</a></p>";
  html += "<p><a href=\"/status\">상태 확인</a></p>";
  html += "<h3>API 엔드포인트:</h3>";
  html += "<ul>";
  html += "<li>GET /stream - MJPEG 스트리밍</li>";
  html += "<li>GET /status - 시스템 상태</li>";
  html += "<li>GET /control?var=framesize&val=8 - 카메라 설정</li>";
  html += "</ul>";
  html += "</body></html>";
  return httpd_resp_send(req, html.c_str(), html.length());
}

// ===== 상태 핸들러 =====
static esp_err_t status_handler(httpd_req_t *req)
{
  httpd_resp_set_type(req, "application/json");

  // 시스템 상태 정보 수집
  size_t free_heap = esp_get_free_heap_size();
  size_t min_free_heap = esp_get_minimum_free_heap_size();

  sensor_t *s = esp_camera_sensor_get();

  String json = "{";
  json += "\"heap\":{\"free\":" + String(free_heap) + ",\"min_free\":" + String(min_free_heap) + "},";
  json += "\"camera\":{";
  json += "\"framesize\":" + String(s->status.framesize) + ",";
  json += "\"quality\":" + String(s->status.quality) + ",";
  json += "\"brightness\":" + String(s->status.brightness) + ",";
  json += "\"contrast\":" + String(s->status.contrast) + ",";
  json += "\"saturation\":" + String(s->status.saturation);
  json += "},";
  json += "\"wifi\":{";
  json += "\"ssid\":\"" + String(ssid) + "\",";
  json += "\"ip\":\"" + WiFi.localIP().toString() + "\",";
  json += "\"rssi\":" + String(WiFi.RSSI());
  json += "}";
  json += "}";

  return httpd_resp_send(req, json.c_str(), json.length());
}

// ===== 카메라 제어 핸들러 =====
static esp_err_t control_handler(httpd_req_t *req)
{
  char *buf = NULL;
  size_t buf_len = 0;
  char variable[32] = {0};
  char value[32] = {0};

  buf_len = httpd_req_get_url_query_len(req) + 1;
  if (buf_len > 1)
  {
    buf = (char *)malloc(buf_len);
    if (httpd_req_get_url_query_str(req, buf, buf_len) == ESP_OK)
    {
      if (httpd_query_key_value(buf, "var", variable, sizeof(variable)) == ESP_OK &&
          httpd_query_key_value(buf, "val", value, sizeof(value)) == ESP_OK)
      {
      }
      else
      {
        free(buf);
        httpd_resp_send_404(req);
        return ESP_FAIL;
      }
    }
    else
    {
      free(buf);
      httpd_resp_send_404(req);
      return ESP_FAIL;
    }
    free(buf);
  }
  else
  {
    httpd_resp_send_404(req);
    return ESP_FAIL;
  }

  int val = atoi(value);
  sensor_t *s = esp_camera_sensor_get();
  int res = 0;

  if (!strcmp(variable, "framesize"))
  {
    if (s->pixformat == PIXFORMAT_JPEG)
      res = s->set_framesize(s, (framesize_t)val);
  }
  else if (!strcmp(variable, "quality"))
    res = s->set_quality(s, val);
  else if (!strcmp(variable, "contrast"))
    res = s->set_contrast(s, val);
  else if (!strcmp(variable, "brightness"))
    res = s->set_brightness(s, val);
  else if (!strcmp(variable, "saturation"))
    res = s->set_saturation(s, val);
  else
  {
    res = -1;
  }

  if (res)
  {
    return httpd_resp_send_500(req);
  }

  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
  return httpd_resp_send(req, NULL, 0);
}

// ===== 카메라 서버 시작 =====
void startCameraServer()
{
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = 80;
  config.max_uri_handlers = 8; // 핸들러 개수 증가

  httpd_uri_t index_uri = {
      .uri = "/",
      .method = HTTP_GET,
      .handler = index_handler,
      .user_ctx = NULL};

  httpd_uri_t stream_uri = {
      .uri = "/stream",
      .method = HTTP_GET,
      .handler = stream_handler,
      .user_ctx = NULL};

  httpd_uri_t status_uri = {
      .uri = "/status",
      .method = HTTP_GET,
      .handler = status_handler,
      .user_ctx = NULL};

  httpd_uri_t control_uri = {
      .uri = "/control",
      .method = HTTP_GET,
      .handler = control_handler,
      .user_ctx = NULL};

  Serial.println("HTTP 서버 시작 중...");
  if (httpd_start(&camera_httpd, &config) == ESP_OK)
  {
    httpd_register_uri_handler(camera_httpd, &index_uri);
    httpd_register_uri_handler(camera_httpd, &stream_uri);
    httpd_register_uri_handler(camera_httpd, &status_uri);
    httpd_register_uri_handler(camera_httpd, &control_uri);
    Serial.println("HTTP 서버 시작됨 (4개 엔드포인트)");
  }
  else
  {
    Serial.println("HTTP 서버 시작 실패!");
  }
}

// ===== DC 모터 속도 제어 (전진/후진 지원) =====
void setMotorSpeed(int speed)
{
  // speed: -255 ~ 255 (0=정지, 양수=전진, 음수=후진)
  speed = constrain(speed, -255, 255);

  if (speed == 0)
  {
    // 정지
    digitalWrite(MOTOR_DIR1_PIN, LOW);
    digitalWrite(MOTOR_DIR2_PIN, LOW);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL, 0);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL);
  }
  else if (speed > 0)
  {
    // 전진
    digitalWrite(MOTOR_DIR1_PIN, HIGH);
    digitalWrite(MOTOR_DIR2_PIN, LOW);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL, speed);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL);
  }
  else
  {
    // 후진
    digitalWrite(MOTOR_DIR1_PIN, LOW);
    digitalWrite(MOTOR_DIR2_PIN, HIGH);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL, -speed);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)MOTOR_PWM_CHANNEL);
  }
}

// ===== 서보모터 조향 제어 =====
void setSteeringAngle(int angle)
{
  // angle: 0-180 (90=중립, <90=좌회전, >90=우회전)
  angle = constrain(angle, 0, 180);
  steeringServo.write(angle);
}

// ===== HC-SR04 거리 측정 (cm) =====
//   - 30ms 타임아웃 → 약 5m 이상은 999(측정불가) 처리
//   - 3회 측정 후 이상치 필터: max-min > 10 이면 양 끝 평균, 아니면 중앙값
//   - 호출자: loop()만. UDP 콜백에서는 절대 호출하지 말 것 (블로킹 방지)
static long readDistanceOnce()
{
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH, 30000UL);
  return (duration == 0) ? 999 : (long)(duration * 0.0343 / 2.0);
}

long readDistanceCM()
{
  const int N = 3;
  long s[N];
  for (int i = 0; i < N; i++)
  {
    s[i] = readDistanceOnce();
    if (i < N - 1)
      delay(10);
  }
  // 정렬(버블)
  for (int i = 0; i < N - 1; i++)
    for (int j = 0; j < N - 1 - i; j++)
      if (s[j] > s[j + 1])
      {
        long t = s[j];
        s[j] = s[j + 1];
        s[j + 1] = t;
      }
  long picked = (s[2] - s[0] > 10) ? (s[0] + s[2]) / 2 : s[1];
  lastDistCM = picked;
  return picked;
}