#include "HardwareSerial.h"


#define EN_PIN D5
#define STEP_PIN D8
#define DIR_PIN D9
#define POT_PIN A0
#define TX_PIN D6
#define RX_PIN D7


const int STEP_PULSE_WIDTH_US = 5;  // Microseconds for the STEP pin to be HIGH (min 1-2us for A4988)

String serialCommand = "";        // String to store incoming serial command
bool newCommandReceived = false;  // Flag to indicate a new command has been received

long stepsToRun = 0;                      // Number of steps to execute for a 'steps' command
unsigned int currentStepPeriodUs = 2000;  // Default total period for one step in microseconds (e.g., 2000us = 500 Hz)
bool motorRunningFromCommand = false;     // Flag to indicate motor is running due to a serial command
bool continuousRun = false;               // Flag for 'speed' command (continuous run)


void setup() {
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  Serial.begin(115200);

  // while (!Serial){
  //   delay(10);
  // }
  Serial1.setRX(RX_PIN); // Set RX pin for Serial1
  Serial1.setTX(TX_PIN); // Set TX pin for Serial1

  Serial1.begin(115200);

    
  Serial.println("Serial Connection Established.");
  Serial.println("Listening for commands on Serial1 (Pins D0-TX, D1-RX).");
  Serial.println("System Ready. Available commands:");
  Serial.println("  speed <totalPeriodMicroseconds>");
  Serial.println("  steps <numberOfMicrosteps><totalPeriodMicroseconds>");
  Serial.println("  stop");



  digitalWrite(DIR_PIN, HIGH);  // set direction
  digitalWrite(EN_PIN, HIGH); // disabled
  delayMicroseconds(100);
}

void loop() {
  if (Serial1.available() > 0) {
    serialCommand = Serial1.readStringUntil('\n');
    serialCommand.trim(); // Remove any leading/trailing whitespace
    newCommandReceived = true;
  } else if (Serial.available() > 0) {
    serialCommand = Serial.readStringUntil('\n');
    serialCommand.trim();  // Remove any leading/trailing whitespace
    newCommandReceived = true;
  }
  if (newCommandReceived) {
    Serial.print("Received: '");
    Serial.print(serialCommand);
    Serial.println("'");
    processSerialCommand();
    newCommandReceived = false;
  }
  if (motorRunningFromCommand) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(STEP_PULSE_WIDTH_US);
    digitalWrite(STEP_PIN, LOW);
    if (currentStepPeriodUs > STEP_PULSE_WIDTH_US) {
      delayMicroseconds(currentStepPeriodUs - STEP_PULSE_WIDTH_US);
    } else {
      delayMicroseconds(1);
    }

    if (!continuousRun && stepsToRun > 0) {
      stepsToRun--;
      if (stepsToRun == 0) {
        motorRunningFromCommand = false;
        Serial.println("Finished commanded steps.");
      }
    }
  }
}

void processSerialCommand() {
  if (serialCommand.startsWith("steps")) {
    // Format: "steps <numberOfSteps> <totalPeriodMicroseconds>"
    // Example: "steps 16000 1000" (16000 microsteps at 1000us period = 1000 Hz)
    long numStepsCmd = 0;
    unsigned int periodCmd = 0;

    // Find the space after "steps"
    int firstSpace = serialCommand.indexOf(' ');
    if (firstSpace == -1) {
      Serial.println("Invalid 'steps' command: Missing arguments.");
      return;
    }

    // Find the space between numberOfSteps and totalPeriodMicroseconds
    int secondSpace = serialCommand.indexOf(' ', firstSpace + 1);
    if (secondSpace == -1) {
      Serial.println("Invalid 'steps' command: Missing period argument.");
      return;
    }

    String stepsStr = serialCommand.substring(firstSpace + 1, secondSpace);
    String periodStr = serialCommand.substring(secondSpace + 1);

    numStepsCmd = stepsStr.toInt();
    periodCmd = periodStr.toInt();

    if (numStepsCmd > 0 && periodCmd >= (STEP_PULSE_WIDTH_US * 2)) {  // Basic validation
      stepsToRun = numStepsCmd;
      currentStepPeriodUs = periodCmd;
      motorRunningFromCommand = true;
      continuousRun = false;  // Not continuous
      // Re-enable driver just in case it was disabled after a previous command
      digitalWrite(EN_PIN, LOW);  // ENABLE driver
      Serial.print("Executing ");
      Serial.print(stepsToRun);
      Serial.print(" steps with total period ");
      Serial.print(currentStepPeriodUs);
      Serial.println(" us.");
    } else {
      Serial.println("Invalid arguments for 'steps'. Ensure steps > 0 and period is large enough.");
      Serial.println("Format: steps <numberOfSteps> <totalPeriodMicroseconds>");
    }

  } else if (serialCommand.startsWith("speed")) {
    // Format: "speed <totalPeriodMicroseconds>"
    // Example: "speed 1000" (continuous run at 1000us period = 1000 Hz)
    int firstSpace = serialCommand.indexOf(' ');
    if (firstSpace == -1) {
      Serial.println("Invalid 'speed' command: Missing period argument.");
      return;
    }
    String periodStr = serialCommand.substring(firstSpace + 1);
    unsigned int periodCmd = periodStr.toInt();

    if (periodCmd >= (STEP_PULSE_WIDTH_US * 2)) {  // Basic validation
      currentStepPeriodUs = periodCmd;
      motorRunningFromCommand = true;
      continuousRun = true;       // Continuous run
      stepsToRun = 0;             // Not used for continuous run but reset it
      digitalWrite(EN_PIN, LOW);  // ENABLE driver
      Serial.print("Running continuously with total period ");
      Serial.print(currentStepPeriodUs);
      Serial.println(" us. Send 'stop' to halt.");
    } else {
      Serial.println("Invalid period for 'speed'. Ensure period is large enough.");
      Serial.println("Format: speed <totalPeriodMicroseconds>");
    }

  } else if (serialCommand.equals("stop")) {
    stepsToRun = 0;
    motorRunningFromCommand = false;
    continuousRun = false;
    Serial.println("Motor stopped by command.");
    // Optionally disable EN_PIN here to reduce motor heat
    // digitalWrite(EN_PIN, HIGH); // DISABLE driver
    // Serial.println("A4988 Driver Disabled (EN_PIN set HIGH).");


  } else {
    Serial.println("Unknown command. Available: steps, speed, stop");
  }
}