#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Stepper.h> 
#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h> 

#define TFT_DC 2
#define TFT_CS 5
#define TFT_RST 4
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

Adafruit_MPU6050 mpu;

const int stepsPerRevolution = 200; 
Stepper LeftMotors(stepsPerRevolution, 12, 13);
Stepper RightMotors(stepsPerRevolution, 14, 27);

const int PIR_PIN = 25;

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "test.mosquitto.org"; 
const char* topic_command = "sokheng/robot/cmd"; 

WiFiClient espClient;
PubSubClient client(espClient);

String currentStatus = "MANUAL MODE"; 
String lastStatus = ""; 
int lastMotion = -1;    
String bottomText = "READY TO RECORD"; 
String lastBottomText = "";
int lastPointCount = -1;

// --- ប្រព័ន្ធកត់ត្រាផ្ទៃក្រឡាស្រែ និង រត់អូតូ ២ម៉ែត្រ ---
struct Point {
  float x; 
  float y; 
};

Point boundaryPoints[10]; 
int pointCount = 0;
bool isAutoPlaying = false;
int currentTargetIndex = 0;
Point autoWaypoints[50];  
int autoWaypointCount = 0;

float currentX = 0.0;     
float currentY = 0.0;
float currentHeading = 0.0; 
unsigned long lastUpdateTime = 0;
unsigned long lastReconnectAttempt = 0;

int motorState = 0; // 0: ឈប់, 1: ទៅមុខ, 2: ថយក្រោយ, 3: បត់ឆ្វេង, 4: បត់ស្តាំ
int motorSpeed = 70; 

float mapMinX = 0, mapMaxX = 0, mapMinY = 0, mapMaxY = 0;
float calculatedArea = 0.0; // អថេររក្សាទុកទំហំផ្ទៃដីក្រឡាស្រែ

// រក្សាទុកទីតាំងចាស់របស់ឡាននៅលើអេក្រង់ ដើម្បីលុប និងគូរថ្មីដោយមិនបាច់គូរផែនទីឡើងវិញ
int lastRobotScreenX = -1;
int lastRobotScreenY = -1;
bool needFullMapRedraw = true;

// មុខងារគណនាផ្ទៃក្រឡាដីស្រែ (Shoelace Formula)
float calculatePolygonArea() {
  if (pointCount < 3) return 0.0;
  float area = 0.0;
  int j = pointCount - 1;
  for (int i = 0; i < pointCount; i++) {
    area += (boundaryPoints[j].x + boundaryPoints[i].x) * (boundaryPoints[j].y - boundaryPoints[i].y);
    j = i;
  }
  return abs(area / 2.0);
}

// មុខងារគូររូបរាងដីស្រែចំកណ្តាល និងខ្សែបន្ទាត់រត់នៅនឹងថ្កល់
void drawRadarMap() {
  if (pointCount < 2) {
    if (needFullMapRedraw) {
      tft.fillRect(10, 30, 300, 140, ILI9341_BLACK);
      tft.drawRect(10, 30, 300, 140, ILI9341_DARKGREEN);
      needFullMapRedraw = false;
    }
    return;
  }

  // ស្វែងរកទំហំដីស្រែពិតប្រាកដជានិច្ចដើម្បីកែ Scale ឱ្យចំកណ្តាល
  float minX = boundaryPoints[0].x, maxX = boundaryPoints[0].x;
  float minY = boundaryPoints[0].y, maxY = boundaryPoints[0].y;
  for (int i = 0; i < pointCount; i++) {
    if (boundaryPoints[i].x < minX) minX = boundaryPoints[i].x;
    if (boundaryPoints[i].x > maxX) maxX = boundaryPoints[i].x;
    if (boundaryPoints[i].y < minY) minY = boundaryPoints[i].y;
    if (boundaryPoints[i].y > maxY) maxY = boundaryPoints[i].y;
  }
  
  if (isAutoPlaying && autoWaypointCount > 0) {
    minX = mapMinX; maxX = mapMaxX; minY = mapMinY; maxY = mapMaxY;
  }

  float w = maxX - minX;
  float h = maxY - minY;
  if(w < 1.0) w = 1.0;
  if(h < 1.0) h = 1.0;
  
  // កំណត់ទំហំគូរលើអេក្រង់ក្នុងចន្លោះប្លុកកណ្តាលពីលើអក្សរ (X: 10->310, Y: 30->170)
  float scaleX = 240.0 / w;
  float scaleY = 100.0 / h;
  float scale = (scaleX < scaleY) ? scaleX : scaleY; 

  // គណនា Offset ដើម្បីរំកិលរូបរាងដីស្រែឱ្យមកចំកណ្តាលទ្រូងអេក្រង់ល្មម
  int offsetX = 10 + (300 - (int)(w * scale)) / 2;
  int offsetY = 30 + (140 - (int)(h * scale)) / 2;

  // គូរផែនទីពេញលេញតែម្តងគត់នៅពេលមានការផ្លាស់ប្តូរ Command ធំៗ (ការពារការព្រិចភ្នែក)
  if (needFullMapRedraw) {
    tft.fillRect(10, 30, 300, 140, ILI9341_BLACK);
    tft.drawRect(10, 30, 300, 140, ILI9341_DARKGREEN);

    // 1. គូររូបរាងដីស្រែពិតប្រាកដជារាងពហុកោណ (ពណ៌ស)
    for (int i = 0; i < pointCount; i++) {
      int nextIndex = (i + 1) % pointCount;
      if (!isAutoPlaying && nextIndex == 0) continue; 
      
      int x1 = offsetX + (int)((boundaryPoints[i].x - minX) * scale);
      int y1 = offsetY + (int)((boundaryPoints[i].y - minY) * scale);
      int x2 = offsetX + (int)((boundaryPoints[nextIndex].x - minX) * scale);
      int y2 = offsetY + (int)((boundaryPoints[nextIndex].y - minY) * scale);
      tft.drawLine(x1, y1, x2, y2, ILI9341_WHITE);
    }

    // 2. គូរខ្សែបន្ទាត់ជួររត់ (ពណ៌ខៀវ និង ពណ៌ក្រហម) ឡើងវិញភ្លាមៗពេល Redraw ធំ
    if (isAutoPlaying && autoWaypointCount > 0) {
      for (int i = 0; i < autoWaypointCount - 1; i++) {
        int x1 = offsetX + (int)((autoWaypoints[i].x - minX) * scale);
        int y1 = offsetY + (int)((autoWaypoints[i].y - minY) * scale);
        int x2 = offsetX + (int)((autoWaypoints[i+1].x - minX) * scale);
        int y2 = offsetY + (int)((autoWaypoints[i+1].y - minY) * scale);
        
        uint16_t lineColor = (i < currentTargetIndex) ? ILI9341_RED : ILI9341_BLUE;
        tft.drawLine(x1, y1, x2, y2, lineColor);
      }
    }
    needFullMapRedraw = false;
  }

  // 3. គូរធ្វើបច្ចុប្បន្នភាពទីតាំងឡាន (លុបចំណុចចាស់ គូរចំណុចថ្មី)
  int robotScreenX = offsetX + (int)((currentX - minX) * scale);
  int robotScreenY = offsetY + (int)((currentY - minY) * scale);

  if (robotScreenX != lastRobotScreenX || robotScreenY != lastRobotScreenY) {
    if (lastRobotScreenX >= 10 && lastRobotScreenX <= 310 && lastRobotScreenY >= 30 && lastRobotScreenY <= 170) {
      tft.fillCircle(lastRobotScreenX, lastRobotScreenY, 4, ILI9341_BLACK);
      // កែសម្រួល៖ ឱ្យវាបាញ់ទង់ Redraw នៅ Loop បន្ទាប់ ដើម្បីកុំឱ្យដាច់បន្ទាត់ផ្លូវរត់
      needFullMapRedraw = true; 
    }
    
    if (robotScreenX >= 10 && robotScreenX <= 310 && robotScreenY >= 30 && robotScreenY <= 170) {
      tft.fillCircle(robotScreenX, robotScreenY, 4, ILI9341_YELLOW);
      tft.drawCircle(robotScreenX, robotScreenY, 4, ILI9341_RED);
      lastRobotScreenX = robotScreenX;
      lastRobotScreenY = robotScreenY;
    }
  }
}

void drawBottomText() {
  if (bottomText == lastBottomText) return; 
  lastBottomText = bottomText;

  tft.fillRect(0, 180, 320, 60, ILI9341_MAROON);
  tft.drawRect(0, 180, 320, 60, ILI9341_WHITE);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE); 
  
  int len = bottomText.length();
  int startX = (320 - (len * 12)) / 2;
  if (startX < 5) startX = 5;
  tft.setCursor(startX, 202);
  tft.print(bottomText);
}

void drawHeader() {
  int currentMotion = digitalRead(PIR_PIN);
  if (currentStatus == lastStatus && currentMotion == lastMotion && pointCount == lastPointCount) return; 
  lastStatus = currentStatus;
  lastMotion = currentMotion;
  lastPointCount = pointCount;

  tft.fillRect(0, 0, 320, 25, ILI9341_NAVY); 
  tft.setTextSize(1); 
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(5, 8);
  tft.print("P-COUNT: " + String(pointCount) + " | "); 
  tft.setTextColor(ILI9341_WHITE);
  tft.print(currentStatus);

  tft.setCursor(230, 8);
  if (currentMotion == HIGH) { tft.setTextColor(ILI9341_RED); tft.print("PIR: ALERT!"); } 
  else { tft.setTextColor(ILI9341_GREEN); tft.print("PIR: OK"); }
}

void trackLocation() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  unsigned long now = millis();
  float dt = (now - lastUpdateTime) / 1000.0;
  lastUpdateTime = now;
  if (dt > 0.5) dt = 0.0;

  currentHeading += (g.gyro.z * dt) * 57.2958;

  float speedMPS = 0.0;
  if (motorState == 1) speedMPS = 0.5;
  else if (motorState == 2) speedMPS = -0.5;

  float headingRad = currentHeading * 0.0174533;
  currentX += speedMPS * cos(headingRad) * dt;
  currentY += speedMPS * sin(headingRad) * dt;
}

void generateGridWaypoints() {
  if (pointCount < 3) return; 

  mapMinX = boundaryPoints[0].x; mapMaxX = boundaryPoints[0].x;
  mapMinY = boundaryPoints[0].y; mapMaxY = boundaryPoints[0].y;
  
  for (int i = 1; i < pointCount; i++) {
    if (boundaryPoints[i].x < mapMinX) mapMinX = boundaryPoints[i].x;
    if (boundaryPoints[i].x > mapMaxX) mapMaxX = boundaryPoints[i].x;
    if (boundaryPoints[i].y < mapMinY) mapMinY = boundaryPoints[i].y;
    if (boundaryPoints[i].y > mapMaxY) mapMaxY = boundaryPoints[i].y;
  }

  autoWaypointCount = 0;
  bool directionToggle = true;

  for (float x = mapMinX + 1.0; x < mapMaxX; x += 2.0) { 
    if (directionToggle) {
      autoWaypoints[autoWaypointCount++] = {x, mapMinY + 0.5};
      autoWaypoints[autoWaypointCount++] = {x, mapMaxY - 0.5};
    } else {
      autoWaypoints[autoWaypointCount++] = {x, mapMaxY - 0.5};
      autoWaypoints[autoWaypointCount++] = {x, mapMinY + 0.5};
    }
    directionToggle = !directionToggle; 
    if (autoWaypointCount >= 48) break;
  }
  currentTargetIndex = 0;
}

void updateAutoNavigation() {
  if (!isAutoPlaying || autoWaypointCount == 0) return;

  Point target = autoWaypoints[currentTargetIndex];
  float dx = target.x - currentX;
  float dy = target.y - currentY;
  float distance = sqrt(dx*dx + dy*dy);

  if (distance < 0.4) {
    currentTargetIndex++; 
    needFullMapRedraw = true; // ឱ្យវាគូរដូរពណ៌ខ្សែបន្ទាត់ដែលរត់រួច (ក្រហម)
    if (currentTargetIndex >= autoWaypointCount) {
      isAutoPlaying = false;
      motorState = 0; 
      currentStatus = "AUTO DONE ALL LINES";
      bottomText = "WORK COMPLETED";
      return;
    }
  }

  float targetAngle = atan2(dy, dx) * 57.2958;
  float angleError = targetAngle - currentHeading;

  while (angleError > 180) angleError -= 360;
  while (angleError < -180) angleError += 360;

  if (angleError > 15.0) {
    motorState = 3; 
    motorSpeed = 80;
    currentStatus = "AUTO: TURN LEFT";
  } else if (angleError < -15.0) {
    motorState = 4; 
    motorSpeed = 80;
    currentStatus = "AUTO: TURN RIGHT";
  } else {
    motorState = 1; 
    motorSpeed = 70;
    currentStatus = "RUNNING LINE: " + String(currentTargetIndex/2 + 1);
  }
}

void runMotorsNonBlocking() {
  trackLocation();          
  updateAutoNavigation();   
  
  if (motorState == 0) return; 

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float tiltError = a.acceleration.y; 
  int stabilizationOffset = (int)(tiltError * 5); 

  unsigned long currentMillis = millis();
  unsigned long stepInterval = 20; 
  
  if (motorSpeed == 40)       stepInterval = 45; 
  else if (motorSpeed == 70)  stepInterval = 30; 
  else if (motorSpeed == 80)  stepInterval = 35; 
  else if (motorSpeed == 100) stepInterval = 15; 
  else if (motorSpeed == 150) stepInterval = 5;  

  unsigned long leftInterval = stepInterval;
  unsigned long rightInterval = stepInterval;

  if (motorState == 1 || motorState == 2) {
    leftInterval  = stepInterval + stabilizationOffset;
    rightInterval = stepInterval - stabilizationOffset;
    if (leftInterval < 2)   leftInterval = 2;
    if (rightInterval < 2)  rightInterval = 2;
  }

  LeftMotors.setSpeed(motorSpeed);
  RightMotors.setSpeed(motorSpeed);

  static unsigned long lastLeftMove = 0;
  if (currentMillis - lastLeftMove >= leftInterval) {
    lastLeftMove = currentMillis;
    if (motorState == 1)      LeftMotors.step(1);  
    else if (motorState == 2) LeftMotors.step(-1); 
    else if (motorState == 3) LeftMotors.step(-1); 
    else if (motorState == 4) LeftMotors.step(1);  
  }

  static unsigned long lastRightMove = 0;
  if (currentMillis - lastRightMove >= rightInterval) {
    lastRightMove = currentMillis;
    if (motorState == 1)      RightMotors.step(1);  
    else if (motorState == 2) RightMotors.step(-1); 
    else if (motorState == 3) RightMotors.step(1);  
    else if (motorState == 4) RightMotors.step(-1); 
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  String direction = "";
  for (int i = 0; i < length; i++) { direction += (char)payload[i]; }
  
  Serial.print("ទទួលបានបញ្ជា៖ ");
  Serial.println(direction);

  if (direction == "ទៅមុខ") { 
    isAutoPlaying = false; 
    currentStatus = "MANUAL: FORWARD"; 
    motorSpeed = 70; 
    motorState = 1; 
  }
  else if (direction == "ថយក្រោយ") { 
    isAutoPlaying = false; 
    currentStatus = "MANUAL: BACKWARD"; 
    motorSpeed = 70; 
    motorState = 2; 
  }
  else if (direction == "បត់ឆ្វេង") { 
    isAutoPlaying = false; 
    currentStatus = "MANUAL: LEFT"; 
    motorSpeed = 80; 
    motorState = 3; 
  }
  else if (direction == "បត់ស្តាំ") { 
    isAutoPlaying = false; 
    currentStatus = "MANUAL: RIGHT"; 
    motorSpeed = 80; 
    motorState = 4; 
  }
  else if (direction == "ឈប់") { 
    isAutoPlaying = false; 
    currentStatus = "MANUAL: STOPPED"; 
    motorState = 0; 
  }
  else if (direction == "រក្សាទុក") {
    if (pointCount < 10) {
      boundaryPoints[pointCount] = {currentX, currentY};
      pointCount++;
      
      motorState = 0; 
      isAutoPlaying = false;
      currentStatus = "STOPPED (SAVED)";
      bottomText = "SAVED POINT " + String(pointCount);
      needFullMapRedraw = true; 
    }
  }
  else if (direction == "ចាប់ផ្ដើម") {
    if (pointCount >= 3) {
      calculatedArea = calculatePolygonArea(); 
      generateGridWaypoints();
      isAutoPlaying = true;
      bottomText = "AREA: " + String(calculatedArea, 1) + " m2"; 
      needFullMapRedraw = true; 
    } else {
      bottomText = "NEED 3+ POINTS!";
    }
  }
  else if (direction == "សម្អាត") {
    pointCount = 0;
    autoWaypointCount = 0;
    isAutoPlaying = false;
    currentX = 0; currentY = 0; currentHeading = 0;
    calculatedArea = 0.0;
    currentStatus = "MANUAL MODE";
    bottomText = "DATA CLEARED";
    needFullMapRedraw = true;
  }
}

void tryReconnect() {
  if (!client.connected()) {
    String clientId = "ESP32Client-" + String(random(0, 0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      client.subscribe(topic_command); 
    }
  }
}

void setup() {
  Serial.begin(115200);
  
  // បើកដំណើរការអេក្រង់ TFT
  tft.begin();
  tft.setRotation(3); 
  tft.fillScreen(ILI9341_BLACK); 

  // បើកទាក់ទង I2C ទៅកាន់ MPU6050
  Wire.begin(21, 22);
  if (!mpu.begin()) {
    Serial.println("រកមិនឃើញឈីប MPU6050 ទេ!");
    tft.setCursor(10, 10);
    tft.setTextColor(ILI9341_RED);
    tft.print("MPU6050 Error!");
  }

  // កំណត់ជើងម៉ូទ័រ និងល្បឿនដំបូង
  pinMode(PIR_PIN, INPUT); 
  LeftMotors.setSpeed(motorSpeed);
  RightMotors.setSpeed(motorSpeed);

  // ភ្ជាប់ប្រព័ន្ធ WiFi & MQTT
  WiFi.begin(ssid, password);
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  
  lastUpdateTime = millis();
}

void loop() {
  if (!client.connected()) {
    unsigned long now = millis();
    if (now - lastReconnectAttempt > 5000) { 
      lastReconnectAttempt = now; 
      tryReconnect(); 
    }
  } else { 
    client.loop(); 
  }
  
  runMotorsNonBlocking();
  
  drawRadarMap(); 
  drawBottomText();   
  drawHeader();       
  delay(10); 
}

តើកួដនឹងមានមុខងារអ្វីខ្លះ