// Board: XIAO ESP32-C6
#include <Arduino.h>

#include <AccelStepper.h>

AccelStepper stepper(AccelStepper::FULL2WIRE, 0, 1, true); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5

uint32_t min_uint32(uint32_t a, uint32_t b);

uint32_t min_uint32(uint32_t a, uint32_t b)
{
  return (a < b) ? a : b;
}

const int button1Pin = 18;   // Pin connected to first pushbutton
const int button2Pin = 19;   // Pin connected to second pushbutton
const int ledPin = 20;       // Pin connected to onboard LED
const int heartbeatPin = 22; // Pin connected to onboard LED

int button1State = 0;
int button2State = 0;
int motorSpeed = 0;
int motorDirection = 0; // 0 for forward, 1 for reverse, -1 for stopped
unsigned long button1PressTime = 0;
unsigned long button2PressTime = 0;
const int maxSpeed = 100; // Maximum speed of the motor

TaskHandle_t Task1, Task2;

//------------------------------------------------------------------------------
// Housekeeping
void one_sec_tick()
{
  static uint32_t lastBlink = 0;
  uint32_t currentMillis = millis();

  if (currentMillis - lastBlink >= 1000) // Blink every second
  {
    lastBlink = currentMillis;
    digitalWrite(heartbeatPin, !digitalRead(heartbeatPin)); // Toggle the LED state
    Serial.println("Tick");
    // Print the current motor speed to the serial monitor
    if (motorSpeed > 0 && (motorDirection == 0 || motorDirection == 1))
    {
      Serial.print("Motor Direction: ");
      Serial.println(motorDirection == 0 ? "Forward" : "Reverse");
      Serial.print("Motor Speed: ");
      Serial.println(motorSpeed);
    }
    else
    {
      motorSpeed = 0;      // Stop the motor if no button is pressed
      motorDirection = -1; // Indicate that the motor is stopped
      Serial.println("Motor is stopped");
    }
  }
}

//------------------------------------------------------------------------------
void setup()
{
  Serial.begin(115200);
  while (!Serial)
    ;
  delay(1000); // Allow serial port open
  Serial.println("Stepper Motor Test - XIAO ESP32 C6");

  delay(1000);
  Serial.println("Init LEDs and Buttons");

  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(heartbeatPin, OUTPUT);

  xTaskCreatePinnedToCore(taskLedAndSwitches, "LED and Switches", 1024, NULL, 1, &Task1, 0);
  xTaskCreatePinnedToCore(taskStepperMotor, "Stepper Motor", 1024, NULL, 1, &Task2, 0);

  stepper.setMaxSpeed(100);
  stepper.setSpeed(0);
}

void loop()
{
  one_sec_tick();                       // Call the one second tick function
  vTaskDelay(100 / portTICK_PERIOD_MS); // Delay to prevent blocking
}

void taskLedAndSwitches(void *pvParameters)
{
  (void)pvParameters;
  for (;;)
  {
    button1State = digitalRead(button1Pin);
    button2State = digitalRead(button2Pin);

    if (button1State == LOW)
    {
      if (button1PressTime == 0)
      {
        button1PressTime = millis();
      }
      motorSpeed = min_uint32((millis() - button1PressTime) / 100, maxSpeed);
      digitalWrite(ledPin, HIGH);
    }
    else if (button2State == LOW)
    {
      if (button2PressTime == 0)
      {
        button2PressTime = millis();
      }
      motorSpeed = min_uint32((millis() - button2PressTime) / 100, maxSpeed);
      digitalWrite(ledPin, HIGH);
    }
    else
    {
      button1PressTime = 0;
      button2PressTime = 0;
      motorSpeed = 0;
      digitalWrite(ledPin, LOW);
    }
    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}

void taskStepperMotor(void *pvParameters)
{
  (void)pvParameters;
  for (;;)
  {
    if (motorSpeed > 0)
    {
      if (button1State == LOW)
      {
        motorDirection = 0; // Forward
        stepper.setSpeed(motorSpeed);
      }
      else if (button2State == LOW)
      {
        motorDirection = 1; // Reverse
        stepper.setSpeed(-motorSpeed);
      }
    }
    else
    {
      motorDirection = -1; // Stopped
      stepper.setSpeed(0);
    }
    stepper.runSpeed(); // Run the stepper motor at the set speed
    
    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}