#include <BluetoothSerial.h>  // ספריית ה-Bluetooth של ESP32

#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <XPT2046_Touchscreen.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// הגדרות למסך ה-TFT
#define TFT_CS 15
#define TFT_DC 2
#define TFT_RST -1  // אם יש פין RST ייעודי, ניתן לשנות
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// הגדרות למסך הטאץ'
#define TOUCH_CS 14
#define TOUCH_IRQ 13
XPT2046_Touchscreen touch(TOUCH_CS, TOUCH_IRQ);

// יצירת אובייקט Bluetooth
BluetoothSerial SerialBT;

unsigned long lastTouchTime = 0;
int debounceDelay = 200; // 200 milliseconds debounce delay

// הגדרת הפינים עבור כל מנוע
int stepPins[] = {14, 26, 33, 21, 19, 17}; // פיני ה-Step לכל מנוע
int dirPins[] = {27, 25, 32, 22, 18, 16};  // פיני ה-Direction לכל מנוע
const int numMotors = 6;  // מספר המנועים

// Struct representing a motor command
struct MotorCommand {
  int motorNum;
  int steps;
  int speed;
  bool direction;
};

// Queue to store motor commands
MotorCommand commandQueue[10]; // Adjust size as needed
int queueCount = 0; // Count of commands in the queue

bool stopFlag = false; // משתנה שיעצור את הפעולה אם התקבל FLAG

void setup() {
  Serial.begin(115200);
  Serial.println("התחלת מערכת");

  // הגדרת הפינים כיציאות
  for (int i = 0; i < numMotors; i++) {
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);
  }
  
  tft.begin();
  tft.fillScreen(ILI9341_BLACK); // Clear screen with black color

  // Set text settings
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(0, 0);
  tft.println("Hello, TFT!");

  // הפעלת מסך הטאץ'
  if (!touch.begin()) {
    Serial.println("לא ניתן להתחבר למסך הטאץ'!");
    while (1);
  }

  touch.setRotation(1);

  // אתחול Bluetooth
  //SerialBT.begin("ESP32_Motor_Control");
}

// פונקציה לקביעת כיוון לכל מנוע
void setMotorDirection(int motor, bool direction) {
  digitalWrite(dirPins[motor], direction ? HIGH : LOW); // קבע כיוון בהתאם לפרמטר
}

// פונקציה להפעלת צעד אחד עבור כל מנוע
void stepMotor(int motor) {
  digitalWrite(stepPins[motor], HIGH);
  delayMicroseconds(10);  // עיכוב קצר כדי להבטיח את פעולת הצעד
  digitalWrite(stepPins[motor], LOW);
}

// פונקציה להזזת כל המנועים במקביל לפי הפקודות שבתור
void moveMotors() {
  if (queueCount == 0) {
    Serial.println("No commands in the queue to process.");
    return; // No commands to process
  }

  MotorCommand currentCommand = commandQueue[0];

  Serial.print("Processing command for motor: ");
  Serial.println(currentCommand.motorNum + 1);
  Serial.print("Steps: ");
  Serial.println(currentCommand.steps);
  Serial.print("Speed: ");
  Serial.println(currentCommand.speed);
  Serial.print("Direction: ");
  Serial.println(currentCommand.direction ? "Forward" : "Backward");

  setMotorDirection(currentCommand.motorNum, currentCommand.direction);

  for (int step = 0; step < currentCommand.steps; step++) {
    if (isStopped()) {
      Serial.println("Movement stopped by stopFlag.");
      stopMotor(currentCommand.motorNum); // Explicitly stop motor when stopped
      return;
    }

    Serial.print("Motor ");
    Serial.print(currentCommand.motorNum + 1);
    Serial.print(" performing step: ");
    Serial.println(step + 1);

    stepMotor(currentCommand.motorNum); // הזזת המנוע בצעד אחד
    delayMicroseconds(currentCommand.speed);
  }

  Serial.println("Motor " + String(currentCommand.motorNum + 1) + " finished its steps.");
  
  // Stop motor after processing the steps
  stopMotor(currentCommand.motorNum);

  // Remove the command from the queue
  for (int i = 1; i < queueCount; i++) {
    commandQueue[i - 1] = commandQueue[i];
  }
  queueCount--;

  // Reset stopFlag after processing
  stopFlag = false;

  Serial.println("Command completed. Queue count is now: " + String(queueCount));
}

// פונקציה לעצור מנוע
void stopMotor(int motorNum) {
  digitalWrite(stepPins[motorNum], LOW); // Ensure the motor is not receiving further steps
  Serial.println("Motor " + String(motorNum + 1) + " stopped.");
}


// פונקציה להזזת כל המנועים במקביל לפי הפקודות שבתור
void moveMotorssss() {
  if (queueCount == 0) {
    //Serial.println("No commands in the queue to process.");
    return; // No commands to process
  }

  MotorCommand currentCommand = commandQueue[0];

  Serial.print("Processing command for motor: ");
  Serial.println(currentCommand.motorNum + 1);
  Serial.print("Steps: ");
  Serial.println(currentCommand.steps);
  Serial.print("Speed: ");
  Serial.println(currentCommand.speed);
  Serial.print("Direction: ");
  Serial.println(currentCommand.direction ? "Forward" : "Backward");

  setMotorDirection(currentCommand.motorNum, currentCommand.direction);

  for (int step = 0; step < currentCommand.steps; step++) {
    if (isStopped()) {
      Serial.println("Movement stopped by stopFlag.");
      return;
    }

    Serial.print("Motor ");
    Serial.print(currentCommand.motorNum + 1);
    Serial.print(" performing step: ");
    Serial.println(step + 1);

    stepMotor(currentCommand.motorNum); // הזזת המנוע בצעד אחד
    delayMicroseconds(currentCommand.speed);
  }

  Serial.println("Motor " + String(currentCommand.motorNum + 1) + " finished its steps.");

  // Remove the command from the queue
  for (int i = 1; i < queueCount; i++) {
    commandQueue[i - 1] = commandQueue[i];
  }
  queueCount--;

  // Reset stopFlag after processing
  stopFlag = false;

  Serial.println("Command completed. Queue count is now: " + String(queueCount));
}


// פונקציה לבדיקת האם קיים פקודת עצירה
void checkForStopCommand() {
  String command = "";
  if (SerialBT.available()) {
    command = SerialBT.readString();
  }
  if (!command) {
    command = Serial.readString();
  }
  if (command == "STOP") {
    stopFlag = true;
  }
}

// פונקציה לקבלת נתונים מהמשתמש והכנסתם לתור
void receiveMotorData() {
  if (SerialBT.available() || Serial.available()) {
    String data = SerialBT.readString();
    if (data == "") {
      data = Serial.readString();
    }

    Serial.println("Received data: " + data);
    
    // נניח שהמבנה הוא: "MOTOR_NUM:STEPS:SPEED:DIRECTION" (מנוע:צעדים:מהירות:כיוון)
    int motorNum = data.substring(0, data.indexOf(':')).toInt() - 1;
    int steps = data.substring(data.indexOf(':') + 1, data.lastIndexOf(':')).toInt();
    int speed = data.substring(data.lastIndexOf(':') + 1, data.length() - 2).toInt();
    bool direction = (data.charAt(data.lastIndexOf(':') + 1) == '1');


    if (motorNum >= 0 && motorNum < numMotors) { // Check if queue is not full
      commandQueue[queueCount] = {motorNum, steps, speed, direction};
      queueCount++;
      Serial.println("Command added to queue: Motor " + String(motorNum + 1) + ", Steps: " + steps + ", Speed: " + speed + ", Direction: " + (direction ? "Positive" : "Negative"));
    } else if(queueCount >= 10){
        Serial.println("Queue is full");
    } else {
      Serial.println("Invalid motor number.");
    }
  }
}

boolean isStopped() {
  checkForStopCommand();
  if (stopFlag) {
    Serial.println("Stopping motors due to stopFlag...");
    return true;
  }
  return false;
}

void loop() {
  if (touch.touched()) {
    unsigned long currentTime = millis();
    if (currentTime - lastTouchTime > debounceDelay) {
      TS_Point p = touch.getPoint();
      p.x = map(p.x, 3800, 200, 0, tft.width());
      p.y = map(p.y, 200, 3800, 0, tft.height());

      tft.println("Touch detected at X: " + String(p.x) + ", Y: " + String(p.y));
      lastTouchTime = currentTime;
    }
  }

  receiveMotorData(); // קבלת הנתונים דרך Bluetooth
  if (queueCount == 0) {
    //Serial.println("No commands in the queue to process.");
    return; // No commands to process
  }
  moveMotors();       // הפעלת המנועים בהתאם לפקודות
}