#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <AccelStepper.h>

// Define the stepper motor pins
#define X_STEP_PIN 18
#define X_DIR_PIN 19
#define X_EN_PIN 5


#define X_STEP_PIN 17
#define Z_DIR_PIN 16
#define Z_EN_PIN 4



// Create instances of AccelStepper for both motors
AccelStepper stepper_x(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepper_z(AccelStepper::DRIVER, X_STEP_PIN, Z_DIR_PIN);
// Struct to hold motor parameters
struct MotorParams {

  int targetPosition;
  int speed;
};

 

unsigned long lastTime = 0;
unsigned long timerDelay = 5000;
int X_Task_working = 0;
int Z_Task_working = 0;
int passValue = 0;
void setup() {
  Serial.begin(115200);  
  delay(2000);  

}

void loop() {
  if ((millis() - lastTime) > timerDelay ) { //&& state_barbot_working == 0&& f4_Task_working == 0
      Serial.println(" Loop");
      
      
      Serial.print(millis());
      Serial.print("-");
      Serial.print(lastTime);
      Serial.print("=");
        Serial.print((millis()-lastTime));
      Serial.print(">");
      Serial.println(timerDelay);
        
        if(X_Task_working == 0){
          MotorParams MOTORXParams = {  5, 200 };
          passValue = 800;
          xTaskCreatePinnedToCore(MotorXTask,"Motor X Task",1000,&MOTORXParams,0,NULL,0);//&MOTORXParams
        }
        if(Z_Task_working == 0){
          MotorParams MOTORZParams = {  6, 200 };
          passValue = 1000;
          xTaskCreatePinnedToCore(MotorZTask,"Motor Z Task",1000,(void*) passValue,1,NULL,1);
        }
          lastTime = millis();
  }

}
void MotorZTask(void *pvParam){        
   // MotorParams *params = (MotorParams *)pvParam;
 int targetPosition = (int)pvParam ;  
 // String targetPosition = params->targetPosition;
  //String speed = params->speed;
   Serial.println("---------");
  Serial.println("Z targetPosition:");
  Serial.print(targetPosition);
  //Serial.print("Z speed:");
 // Serial.println(speed);
  Z_Task_working = 1;
 
    // Move Motor Z
    stepper_z.moveTo(800); // Move 800 steps
    while (stepper_z.distanceToGo() != 0) {
      stepper_z.run();
      vTaskDelay(1); // Small delay to avoid high CPU usage
      Z_Task_working = 0;
       yield();
    }

  
}
void MotorXTask(void *pvParam){
  MotorParams *params = (MotorParams *)pvParam;        
 
 int targetPosition =  params->targetPosition;
  int speed = (int)params->speed;
   Serial.println("---------");
  Serial.println("X targetPosition:");
  Serial.print(targetPosition);
 //Serial.print("Z speed:");
  //Serial.println(speed);
  X_Task_working = 1;
 
    // Move Motor X
    stepper_x.moveTo(800); // Move 800 steps
    while (stepper_x.distanceToGo() != 0) {
      stepper_x.run();
      vTaskDelay(1); // Small delay to avoid high CPU usage
      X_Task_working = 0;
       yield();
    }

  
}