#include <WiFi.

h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>
#include <Preferences.h>

#define BLYNK_PRINT Serial

#define RX_PIN 16
#define TX_PIN 17
HardwareSerial SerialUART(2);

char auth[] = "S7jN-YlLo34WOd1xJDQlMiY5-W4bupme";

char ssid[] = "HuThen";
char pass[] = "00000000";

// Khởi tạo Preferences

Preferences preferences;

// Cấu trúc lưu trữ một Stage cho động cơ DC

struct Stage {
float distance; // mm
float speed; // mm/s
};

// Cấu trúc lưu trữ một Option cho động cơ DC

struct Option {
Stage stages[25];
};

// Mảng lưu trữ các Option cho động cơ DC

Option options[6];

// Cấu trúc lưu trữ một Stage cho động cơ pan-tilt

struct PanTiltStage {
float theta1; // Góc pan (độ)
float theta2; // Góc tilt (độ)
float panSpeed; // Tốc độ pan
float tiltSpeed; // Tốc độ tilt
};

// Cấu trúc lưu trữ một Option cho động cơ pan-tilt

struct PanTiltOption {
PanTiltStage stages[25];
};

// Mảng lưu trữ các Option cho động cơ pan-tilt

PanTiltOption panTiltOptions[6];

// Biến toàn cục

int speed = 0;
int currentSpeed = 0;
int dir = 0;
float NewCurPos_global = 0;
bool joystickActive = false;
int currentOption = 1;
int currentStage = 1;
int currentPanTiltStage = 1;
int currentPanTiltOption = 1;
float inputDistance = 0;
float inputSpeed = 0; // Khởi tạo mặc định
int currentStageIndex = 0;
int currentPanTiltStageIndex = 0;
const int stageNames[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
17, 18, 19, 20, 21, 22, 23, 24, 25};
const int optionNames[] = {1, 2, 3, 4, 5, 6};
bool resetPending = false;
unsigned long lastResetTime = 0;
float totalDistance = 0;

// Biến cho động cơ bước (pan-tilt)

int panDir = 0;
int tiltDir = 0;
float panSpeed = 0;
float tiltSpeed = 0;
bool panTiltActive = false;

// Biến cho động học pan-tilt

float theta1 = 0;
float theta2 = 0;
float lastTheta1 = 0;
float lastTheta2 = 0;
const float L1 = 0.10848; // Chiều dài L1 (m)
const float L2 = 0.11348; // Chiều dài L2 (m)
const int STEPS_PER_REVOLUTION = 200;

// Biến tần suất gửi

unsigned long lastTerminalUpdate = 0;
unsigned long lastUpdateTime = 0;
unsigned long lastJoystickSendTime = 0;
const unsigned long JOYSTICK_SEND_INTERVAL = 500; // Gửi mỗi 500ms

// Biến cho kịch bản

float inputTheta1 = 0;
float inputTheta2 = 0;
float inputPanSpeed = 0;
float inputTiltSpeed = 0;
bool panTiltRunning = false;
int currentPanTiltStageStep = 0;
int maxSavedStage = 0;

// Biến chế độ
bool isAutoMode = false;
bool isPaused = false;

// Hàm lưu stage động cơ DC

void saveStageToPreferences(int optionIdx, int stageIdx) {
preferences.begin("motor_control", false);
String keyDistance = "opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_dist";
String keySpeed = "opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_speed";
preferences.putFloat(keyDistance.c_str(),
options[optionIdx].stages[stageIdx].distance);
preferences.putFloat(keySpeed.c_str(),
options[optionIdx].stages[stageIdx].speed);

float savedDistance = preferences.getFloat(keyDistance.c_str(), -1.0);

float savedSpeed = preferences.getFloat(keySpeed.c_str(), -1.0);
if (savedDistance == options[optionIdx].stages[stageIdx].distance &&
 savedSpeed == options[optionIdx].stages[stageIdx].speed) {
Serial.println("Save DC Stage S" + String(stageIdx + 1) + " to Option " +
String(optionIdx + 1) + " successful");
} else {
Serial.println("Save DC Stage S" + String(stageIdx + 1) + " to Option " +
String(optionIdx + 1) + " failed");
}

preferences.end();
if (stageIdx + 1 > maxSavedStage) maxSavedStage = stageIdx + 1;
}

// Hàm lưu stage động cơ pan-tilt

void savePanTiltStageToPreferences(int optionIdx, int stageIdx) {
preferences.begin("pantilt_control", false);
String keyTheta1 = "pt_opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_theta1";
String keyTheta2 = "pt_opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_theta2";
String keyPanSpeed = "pt_opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_panSpeed";
String keyTiltSpeed = "pt_opt" + String(optionIdx) + "_st" + String(stageIdx) +
"_tiltSpeed";
preferences.putFloat(keyTheta1.c_str(),
panTiltOptions[optionIdx].stages[stageIdx].theta1);
preferences.putFloat(keyTheta2.c_str(),
panTiltOptions[optionIdx].stages[stageIdx].theta2);
preferences.putFloat(keyPanSpeed.c_str(),
panTiltOptions[optionIdx].stages[stageIdx].panSpeed);
preferences.putFloat(keyTiltSpeed.c_str(),
panTiltOptions[optionIdx].stages[stageIdx].tiltSpeed);

float savedTheta1 = preferences.getFloat(keyTheta1.c_str(), -1.0);

float savedTheta2 = preferences.getFloat(keyTheta2.c_str(), -1.0);
float savedPanSpeed = preferences.getFloat(keyPanSpeed.c_str(), -1.0);
float savedTiltSpeed = preferences.getFloat(keyTiltSpeed.c_str(), -1.0);
if (savedTheta1 == panTiltOptions[optionIdx].stages[stageIdx].theta1 &&
savedTheta2 == panTiltOptions[optionIdx].stages[stageIdx].theta2 &&
savedPanSpeed == panTiltOptions[optionIdx].stages[stageIdx].panSpeed &&
savedTiltSpeed == panTiltOptions[optionIdx].stages[stageIdx].tiltSpeed) {
Serial.println("Save Pan-Tilt Stage S" + String(stageIdx + 1) + " to Option " +
String(optionIdx + 1) + " successful");
} else {
Serial.println("Save Pan-Tilt Stage S" + String(stageIdx + 1) + " to Option " +
String(optionIdx + 1) + " failed");
}

preferences.end();
if (stageIdx + 1 > maxSavedStage) maxSavedStage = stageIdx + 1;
}

// Hàm đọc options động cơ DC

void loadOptionsFromPreferences() {
preferences.begin("motor_control", true);
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 25; j++) {
String keyDistance = "opt" + String(i) + "_st" + String(j) + "_dist";
String keySpeed = "opt" + String(i) + "_st" + String(j) + "_speed";
options[i].stages[j].distance = preferences.getFloat(keyDistance.c_str(),
0.0);
options[i].stages[j].speed = preferences.getFloat(keySpeed.c_str(), 0.0);
if (options[i].stages[j].distance != 0.0 || options[i].stages[j].speed !=
0.0) {
if (j + 1 > maxSavedStage) maxSavedStage = j + 1;
}
}
}
preferences.end();
Serial.println("Loaded DC options from Preferences");
}

// Hàm đọc options động cơ pan-tilt

void loadPanTiltOptionsFromPreferences() {
preferences.begin("pantilt_control", true);
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 25; j++) {
String keyTheta1 = "pt_opt" + String(i) + "_st" + String(j) + "_theta1";
String keyTheta2 = "pt_opt" + String(i) + "_st" + String(j) + "_theta2";
String keyPanSpeed = "pt_opt" + String(i) + "_st" + String(j) + "_panSpeed";
String keyTiltSpeed = "pt_opt" + String(i) + "_st" + String(j) +
"_tiltSpeed";
panTiltOptions[i].stages[j].theta1 = preferences.getFloat(keyTheta1.c_str(),
0.0);
panTiltOptions[i].stages[j].theta2 = preferences.getFloat(keyTheta2.c_str(),
0.0);
panTiltOptions[i].stages[j].panSpeed =
preferences.getFloat(keyPanSpeed.c_str(), 0.0);
panTiltOptions[i].stages[j].tiltSpeed =
preferences.getFloat(keyTiltSpeed.c_str(), 0.0);
if (panTiltOptions[i].stages[j].theta1 != 0.0 ||
panTiltOptions[i].stages[j].theta2 != 0.0 ||
panTiltOptions[i].stages[j].panSpeed != 0.0 ||
panTiltOptions[i].stages[j].tiltSpeed != 0.0) {
if (j + 1 > maxSavedStage) maxSavedStage = j + 1;
}
}
}
preferences.end();
Serial.println("Loaded Pan-Tilt options from Preferences");
}

// Joystick DC (V0)
BLYNK_WRITE(V0) {
Serial.println("V0 (Joystick DC) - Called, isAutoMode: " + String(isAutoMode));
if (!isAutoMode) {
int x = param[0].asInt();

if (x == -1) {
dir = -1; // Trái: chiều ngược
currentSpeed = constrain(inputSpeed, 0, 11); // Giới hạn tốc độ từ 0-11
joystickActive = true;
} else if (x == 1) {
dir = 1; // Phải: chiều thuận
currentSpeed = constrain(inputSpeed, 0, 11); // Giới hạn tốc độ từ 0-11
joystickActive = true;
} else {
dir = 0; // Buông: dừng
currentSpeed = 0;
 joystickActive = false;
}
Serial.println("V0 (Joystick DC) - X: " + String(x) + ", Dir: " + String(dir) +
", Speed: " + String(currentSpeed) + ", InputSpeed: " +
String(inputSpeed) +
", JoystickActive: " + String(joystickActive));
} else {
Serial.println("V0 (Joystick DC) - Ignored: Auto mode is active");
}
}

// Joystick Pan-Tilt (V3)

BLYNK_WRITE(V3) {
if (!isAutoMode) {
unsigned long currentTime = millis();
int x = param[0].asInt();
int y = param[1].asInt();

// Điều khiển hướng và tốc độ pan

if (x > 20) {
panDir = 0;
panSpeed = map(abs(x), 20, 255, 0, 20); // Tốc độ pan từ 0-20 độ/s
panTiltActive = true;
} else if (x < -20) {
panDir = 1;
panSpeed = map(abs(x), 20, 255, 0, 20);
panTiltActive = true;
} else {
panSpeed = 0;
}

// Điều khiển hướng và tốc độ tilt

if (y > 20) {
tiltDir = 0;
tiltSpeed = map(abs(y), 20, 255, 0, 20); // Tốc độ tilt từ 0-20 độ/s
panTiltActive = true;
} else if (y < -20) {
tiltDir = 1;
tiltSpeed = map(abs(y), 20, 255, 0, 20);
panTiltActive = true;
} else {
tiltSpeed = 0;
}

// Tính toán góc từ joystick

float newTheta1 = (x / 255.0) * 90.0; // Pan: -90 đến 90 độ
float newTheta2 = (y / 255.0) * 90.0; // Tilt: -90 đến 90 độ
float deltaTheta1 = newTheta1 - lastTheta1;
float deltaTheta2 = newTheta2 - lastTheta2;
int steps1 = (deltaTheta1 * STEPS_PER_REVOLUTION) / 360.0;
int steps2 = (deltaTheta2 * STEPS_PER_REVOLUTION) / 360.0;

// Cập nhật góc hiện tại

theta1 = newTheta1;
theta2 = newTheta2;
lastTheta1 = theta1;
lastTheta2 = theta2;

// Tính toán vị trí (x, y, z) dựa trên động học

 float theta1_rad = theta1 * PI / 180.0;
float theta2_rad = theta2 * PI / 180.0;
float xPos = L2 * cos(theta1_rad);
float yPos = L2 * sin(theta1_rad);
float zPos = L1 + L2 * sin(theta2_rad);

// Gửi lệnh tới STM32

if (panTiltActive && (abs(steps1) > 0 || abs(steps2) > 0)) {
String msg = "P:" + String(steps1) + ",T:" + String(steps2) + "\n";
SerialUART.print(msg);
Serial.println("V3 (Joystick Pan-Tilt) - Sent to STM32: " + msg);
}

// Cập nhật terminal

if (panTiltActive && (currentTime - lastTerminalUpdate >= 1000)) {
String terminalMsg = "Pan Speed: " + String(panSpeed) + " deg/s, Tilt Speed:
" + String(tiltSpeed) + " deg/s\n";
Blynk.virtualWrite(V11, terminalMsg);
Serial.println("V3 (Joystick Pan-Tilt) - Terminal V11: " + terminalMsg);
lastTerminalUpdate = currentTime;
}

Serial.println("V3 (Joystick Pan-Tilt) - X: " + String(x) + ", Y: " + String(y)

+
", PanDir: " + String(panDir) + ", PanSpeed: " +
String(panSpeed) +
", TiltDir: " + String(tiltDir) + ", TiltSpeed: " +
String(tiltSpeed) +
", Theta1: " + String(theta1) + ", Theta2: " + String(theta2) +
", Steps1: " + String(steps1) + ", Steps2: " + String(steps2) +
", Position (X, Y, Z): (" + String(xPos) + ", " + String(yPos) +
", " + String(zPos) + ")");
}
}

// Reset Position (V4)

BLYNK_WRITE(V4) {
int btn = param.asInt();
if (btn == 1) {
while (SerialUART.available()) SerialUART.read();
SerialUART.print("RESET_POS\n");
SerialUART.print("NP:0.0\n");
Serial.println("V4 (Reset Position) - Sent RESET_POS and NP:0.0 to STM32,
NewCurPos_global: 0");
NewCurPos_global = 0;
resetPending = true;
lastResetTime = millis();
Blynk.virtualWrite(V4, 0);
}
}

// Menu Option DC (V5)

BLYNK_WRITE(V5) {
int newOption = param.asInt();
if (newOption >= 1 && newOption <= 6) {
currentOption = newOption;
Serial.println("V5 (Menu Option DC) - Selected Option: " +
String(currentOption));
Blynk.virtualWrite(V5, currentOption);
 if (isAutoMode) {
currentPanTiltOption = currentOption;
Blynk.virtualWrite(V13, currentPanTiltOption);
Serial.println("V5 (Menu Option DC) - Synced Pan-Tilt Option to V13: " +
String(currentPanTiltOption));
}
} else {
Serial.println("V5 (Menu Option DC) - Invalid Option: " + String(newOption));
}
}

// Menu Stage DC (V6)

BLYNK_WRITE(V6) {
int newStage = param.asInt();
if (newStage >= 1 && newStage <= 25) {
currentStage = newStage;
currentStageIndex = newStage - 1;
Serial.println("V6 (Menu Stage DC) - Selected Stage: S" +
String(currentStage));
Blynk.virtualWrite(V6, currentStage);
} else {
Serial.println("V6 (Menu Stage DC) - Invalid Stage: " + String(newStage));
}
}

// Distance DC (V7)
BLYNK_WRITE(V7) {
inputDistance = param.asFloat();
Serial.println("V7 (Distance DC) - Input Distance: " + String(inputDistance));
}

// Speed DC (V8)
BLYNK_WRITE(V8) {
inputSpeed = param.asFloat();
inputSpeed = constrain(inputSpeed, 0, 11); // Giới hạn tốc độ từ 0-11
Serial.println("V8 (Speed DC) - Input Speed: " + String(inputSpeed));
}

// Save Stage DC (V9)

BLYNK_WRITE(V9) {
int btn = param.asInt();
if (btn == 1) {
int optionIdx = currentOption - 1;
int stageIdx = currentStageIndex;
options[optionIdx].stages[stageIdx].distance = inputDistance;
options[optionIdx].stages[stageIdx].speed = constrain(inputSpeed, 0, 11); //
Giới hạn tốc độ
saveStageToPreferences(optionIdx, stageIdx);
Serial.println("V9 (Save Stage DC) - Saved Stage S" + String(currentStage) + "
to Option " + String(currentOption) +
": Distance=" + String(inputDistance) + ", Speed=" +
String(inputSpeed));
Blynk.virtualWrite(V6, currentStage);
}
}

// Zoom (V10)
BLYNK_WRITE(V10) {
int zoomState = param.asInt();
 if (zoomState == 1) {
Serial.println("V10 (Zoom) - Zoom In triggered");
} else {
Serial.println("V10 (Zoom) - Zoom Out triggered");
}
}

// Menu Option Pan-Tilt (V13)

BLYNK_WRITE(V13) {
int newOption = param.asInt();
if (newOption >= 1 && newOption <= 6) {
currentOption = newOption;
currentPanTiltOption = newOption;
Serial.println("V13 (Menu Option Pan-Tilt) - Selected Option: " +
String(currentPanTiltOption));
Blynk.virtualWrite(V13, currentPanTiltOption);
if (isAutoMode) {
Blynk.virtualWrite(V5, currentOption);
Serial.println("V13 (Menu Option Pan-Tilt) - Synced DC Option to V5: " +
String(currentOption));
}
} else {
Serial.println("V13 (Menu Option Pan-Tilt) - Invalid Option: " +
String(newOption));
}
}

// Menu Stage Pan-Tilt (V14)

BLYNK_WRITE(V14) {
int newStage = param.asInt();
if (newStage >= 1 && newStage <= 25) {
currentPanTiltStage = newStage;
currentPanTiltStageIndex = newStage - 1;
Serial.println("V14 (Menu Stage Pan-Tilt) - Selected Stage: S" +
String(currentPanTiltStage));
Blynk.virtualWrite(V14, currentPanTiltStage);
} else {
Serial.println("V14 (Menu Stage Pan-Tilt) - Invalid Stage: " +
String(newStage));
}
}

// Theta1 (V15)
BLYNK_WRITE(V15) {
inputTheta1 = param.asFloat();
Serial.println("V15 (Theta1 Pan-Tilt) - Input Theta1: " + String(inputTheta1));
}

// Theta2 (V16)
BLYNK_WRITE(V16) {
inputTheta2 = param.asFloat();
Serial.println("V16 (Theta2 Pan-Tilt) - Input Theta2: " + String(inputTheta2));
}

// Pan Speed (V17)

BLYNK_WRITE(V17) {
inputPanSpeed = param.asFloat();
Serial.println("V17 (Pan Speed Pan-Tilt) - Input PanSpeed: " +
String(inputPanSpeed));
}

// Tilt Speed (V18)

BLYNK_WRITE(V18) {
inputTiltSpeed = param.asFloat();
Serial.println("V18 (Tilt Speed Pan-Tilt) - Input TiltSpeed: " +
String(inputTiltSpeed));
}

// Save Stage Pan-Tilt (V19)

BLYNK_WRITE(V19) {
int btn = param.asInt();
if (btn == 1) {
int optionIdx = currentPanTiltOption - 1;
int stageIdx = currentPanTiltStageIndex;
panTiltOptions[optionIdx].stages[stageIdx].theta1 = inputTheta1;
panTiltOptions[optionIdx].stages[stageIdx].theta2 = inputTheta2;
panTiltOptions[optionIdx].stages[stageIdx].panSpeed = inputPanSpeed;
panTiltOptions[optionIdx].stages[stageIdx].tiltSpeed = inputTiltSpeed;
savePanTiltStageToPreferences(optionIdx, stageIdx);
Serial.println("V19 (Save Stage Pan-Tilt) - Saved Stage S" +
String(currentPanTiltStage) + " to Option " + String(currentPanTiltOption) +
": Theta1=" + String(inputTheta1) + ", Theta2=" +
String(inputTheta2) +
", PanSpeed=" + String(inputPanSpeed) + ", TiltSpeed=" +
String(inputTiltSpeed));
Blynk.virtualWrite(V14, currentPanTiltStage);
}
}

// Switch Mode (V20)

BLYNK_WRITE(V20) {
int mode = param.asInt();
Serial.println("V20 (Switch Mode) - Received mode: " + String(mode));
isAutoMode = (mode == 1);
Blynk.virtualWrite(V20, isAutoMode ? 1 : 0); // Đồng bộ trạng thái với Blynk
Serial.println("V20 (Switch Mode) - Switched to: " + String(isAutoMode ? "Auto" :
"Manual") +
", isAutoMode: " + String(isAutoMode));
if (!isAutoMode) {
panTiltRunning = false;
currentPanTiltStageStep = 0;
currentSpeed = 0;
dir = 0;
joystickActive = false;
// String msg = "D:0,S:0\n";
// SerialUART.print(msg);
// Serial.println("V20 (Switch Mode) - Stopped DC: Sent to STM32: " + msg);
// msg = "P:0,T:0\n";
// SerialUART.print(msg);
// Serial.println("V20 (Switch Mode) - Stopped Pan-Tilt: Sent to STM32: " +
msg);
}
}

// Run Auto (V21)

BLYNK_WRITE(V21) {
int btn = param.asInt();
if (btn == 1 && isAutoMode) {
 panTiltRunning = true;
currentPanTiltStageStep = 0;
totalDistance = 0;
Serial.println("V21 (Run Auto) - Starting Auto mode for Option " +
String(currentOption));
Blynk.virtualWrite(V21, 0);
}
}

// Pause Auto (V22)

BLYNK_WRITE(V22) {
int btn = param.asInt();
if (btn == 1) {
if (isAutoMode && panTiltRunning) {
if (!isPaused) {
isPaused = true;
currentSpeed = 0;
String msgDC = "D:0,S:0\n";
SerialUART.print(msgDC);
Serial.println("V22 (Pause Auto) - Paused: Sent to STM32 (DC): " + msgDC);

String msgPT = "P:0,T:0\n";

SerialUART.print(msgPT);
Serial.println("V22 (Pause Auto) - Paused: Sent to STM32 (Pan-Tilt): " +
msgPT);

Serial.println("V22 (Pause Auto) - Auto mode paused at Stage S" +

String(currentPanTiltStageStep + 1));
} else {
isPaused = false;
Serial.println("V22 (Pause Auto) - Resumed Auto mode at Stage S" +
String(currentPanTiltStageStep + 1));
}
} else {
Serial.println("V22 (Pause Auto) - Cannot pause/resume: Auto mode not
running");
}
}
}

unsigned long lastSendTime = 0;

unsigned long lastPrintTime_CV = 0;
unsigned long lastPrintTime_NP = 0;
String uartData = "";

void setup() {
Serial.begin(115200);
delay(100);
Serial.println("ESP32 Started");
SerialUART.begin(115200, SERIAL_8N1, RX_PIN, TX_PIN);
Serial.println("UART Initialized");

loadOptionsFromPreferences();
loadPanTiltOptionsFromPreferences();

// Đặt chế độ Manual mặc định

isAutoMode = false;
Blynk.begin(auth, ssid, pass, "blynk.dke.vn", 8888);
Serial.println("Blynk Connected");
 if (WiFi.status() == WL_CONNECTED) Serial.println("WiFi Connected");
else Serial.println("WiFi Not Connected");

// Đồng bộ trạng thái ban đầu với Blynk

Blynk.virtualWrite(V5, currentOption);
Blynk.virtualWrite(V6, currentStage);
Blynk.virtualWrite(V13, currentPanTiltOption);
Blynk.virtualWrite(V14, currentPanTiltStage);
Blynk.virtualWrite(V20, 0); // Đảm bảo V20 hiển thị Manual
Serial.println("Setup - Initialized in Manual mode, isAutoMode: " +
String(isAutoMode));
}

void loop() {
Blynk.run();
unsigned long now = millis();

// Gửi lệnh joystick liên tục trong Manual mode

if (!isAutoMode && now - lastJoystickSendTime >= JOYSTICK_SEND_INTERVAL) {
String msg;
if (joystickActive) {
msg = "D:" + String(dir) + ",S:" + String(currentSpeed) + "\n"; // Gửi liên
tục khi giữ joystick
SerialUART.print(msg);
Serial.println("Loop (Joystick DC) - Sent to STM32: " + msg);
} else {
msg = "D:0,S:0\n"; // Gửi dừng khi không giữ joystick
SerialUART.print(msg);
Serial.println("Loop (Joystick DC) - Stopped: Sent to STM32: " + msg);
}
lastJoystickSendTime = now;
}

// Chạy kịch bản ở chế độ Auto

if (isAutoMode && panTiltRunning && !isPaused && now - lastUpdateTime >= 2000) {
lastUpdateTime = now;
int optionIdx = currentOption - 1;

// Chạy kịch bản động cơ DC

if (currentPanTiltStageStep < maxSavedStage) {
float targetDistance = 0;
float targetSpeed = 0;
bool stageFound = false;
int currentStageIdx = -1;

for (int i = 0; i <= currentPanTiltStageStep; i++) {

targetDistance += options[optionIdx].stages[i].distance;
}
if (NewCurPos_global <= targetDistance) {
targetSpeed = options[optionIdx].stages[currentPanTiltStageStep].speed;
currentStageIdx = currentPanTiltStageStep;
stageFound = true;
}

if (stageFound && NewCurPos_global <= targetDistance) {

currentSpeed = constrain(targetSpeed, 0, 11); // Giới hạn tốc độ từ 0-11
Serial.println("Auto (DC) - Running Stage S" + String(currentStageIdx + 1)
+ ": Speed=" + String(currentSpeed) +
", NewCurPos_global=" + String(NewCurPos_global) + ",
TargetDistance=" + String(targetDistance));
String msg = "D:0,S:" + String(currentSpeed) + "\n";
SerialUART.print(msg);
Serial.println("Auto (DC) - Sent to STM32: " + msg);
} else {
currentSpeed = 0;
String msg = "D:0,S:0\n";
SerialUART.print(msg);
Serial.println("Auto (DC) - Stopped: Exceeded TargetDistance");
}

// Chạy kịch bản động cơ pan-tilt

PanTiltStage targetStage =
panTiltOptions[optionIdx].stages[currentPanTiltStageStep];
float deltaTheta1 = targetStage.theta1 - theta1;
float deltaTheta2 = targetStage.theta2 - theta2;
int steps1 = (deltaTheta1 * STEPS_PER_REVOLUTION) / 360.0;
int steps2 = (deltaTheta2 * STEPS_PER_REVOLUTION) / 360.0;

if (abs(steps1) > 0 || abs(steps2) > 0) {

String msg = "P:" + String(steps1) + ",T:" + String(steps2) + "\n";
SerialUART.print(msg);
Serial.println("Auto (Pan-Tilt) - Sent to STM32 Stage S" +
String(currentPanTiltStageStep + 1) + ": " + msg);
}

theta1 = targetStage.theta1;
theta2 = targetStage.theta2;
lastTheta1 = theta1;
lastTheta2 = theta2;
panSpeed = targetStage.panSpeed;
tiltSpeed = targetStage.tiltSpeed;
panDir = (deltaTheta1 >= 0) ? 0 : 1;
tiltDir = (deltaTheta2 >= 0) ? 0 : 1;

Serial.println("Auto (Pan-Tilt) - Running Stage S" +

String(currentPanTiltStageStep + 1) +
": Theta1=" + String(theta1) + ", Theta2=" + String(theta2) +
", PanSpeed=" + String(panSpeed) + ", TiltSpeed=" +
String(tiltSpeed));

currentPanTiltStageStep++;
} else {
currentPanTiltStageStep = 0;
panTiltRunning = false;
currentSpeed = 0;
String msg = "D:0,S:0\n";
SerialUART.print(msg);
Serial.println("Auto - Option " + String(currentOption) + " completed up to
saved stage S" + String(maxSavedStage));
}
}

// Xử lý dữ liệu từ STM32
while (SerialUART.available()) {
char c = SerialUART.read();
if (c == '\n') {
if (uartData.startsWith("V:")) {
float CurVel = uartData.substring(2).toFloat();
 if (now - lastPrintTime_CV >= 1000) {
lastPrintTime_CV = now;
Serial.println("STM32 - CurVel: " + String(CurVel));
}
} else if (uartData.startsWith("NP:")) {
float NewCurPos = uartData.substring(3).toFloat();
if (resetPending && (now - lastResetTime < 2000)) {
if (abs(NewCurPos) < 1.0) {
Serial.println("STM32 - Confirmed reset: NewCurPos=" +
String(NewCurPos));
resetPending = false;
} else {
Serial.println("STM32 - Error: Invalid position after reset: NP:" +
String(NewCurPos) + ", forcing to 0");
NewCurPos_global = 0;
}
} else {
NewCurPos_global = NewCurPos;
}
if (now - lastPrintTime_NP >= 1000) {
lastPrintTime_NP = now;
Serial.println("STM32 - NewCurPos: " + String(NewCurPos_global));
}
} else if (uartData.startsWith("Z:")) {
String zoomCmd = uartData.substring(2);
if (zoomCmd == "IN") {
Blynk.virtualWrite(V10, 1);
Serial.println("STM32 - Zoom In triggered");
} else if (zoomCmd == "OUT") {
Blynk.virtualWrite(V10, 0);
Serial.println("STM32 - Zoom Out triggered");
}
}
uartData = "";
} else {
uartData += c;
}
}
}