#define BLYNK_TEMPLATE_ID "TMPL6j4zgdkix"
#define BLYNK_TEMPLATE_NAME "delivery robot"
#define BLYNK_AUTH_TOKEN "xZ_gSpH3QKTX2fCxYnPS7416bb08A_H-"

#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const int STEP_DELAY = 800;

char ssid[] = "Wokwi-GUEST";
char pass[] = "";

#define VROOM1_BTN   V1
#define VROOM2_BTN   V2
#define VROOM3_BTN   V3
#define VRETURN_BTN  V4

#define VLED_DELIV   V9
#define VLED_INTR    V10
#define VLED_SUCCESS V11
#define VTIMER       V8

const float STEPS_PER_CM = 10.61f;
const int TURN_STEPS = 150;

int room1_main_cm = 100;
int room1_short_cm = 30;
int room2_main_cm = 100;
int room2_short_cm = 30;
int room3_main_cm = 130;

int ROOM1_FWD_STEPS;
int ROOM1_SHORT_STEPS;
int ROOM2_FWD_STEPS;
int ROOM2_SHORT_STEPS;
int ROOM3_FWD_STEPS;

const int stepPins[2] = { 2, 4 };
const int dirPins[2] = { 5, 22 };

const int trigPin = 14;
const int echoPin = 27;
const int buzzerPin = 12;
const long PRESENCE_THRESHOLD_CM = 15;

int currentRoom = 0;
bool returnRequested = false;

int lastMainSteps = 0;
int lastTurnSteps = 0;
int lastShortSteps = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setDirection(bool leftDir, bool rightDir) {
  digitalWrite(dirPins[0], leftDir);
  digitalWrite(dirPins[1], rightDir);
}

void stepBoth(int steps) {
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPins[0], HIGH);
    digitalWrite(stepPins[1], HIGH);
    delayMicroseconds(STEP_DELAY);
    digitalWrite(stepPins[0], LOW);
    digitalWrite(stepPins[1], LOW);
    delayMicroseconds(STEP_DELAY);
  }
}

void moveForwardSteps(int s) { setDirection(HIGH, HIGH); stepBoth(s); }
void moveBackwardSteps(int s) { setDirection(LOW, LOW); stepBoth(s); }
void turnRightInPlace(int s) { setDirection(HIGH, LOW); stepBoth(s); }
void turnLeftInPlace(int s) { setDirection(LOW, HIGH); stepBoth(s); }

void beepBuzzer() {
  tone(buzzerPin, 2000);
  delay(500);
  noTone(buzzerPin);
}

long readUltrasonicCM() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);
  if (duration == 0) return 999;
  return duration * 0.0343 / 2.0;
}

bool waitForItemLoad() {
  unsigned long start = millis();
  while (millis() - start < 3000) {
    if (readUltrasonicCM() < PRESENCE_THRESHOLD_CM) return true;
    delay(100);
    Blynk.run();
  }
  beepBuzzer();
  Blynk.virtualWrite(VLED_INTR, 255);
  return false;
}

bool waitForItemRemoval() {
  unsigned long start = millis();
  while (millis() - start < 60000) {
    if (readUltrasonicCM() >= PRESENCE_THRESHOLD_CM) return true;
    delay(200);
    Blynk.run();
  }
  Blynk.virtualWrite(VLED_INTR, 255);
  return false;
}

bool waitCountdownSeconds(int totalSeconds) {
  unsigned long startMillis = millis();
  for (int sec = totalSeconds; sec >= 0; sec--) {
    if (returnRequested) return false;
    if (readUltrasonicCM() > PRESENCE_THRESHOLD_CM) return true;

    lcd.setCursor(0, 0);
    lcd.printf("Waiting: %2d s", sec);
    Blynk.virtualWrite(VTIMER, sec);
    Blynk.run();

    unsigned long target = startMillis + (unsigned long)((totalSeconds - sec + 1) * 1000UL);
    while (millis() < target) {
      if (returnRequested) return false;
      Blynk.run();
      delay(50);
    }
  }
  return false;
}

void returnToBaseFromRecorded(int room) {
  if (lastShortSteps > 0) moveBackwardSteps(lastShortSteps);
  if (lastTurnSteps > 0) {
    if (room == 1) turnLeftInPlace(lastTurnSteps);
    else if (room == 2) turnRightInPlace(lastTurnSteps);
  }
  if (lastMainSteps > 0) moveBackwardSteps(lastMainSteps);

  Blynk.virtualWrite(VLED_DELIV, 0);
  Blynk.virtualWrite(VLED_SUCCESS, 0);
  Blynk.virtualWrite(VLED_INTR, 0);
  Blynk.virtualWrite(VTIMER, 0);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("   STATUS: IDLE  ");
  lcd.setCursor(0, 1);
  lcd.print("Awaiting cmd...  ");

  currentRoom = 0;
  returnRequested = false;
  lastMainSteps = 0;
  lastTurnSteps = 0;
  lastShortSteps = 0;
}

void deliverToRoom(int room) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.printf("Deliver to R%d...", room);
  lcd.setCursor(0, 1);
  lcd.print("Moving...         ");

  Blynk.virtualWrite(VLED_DELIV, 255);
  Blynk.virtualWrite(VLED_SUCCESS, 0);
  Blynk.virtualWrite(VLED_INTR, 0);
  Blynk.virtualWrite(VTIMER, 60);

  currentRoom = room;
  returnRequested = false;

  lastMainSteps = 0;
  lastTurnSteps = 0;
  lastShortSteps = 0;

  int mainSteps = 0, shortSteps = 0, turnSteps = TURN_STEPS;

  if (room == 1) {
    mainSteps = ROOM1_FWD_STEPS;
    shortSteps = ROOM1_SHORT_STEPS;
  } else if (room == 2) {
    mainSteps = ROOM2_FWD_STEPS;
    shortSteps = ROOM2_SHORT_STEPS;
  } else if (room == 3) {
    mainSteps = ROOM3_FWD_STEPS;
    shortSteps = 0;
  } else return;

  moveForwardSteps(mainSteps); lastMainSteps = mainSteps;
  if (room == 1) turnRightInPlace(turnSteps);
  else if (room == 2) turnLeftInPlace(turnSteps);
  lastTurnSteps = turnSteps;

  if (room == 1 || room == 2) {
    moveForwardSteps(shortSteps);
    lastShortSteps = shortSteps;
  }

  if (!waitForItemLoad() || returnRequested) {
    returnToBaseFromRecorded(room);
    return;
  }

  if (!waitCountdownSeconds(60) || returnRequested) {
    returnToBaseFromRecorded(room);
    return;
  }

  if (!waitForItemRemoval() || returnRequested) {
    returnToBaseFromRecorded(room);
    return;
  }

  beepBuzzer();
  Blynk.virtualWrite(VLED_DELIV, 0);
  Blynk.virtualWrite(VLED_SUCCESS, 255);
  Blynk.virtualWrite(VLED_INTR, 0);
  Blynk.virtualWrite(VTIMER, 0);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Delivery done!  ");
  lcd.setCursor(0, 1);
  lcd.print("Returning...    ");

  returnToBaseFromRecorded(room);
}

void handleReturn() {
  returnRequested = true;
  lcd.setCursor(0, 0);
  lcd.print("Waiting:  0 s     ");
  lcd.setCursor(0, 1);
  lcd.print("Returning now... ");
  Blynk.virtualWrite(VTIMER, 0);
  Blynk.run();
}

BLYNK_WRITE(VROOM1_BTN) {
  if (param.asInt() == 1 && currentRoom == 0) deliverToRoom(1);
}

BLYNK_WRITE(VROOM2_BTN) {
  if (param.asInt() == 1 && currentRoom == 0) deliverToRoom(2);
}

BLYNK_WRITE(VROOM3_BTN) {
  if (param.asInt() == 1 && currentRoom == 0) deliverToRoom(3);
}

BLYNK_WRITE(VRETURN_BTN) {
  if (param.asInt() == 1 && currentRoom != 0) handleReturn();
}

void setup() {
  Serial.begin(115200);
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);

  ROOM1_FWD_STEPS = int(round(room1_main_cm * STEPS_PER_CM));
  ROOM1_SHORT_STEPS = int(round(room1_short_cm * STEPS_PER_CM));
  ROOM2_FWD_STEPS = int(round(room2_main_cm * STEPS_PER_CM));
  ROOM2_SHORT_STEPS = int(round(room2_short_cm * STEPS_PER_CM));
  ROOM3_FWD_STEPS = int(round(room3_main_cm * STEPS_PER_CM));

  for (int i = 0; i < 2; i++) {
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);
  }

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  digitalWrite(buzzerPin, LOW);

  Wire.begin(21, 22);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("  Delivery Bot  ");
  delay(1500);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("   STATUS: IDLE ");
  lcd.setCursor(0, 1);
  lcd.print("Awaiting cmd...");

  Blynk.virtualWrite(VLED_DELIV, 0);
  Blynk.virtualWrite(VLED_SUCCESS, 0);
  Blynk.virtualWrite(VLED_INTR, 0);
  Blynk.virtualWrite(VTIMER, 0);
}

void loop() {
  Blynk.run();
}