#include <AccelStepper.h>
#include <limits.h>
#define ONE_ROTATION 1600
#include <esp_task_wdt.h>

TaskHandle_t stepperTask;

#define DEFAULT_ACCEL INT_MAX
#define DEFAULT_SPEED INT_MAX

AccelStepper stepper1(1, 18, 19); // (Typeof driver: with 2 pins, STEP, DIR)
AccelStepper stepper2(1, 5, 4);
// AccelStepper stepper3(1, 6, 7);

AccelStepper stepper_motors[2];

struct StepperParams
{
    AccelStepper *stepper;
    int speed;
    int acceleration;
    int steps;
    int motor_index;
};

void stepper_move(void *parameter)
{
    StepperParams *params = (StepperParams *)parameter;
    AccelStepper *stepper = params->stepper;
    int speed = params->speed;
    int acceleration = params->acceleration;
    int steps = params->steps;
    int motor_index = params->motor_index;

    stepper->setCurrentPosition(0);
    stepper->setSpeed(speed);
    stepper->setAcceleration(acceleration);
    stepper->move(steps);

    // disableCore0WDT(); // This is not good use another way so the cores are both used properly

    stepper->runToPosition();

    // disableCore0WDT(); // This is not good use another way so the cores are both used properly
    // disableCore1WDT();
    // while (stepper->distanceToGo())
    // {
    //     stepper->run();
    // }

    // enableCore0WDT();
    // enableCore1WDT();

    stepper->setAcceleration(DEFAULT_ACCEL);
    stepper->setCurrentPosition(0);
    Serial.println("M" + String(motor_index) + ":ok");
    vTaskDelete(NULL);
}


void checkSerial()
{
    String cmd[4] = {}; // will hold all of the commands and values for processing...

    if (Serial.available())
    {
      String serialIn = Serial.readStringUntil('\n');
      String token; // tokenizer
      int index = 0;
      int cmdIndex = -1;

      Serial.println(serialIn);

      while (serialIn.length() > 0)
      {
        index = serialIn.indexOf(" ");
        if (index == -1) index = serialIn.length(); // this is for the last part or single word commands...
        token = serialIn.substring(0, index); // get the token
        serialIn.remove(0, index + 1); // remove upto the space
        cmd[++cmdIndex] = token; // save the token
      }
    }

    int cmdSize = 0;

    for (int i = 0; i < sizeof(cmd) / sizeof(cmd[0]); i++)
    {
        if (cmd[i] == NULL)
            break;
        cmdSize++;
    }

    if (cmdSize == 0) return;

    // // yes switch case is better but arduino String is setupt wierd so switch case throws erro rcuz cmd is void pointer
    if (cmd[0].equalsIgnoreCase("reset") || cmd[0].equalsIgnoreCase("r"))
        Serial.println("RESET");

    else if (String(cmd[0][0]).equalsIgnoreCase("m"))
    {
        int motor_index = String(cmd[0].substring(1)).toInt();
        if (motor_index >= 0 && motor_index < sizeof(stepper_motors) / sizeof(stepper_motors[0]))
        {
            for (int i = 1; i < 4; i++)
                if (cmd[i].equalsIgnoreCase("max"))
                    cmd[i] = INT_MAX;

            StepperParams params = {&stepper_motors[motor_index], cmd[1].toInt(), cmd[2].toInt(), cmd[3].toInt(), motor_index};
            xTaskCreatePinnedToCore(stepper_move, ("M" + String(motor_index) + "_T").c_str(), 10000, (void *)&params, 1, &stepperTask, tskNO_AFFINITY);
        }
        else
        {
            Serial.println("Invalid motor index.");
        }
    }


  else
    Serial.println("COMMAND NOT FOUND");
}

void setup() 
{
  // put your setup code here, to run once:
  Serial.begin(115200);
  stepper_motors[0] = stepper1;
  stepper_motors[1] = stepper2;

  for (int i = 0; i < sizeof(stepper_motors) / sizeof(stepper_motors[0]); i++)
  {
    stepper_motors[i].setMaxSpeed(DEFAULT_SPEED);
    stepper_motors[i].setSpeed(DEFAULT_SPEED);
    stepper_motors[i].setAcceleration(DEFAULT_ACCEL);
  }

}

void loop() {
  // put your main code here, to run repeatedly:
checkSerial();
}