---

#include <Wire.h>
#include <Servo.h>
#include <SoftwareSerial.h>

// =====================================================
// ПИНЫ
// =====================================================
const int RIGHT_DIR = 2;
const int RIGHT_PWM = 5;
const int LEFT_DIR  = 4;
const int LEFT_PWM  = 6;

const int SERVO_PIN = A0;

const int TF_RX_PIN = 10;   // Arduino RX <- TF-Luna TX
const int TF_TX_PIN = 9;    // Arduino TX -> TF-Luna RX

const int REED_PIN = A3;    // геркон
const int HALL_PIN = 13;    // холл D0

// =====================================================
// НАСТРОЙКИ
// =====================================================
const bool HALL_ACTIVE_LOW = true;   // если датчик инверсный — true

// Скорости. Подстрой под свою машинку.
int rightForwardSpeed  = 155;
int leftForwardSpeed   = 118;

int rightBackwardSpeed = 150;
int leftBackwardSpeed  = 112;

// Поворот. Если левое колесо слабее — левый PWM можно выше
int rightTurnSpeed = 168;
int leftTurnSpeed  = 188;

// Коррекция курса на прямых
float KpYaw = 2.2;
const int MAX_YAW_CORR = 28;

// Защита от препятствия
const int OBSTACLE_STOP_CM = 18;

// Лидар
const unsigned long SERVO_INTERVAL_MS = 38;
const int SERVO_MIN_ANGLE = 10;
const int SERVO_MAX_ANGLE = 170;
const int SERVO_STEP = 4;

// Маршрут
const int MAX_ROUTE_STEPS = 120;
const int MIN_MOVE_MS = 120;
const int MIN_TURN_DEG = 8;

// Сектора
const int MAX_SECTORS = 8;
const int SECTOR_BINS_HALF = 6;
const int SECTOR_BINS_TOTAL = 12;

// =====================================================
// СОСТОЯНИЕ
// =====================================================
enum MotionState { STOPPED, FORWARD, BACKWARD, LEFT, RIGHT };
MotionState currentMotion = STOPPED;

enum ModeState { MODE_MANUAL = 'M', MODE_RECORD = 'V', MODE_REPLAY = 'P' };
char currentMode = MODE_MANUAL;

struct RouteStep {
  char cmd;        // F B L R
  uint16_t value;  // для F/B = время, для L/R = угол
};

RouteStep route[MAX_ROUTE_STEPS];
int routeCount = 0;

uint8_t sectorSaved[MAX_SECTORS];
uint8_t sectorAtStep[MAX_SECTORS];
uint8_t sectorSig[MAX_SECTORS][SECTOR_BINS_TOTAL];

int currentSector = 1;

char activeRecordCmd = 0;
unsigned long activeRecordStartMs = 0;
float activeRecordStartYaw = 0.0;

bool replayAbort = false;

// Датчики
int reedDetected = 0;
int hallDetected = 0;
int magnetDetected = 0;
int frontDistanceCm = 255;

// =====================================================
// MPU6050 / MPU6500 (минимальный raw-драйвер)
// =====================================================
const uint8_t MPU_ADDR = 0x68;
float yawDeg = 0.0;
float gyroZBias = 0.0;
unsigned long lastImuMicros = 0;

// =====================================================
// ЛИДАР + СЕРВО
// =====================================================
SoftwareSerial tfSerial(TF_RX_PIN, TF_TX_PIN);
Servo scanServo;

int servoAngle = 90;
int servoDir = 1;
unsigned long lastServoMoveMs = 0;
bool scanPaused = false;

// =====================================================
// СТАТУС В ESP32
// =====================================================
unsigned long lastStatusMs = 0;
const unsigned long STATUS_INTERVAL_MS = 220;

// =====================================================
// ВСПОМОГАТЕЛЬНОЕ
// =====================================================
float normAngle180(float a) {
  while (a > 180.0) a -= 360.0;
  while (a < -180.0) a += 360.0;
  return a;
}

int clampPWM(int v) {
  if (v < 0) return 0;
  if (v > 255) return 255;
  return v;
}

void sendEvent(const char* msg) {
  Serial.print("@EVENT ");
  Serial.println(msg);
}

void sendEventSector(const char* msg, int sector) {
  Serial.print("@EVENT ");
  Serial.print(msg);
  Serial.print(" sector=");
  Serial.println(sector);
}

void sendScanPoint(int angle, int dist) {
  Serial.print("@SCAN a=");
  Serial.print(angle);
  Serial.print(" d=");
  Serial.println(dist);
}

void sendStatus() {
  Serial.print("@STATUS mode=");
  Serial.print(currentMode);
  Serial.print(" motion=");

  char m = 'S';
  if (currentMotion == FORWARD)  m = 'F';
  if (currentMotion == BACKWARD) m = 'B';
  if (currentMotion == LEFT)     m = 'L';
  if (currentMotion == RIGHT)    m = 'R';

  Serial.print(m);
  Serial.print(" sector=");
  Serial.print(currentSector);
  Serial.print(" reed=");
  Serial.print(reedDetected);
  Serial.print(" hall=");
  Serial.print(hallDetected);
  Serial.print(" mag=");
  Serial.print(magnetDetected);
  Serial.print(" dist=");
  Serial.println(frontDistanceCm);
}

// =====================================================
// МОТОРЫ
// =====================================================
// ВАЖНО: оставил твою рабочую логику направления
void rightForward(int spd) {
  digitalWrite(RIGHT_DIR, HIGH);
  analogWrite(RIGHT_PWM, clampPWM(spd));
}

void rightBackward(int spd) {
  digitalWrite(RIGHT_DIR, LOW);
  analogWrite(RIGHT_PWM, clampPWM(spd));
}

void leftForward(int spd) {
  digitalWrite(LEFT_DIR, LOW);
  analogWrite(LEFT_PWM, clampPWM(spd));
}

void leftBackward(int spd) {
  digitalWrite(LEFT_DIR, HIGH);
  analogWrite(LEFT_PWM, clampPWM(spd));
}

void softStop() {
  analogWrite(RIGHT_PWM, 0);
  analogWrite(LEFT_PWM, 0);
  digitalWrite(RIGHT_DIR, LOW);
  digitalWrite(LEFT_DIR, LOW);
  currentMotion = STOPPED;
}

void applyForwardBase() {
  rightBackward(rightForwardSpeed);
  leftBackward(leftForwardSpeed);
  currentMotion = FORWARD;
}

void applyBackwardBase() {
  rightForward(rightBackwardSpeed);
  leftForward(leftBackwardSpeed);
  currentMotion = BACKWARD;
}

void applyLeftBase() {
  rightForward(rightTurnSpeed);
  leftBackward(leftTurnSpeed);
  currentMotion = LEFT;
}

void applyRightBase() {
  rightBackward(rightTurnSpeed);
  leftForward(leftTurnSpeed);
  currentMotion = RIGHT;
}

void applyForwardHoldYaw(float targetYaw) {
  float err = normAngle180(targetYaw - yawDeg);
  int corr = (int)(KpYaw * err);
  if (corr > MAX_YAW_CORR) corr = MAX_YAW_CORR;
  if (corr < -MAX_YAW_CORR) corr = -MAX_YAW_CORR;

  int r = rightForwardSpeed - corr;
  int l = leftForwardSpeed + corr;

  rightBackward(r);
  leftBackward(l);
  currentMotion = FORWARD;
}

void applyBackwardHoldYaw(float targetYaw) {
  float err = normAngle180(targetYaw - yawDeg);
  int corr = (int)(KpYaw * err);
  if (corr > MAX_YAW_CORR) corr = MAX_YAW_CORR;
  if (corr < -MAX_YAW_CORR) corr = -MAX_YAW_CORR;

  int r = rightBackwardSpeed + corr;
  int l = leftBackwardSpeed - corr;

  rightForward(r);
  leftForward(l);
  currentMotion = BACKWARD;
}

// =====================================================
// MPU
// =====================================================
void mpuWriteByte(uint8_t reg, uint8_t data) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

int16_t mpuReadInt16(uint8_t reg) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom((int)MPU_ADDR, 2);
  int16_t hi = Wire.read();
  int16_t lo = Wire.read();
  return (hi << 8) | lo;
}

void initMPU() {
  delay(100);
  mpuWriteByte(0x6B, 0x00); // wake up
  delay(50);
  mpuWriteByte(0x1A, 0x03); // DLPF
  mpuWriteByte(0x1B, 0x08); // gyro ±500 dps
  delay(50);
}

float readGyroZ_dps() {
  int16_t gzRaw = mpuReadInt16(0x47);
  return ((float)gzRaw) / 65.5; // sensitivity for ±500 dps
}

void calibrateGyro() {
  const int N = 500;
  float sum = 0.0;
  for (int i = 0; i < N; i++) {
    sum += readGyroZ_dps();
    delay(3);
  }
  gyroZBias = sum / N;
}

void updateIMU() {
  unsigned long nowUs = micros();
  if (lastImuMicros == 0) {
    lastImuMicros = nowUs;
    return;
  }

  float dt = (nowUs - lastImuMicros) / 1000000.0;
  lastImuMicros = nowUs;

  float gz = readGyroZ_dps() - gyroZBias;
  if (gz > -0.4 && gz < 0.4) gz = 0.0; // мёртвая зона
  yawDeg += gz * dt;
  yawDeg = normAngle180(yawDeg);
}

// =====================================================
// ДАТЧИКИ МАГНИТА
// =====================================================
int readReedStable() {
  int hits = 0;
  const int samples = 8;
  for (int i = 0; i < samples; i++) {
    if (digitalRead(REED_PIN) == LOW) hits++;
    delay(2);
  }
  return (hits >= 6) ? 1 : 0;
}

void updateMagSensors() {
  reedDetected = readReedStable();

  int rawHall = digitalRead(HALL_PIN);
  if (HALL_ACTIVE_LOW) {
    hallDetected = (rawHall == LOW) ? 1 : 0;
  } else {
    hallDetected = (rawHall == HIGH) ? 1 : 0;
  }

  magnetDetected = (reedDetected || hallDetected) ? 1 : 0;
}

// =====================================================
// TF-LUNA
// =====================================================
bool readTfLunaOnce(int &distCm) {
  while (tfSerial.available() >= 9) {
    if (tfSerial.read() == 0x59) {
      if (tfSerial.read() == 0x59) {
        uint8_t buf[7];
        for (int i = 0; i < 7; i++) {
          while (!tfSerial.available()) {}
          buf[i] = tfSerial.read();
        }

        uint16_t dist = buf[0] | (buf[1] << 8);
        if (dist > 0 && dist < 1200) {
          distCm = (int)dist;
          return true;
        }
      }
    }
  }
  return false;
}

int readTfLunaAverage(int tries, int timeoutMs) {
  unsigned long t0 = millis();
  long sum = 0;
  int count = 0;
  int d = 0;

  while ((millis() - t0) < (unsigned long)timeoutMs && count < tries) {
    if (readTfLunaOnce(d)) {
      if (d > 0 && d < 400) {
        sum += d;
        count++;
      }
    }
  }

  if (count == 0) return 255;
  return (int)(sum / count);
}

// =====================================================
// НЕПРЕРЫВНЫЙ ЛИДАР-СКАН
// =====================================================
void updateContinuousScan() {
  if (scanPaused) return;

  unsigned long now = millis();
  if (now - lastServoMoveMs < SERVO_INTERVAL_MS) return;
  lastServoMoveMs = now;

  servoAngle += servoDir * SERVO_STEP;
  if (servoAngle >= SERVO_MAX_ANGLE) {
    servoAngle = SERVO_MAX_ANGLE;
    servoDir = -1;
  }
  if (servoAngle <= SERVO_MIN_ANGLE) {
    servoAngle = SERVO_MIN_ANGLE;
    servoDir = 1;
  }

  scanServo.write(servoAngle);
  delay(18);

  int d = readTfLunaAverage(1, 8);
  if (d > 0 && d < 255) {
    frontDistanceCm = d;
    sendScanPoint(servoAngle, d);
  }
}

// =====================================================
// ЗАПИСЬ МАРШРУТА
// =====================================================
void clearRouteAll() {
  routeCount = 0;
  activeRecordCmd = 0;
  currentSector = 1;

  for (int i = 0; i < MAX_SECTORS; i++) {
    sectorSaved[i] = 0;
    sectorAtStep[i] = 0;
    for (int j = 0; j < SECTOR_BINS_TOTAL; j++) sectorSig[i][j] = 0;
  }

  sendEvent("ROUTE_ERASED");
}

void pushRouteStep(char cmd, uint16_t value) {
  if (routeCount >= MAX_ROUTE_STEPS) return;
  route[routeCount].cmd = cmd;
  route[routeCount].value = value;
  routeCount++;
}

void startRecordSegment(char cmd) {
  activeRecordCmd = cmd;
  activeRecordStartMs = millis();
  activeRecordStartYaw = yawDeg;
}

void finalizeRecordSegment() {
  if (activeRecordCmd == 0) return;

  if (activeRecordCmd == 'F' || activeRecordCmd == 'B') {
    unsigned long dt = millis() - activeRecordStartMs;
    if (dt >= MIN_MOVE_MS) {
      pushRouteStep(activeRecordCmd, (uint16_t)dt);
    }
  } else if (activeRecordCmd == 'L' || activeRecordCmd == 'R') {
    float dYaw = normAngle180(yawDeg - activeRecordStartYaw);
    int angle = (int)fabs(dYaw);
    if (angle >= MIN_TURN_DEG) {
      pushRouteStep(activeRecordCmd, (uint16_t)angle);
    }
  }

  activeRecordCmd = 0;
}

// =====================================================
// ПАНОРАМА СЕКТОРА
// =====================================================
const int sectorAngles[SECTOR_BINS_HALF] = {15, 45, 75, 105, 135, 165};

void captureHalfSignature(uint8_t *dst, int offset) {
  for (int i = 0; i < SECTOR_BINS_HALF; i++) {
    scanServo.write(sectorAngles[i]);
    delay(140);
    int d = readTfLunaAverage(3, 35);
    if (d < 0) d = 255;
    if (d > 255) d = 255;
    dst[offset + i] = (uint8_t)d;
  }
}

void turnRightByAngle(float angleDeg);
void turnLeftByAngle(float angleDeg);

void capturePanoramaSignature(uint8_t *dst12) {
  scanPaused = true;
  softStop();

  sendEvent("SECTOR_SCAN_START");

  // передняя полусфера
  captureHalfSignature(dst12, 0);

  // поворот корпуса на 180 и задняя полусфера
  turnRightByAngle(180.0);
  delay(100);
  captureHalfSignature(dst12, SECTOR_BINS_HALF);

  // вернуть ориентацию как была
  turnLeftByAngle(180.0);
  delay(60);

  scanServo.write(90);
  scanPaused = false;
}

int compareSectorSignature(uint8_t *a, uint8_t *b) {
  long err = 0;
  for (int i = 0; i < SECTOR_BINS_TOTAL; i++) {
    err += abs((int)a[i] - (int)b[i]);
  }
  return (int)err;
}

void saveCurrentSector() {
  if (currentSector < 1 || currentSector > MAX_SECTORS) return;

  finalizeRecordSegment();

  uint8_t temp[SECTOR_BINS_TOTAL];
  capturePanoramaSignature(temp);

  int idx = currentSector - 1;
  for (int i = 0; i < SECTOR_BINS_TOTAL; i++) sectorSig[idx][i] = temp[i];
  sectorSaved[idx] = 1;
  sectorAtStep[idx] = routeCount;

  sendEventSector("SECTOR_CAPTURED", currentSector);

  if (currentSector < MAX_SECTORS) {
    currentSector++;
    sendEventSector("SECTOR_SET", currentSector);
  }
}

bool checkSectorIfNeeded(int executedSteps) {
  for (int i = 0; i < MAX_SECTORS; i++) {
    if (sectorSaved[i] && sectorAtStep[i] == executedSteps) {
      uint8_t temp[SECTOR_BINS_TOTAL];
      sendEvent("SECTOR_CHECK_START");
      capturePanoramaSignature(temp);

      int err = compareSectorSignature(temp, sectorSig[i]);
      if (err < 240) {
        sendEvent("SECTOR_MATCH");
        return true;
      } else {
        sendEvent("SECTOR_MISMATCH");
        return false;
      }
    }
  }
  return true;
}

// =====================================================
// ФОН / ОБСЛУЖИВАНИЕ
// =====================================================
void serviceBackground() {
  updateIMU();
  updateMagSensors();
  updateContinuousScan();

  if (millis() - lastStatusMs >= STATUS_INTERVAL_MS) {
    lastStatusMs = millis();
    sendStatus();
  }

  if (magnetDetected) {
    sendEvent("MAGNET");
  }
}

bool readStopCommandOnly() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == 'S') return true;
  }
  return false;
}

// =====================================================
// ПОВТОР МАРШРУТА
// =====================================================
void moveForwardReplay(uint16_t ms) {
  float targetYaw = yawDeg;
  unsigned long t0 = millis();

  while ((millis() - t0) < ms) {
    serviceBackground();
    if (readStopCommandOnly()) { replayAbort = true; break; }

    if (frontDistanceCm > 0 && frontDistanceCm < OBSTACLE_STOP_CM) {
      sendEvent("OBSTACLE");
      replayAbort = true;
      break;
    }

    applyForwardHoldYaw(targetYaw);
  }
  softStop();
}

void moveBackwardReplay(uint16_t ms) {
  float targetYaw = yawDeg;
  unsigned long t0 = millis();

  while ((millis() - t0) < ms) {
    serviceBackground();
    if (readStopCommandOnly()) { replayAbort = true; break; }
    applyBackwardHoldYaw(targetYaw);
  }
  softStop();
}

void turnLeftByAngle(float angleDeg) {
  float startYaw = yawDeg;
  while (fabs(normAngle180(yawDeg - startYaw)) < angleDeg) {
    serviceBackground();
    if (currentMode == MODE_REPLAY && readStopCommandOnly()) { replayAbort = true; break; }
    applyLeftBase();
  }
  softStop();
}

void turnRightByAngle(float angleDeg) {
  float startYaw = yawDeg;
  while (fabs(normAngle180(yawDeg - startYaw)) < angleDeg) {
    serviceBackground();
    if (currentMode == MODE_REPLAY && readStopCommandOnly()) { replayAbort = true; break; }
    applyRightBase();
  }
  softStop();
}

void replayRoute() {
  if (routeCount <= 0) return;

  finalizeRecordSegment();
  replayAbort = false;
  currentMode = MODE_REPLAY;
  sendEvent("REPLAY_START");

  for (int i = 0; i < routeCount; i++) {
    if (replayAbort) break;

    char cmd = route[i].cmd;
    uint16_t value = route[i].value;

    if (cmd == 'F') moveForwardReplay(value);
    else if (cmd == 'B') moveBackwardReplay(value);
    else if (cmd == 'L') turnLeftByAngle((float)value);
    else if (cmd == 'R') turnRightByAngle((float)value);

    softStop();
    delay(80);

    if (!checkSectorIfNeeded(i + 1)) {
      replayAbort = true;
      sendEvent("REPLAY_ABORT_SECTOR");
      break;
    }
  }

  softStop();

  if (replayAbort) sendEvent("REPLAY_ABORT_STEP");
  else sendEvent("REPLAY_DONE");

  currentMode = MODE_MANUAL;
}

// =====================================================
// РУЧНОЕ УПРАВЛЕНИЕ / КОМАНДЫ С ESP32
// =====================================================
void handleMotionCommand(char c) {
  if (currentMode == MODE_REPLAY) return;

  if (c == 'F') {
    if (activeRecordCmd != 'F') {
      finalizeRecordSegment();
      if (currentMode == MODE_RECORD) startRecordSegment('F');
    }
    applyForwardBase();
  }
  else if (c == 'B') {
    if (activeRecordCmd != 'B') {
      finalizeRecordSegment();
      if (currentMode == MODE_RECORD) startRecordSegment('B');
    }
    applyBackwardBase();
  }
  else if (c == 'L') {
    if (activeRecordCmd != 'L') {
      finalizeRecordSegment();
      if (currentMode == MODE_RECORD) startRecordSegment('L');
    }
    applyLeftBase();
  }
  else if (c == 'R') {
    if (activeRecordCmd != 'R') {
      finalizeRecordSegment();
      if (currentMode == MODE_RECORD) startRecordSegment('R');
    }
    applyRightBase();
  }
  else if (c == 'S') {
    finalizeRecordSegment();
    softStop();
  }
}

void processCommand(char c) {
  if (c == '\n' || c == '\r') return;

  if (c == 'F' || c == 'B' || c == 'L' || c == 'R' || c == 'S') {
    handleMotionCommand(c);
    return;
  }

  if (c == 'V') {
    if (currentMode != MODE_RECORD) {
      currentMode = MODE_RECORD;
      activeRecordCmd = 0;
      sendEvent("RECORD_ON");
    } else {
      finalizeRecordSegment();
      currentMode = MODE_MANUAL;
      sendEvent("RECORD_OFF");
      sendEvent("ROUTE_SAVED");
    }
    return;
  }

  if (c == 'N') {
    if (currentMode == MODE_RECORD) {
      saveCurrentSector();
    }
    return;
  }

  if (c == 'P') {
    if (currentMode != MODE_REPLAY && routeCount > 0) {
      replayRoute();
    }
    return;
  }

  if (c == 'E') {
    if (currentMode != MODE_REPLAY) {
      softStop();
      currentMode = MODE_MANUAL;
      clearRouteAll();
    }
    return;
  }
}

void readESPCommands() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    processCommand(c);
  }
}

// =====================================================
// SETUP / LOOP
// =====================================================
void setup() {
  Serial.begin(9600);     // связь с ESP32
  Wire.begin();
  tfSerial.begin(115200);

  pinMode(RIGHT_DIR, OUTPUT);
  pinMode(RIGHT_PWM, OUTPUT);
  pinMode(LEFT_DIR, OUTPUT);
  pinMode(LEFT_PWM, OUTPUT);

  pinMode(REED_PIN, INPUT_PULLUP);
  pinMode(HALL_PIN, INPUT);

  scanServo.attach(SERVO_PIN);
  scanServo.write(90);

  softStop();

  initMPU();
  calibrateGyro();

  clearRouteAll();

  sendEvent("SYSTEM_READY");
  sendStatus();
}

void loop() {
  readESPCommands();
  serviceBackground();
}