#include <AccelStepper.h>

#define DIR_PIN      10
#define PUL_PIN      9
#define DEBUG false // Enable or disable debugging

// Variables
float Actual_Feed_Speed = 0.0; // mm/sec
int wheel_diameter = 10; // mm
bool Calibration_Status = 0;
bool Triger_Started = false;

int Start_Delay = 2; // sec
int End_Delay = 2; // sec
int Burn_Offset = 5; // mm
int Calibration_Offset = 4; // mm
float Feed_Speed = 15.0; // mm/sec
float Calibration_Speed = 5.0; // mm/sec

float Manual_Speed = 10.0; // mm/sec
bool Manual_Forward = 0;
bool Manual_Reverse = 0;
bool Manual_Stop = 0;
bool Calibration_Start = 0;
bool Calibration_Active = 0;
bool Calibration_Bypass = 0;

// Digital Inputs
#define Trigger_Start 12
#define Bed_Touched 11
#define M_For 2
#define M_Stop 13
#define M_Rev 3

// Digital Outputs
#define Welding_ON 7
#define Bed_Calibration_Supply 5
#define Wire_Feeder_Ready 6

AccelStepper stepper(AccelStepper::DRIVER, PUL_PIN, DIR_PIN);

const int stepsPerRevolution = 200; // Define steps per revolution of the stepper motor

unsigned long previousMillisStart = 0; // Store the last time the Start delay was triggered
unsigned long previousMillisEnd = 0;   // Store the last time the End delay was triggered
bool isStartDelayActive = false; // Track if Start delay is active
bool isEndDelayActive = false;   // Track if End delay is active

void debugPrint(const char* message) {
  if (DEBUG) {
    Serial.println(message);
  }
}

void debugStepperSpeed(float speed) {
  if (DEBUG) {
    Serial.print("Calculated Stepper Speed (steps/sec): ");
    Serial.println(speed);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(Trigger_Start, INPUT);
  pinMode(Bed_Touched, INPUT);
  pinMode(M_For, INPUT);
  pinMode(M_Stop, INPUT);
  pinMode(M_Rev, INPUT);
  
  pinMode(Welding_ON, OUTPUT);
  pinMode(Bed_Calibration_Supply, OUTPUT);
  pinMode(Wire_Feeder_Ready, OUTPUT);

  digitalWrite(Welding_ON, LOW);
  digitalWrite(Bed_Calibration_Supply, LOW);
  digitalWrite(Wire_Feeder_Ready, LOW);

  stepper.setAcceleration(20000.0);
  stepper.enableOutputs();

  debugPrint("Setup completed");
}

float calculateStepperSpeed(float mmPerSec) {
  float circumference = PI * wheel_diameter;
  return (mmPerSec * stepsPerRevolution * 5) / circumference;
}

void manualControl() {
  if (Manual_Forward) {
    debugPrint("Manual Forward Active");
    float speed = calculateStepperSpeed(Manual_Speed);
    stepper.setMaxSpeed(10000.0);
    stepper.setSpeed(speed);
    debugStepperSpeed(speed);
    stepper.runSpeed();
    digitalWrite(Wire_Feeder_Ready, LOW);
    Calibration_Status = 0;
    Calibration_Active = 0;
  } else if (Manual_Reverse) {
    debugPrint("Manual Reverse Active");
    float speed = -calculateStepperSpeed(Manual_Speed);
    stepper.setMaxSpeed(10000.0);
    stepper.setSpeed(speed);
    debugStepperSpeed(speed);
    stepper.runSpeed();
    digitalWrite(Wire_Feeder_Ready, LOW);
    Calibration_Status = 0;
    Calibration_Active = 0;
  } else if (Manual_Stop) {
    debugPrint("Manual Stop Active");
    stepper.stop();
  }
}

void calibrationRoutine() {
  if (Calibration_Bypass) {
    debugPrint("Calibration Bypassed");
    Calibration_Status = 1;
    return;
  }

  if (Calibration_Start) {
    debugPrint("Calibration Started");
    Calibration_Status = 0;
    Calibration_Active = 1;
    digitalWrite(Bed_Calibration_Supply, HIGH);
    digitalWrite(Welding_ON, LOW);
    digitalWrite(Wire_Feeder_Ready, LOW);
    float speed = calculateStepperSpeed(Calibration_Speed);
    stepper.setMaxSpeed(10000.0);
    stepper.setSpeed(speed);
    debugStepperSpeed(speed);

    while (!digitalRead(Bed_Touched)) {
      stepper.runSpeed();
    }

    debugPrint("Bed Touched");
    stepper.stop();
    stepper.move(-Calibration_Offset * stepsPerRevolution * 5 / (PI * wheel_diameter));
    stepper.runToPosition();
    debugStepperSpeed(speed);

    Calibration_Status = 1;
    Calibration_Active = 0;
    digitalWrite(Bed_Calibration_Supply, LOW);
    digitalWrite(Wire_Feeder_Ready, HIGH);
    debugPrint("Calibration Completed");
  }
}

void automaticWireFeeding() {

  if (digitalRead(Trigger_Start)) {
    debugPrint("Trigger Start Active");
    digitalWrite(Welding_ON, HIGH);

    if (Triger_Started == false){
      delay(Start_Delay * 1000);
      if (!digitalRead(Trigger_Start)){
        digitalWrite(Welding_ON, LOW);
        return;
      }
      Triger_Started = true;
    }
    
    Calibration_Status = 0;
    digitalWrite(Wire_Feeder_Ready, LOW);
    float speed = calculateStepperSpeed(Feed_Speed);
    stepper.setMaxSpeed(10000.0);
    stepper.setSpeed(speed);
    debugStepperSpeed(speed);
    stepper.run();  // Changed to stepper.run() for continuous operation  
  
  } else if(digitalRead(Trigger_Start) == false && Triger_Started == true) {
    debugPrint("Trigger Stopped");
    float speed = calculateStepperSpeed(Feed_Speed);
    Triger_Started = false;
    stepper.stop();
    stepper.setMaxSpeed(speed);
    stepper.move(-10 * stepsPerRevolution * 5 / (PI * wheel_diameter));
    stepper.runToPosition();
    delay(End_Delay * 1000);
    digitalWrite(Welding_ON, LOW);
    stepper.setMaxSpeed(speed);
    stepper.move((Burn_Offset + 10) * stepsPerRevolution * 5 / (PI * wheel_diameter));
    stepper.runToPosition();
    digitalWrite(Wire_Feeder_Ready, HIGH);
    Calibration_Status = 1;
  
  }
}

void loop() {
  // Update manual control inputs
  Manual_Forward = digitalRead(M_For);
  Manual_Reverse = digitalRead(M_Rev);
  Calibration_Start = digitalRead(M_Stop);

  // Call manual control logic only if any manual input is active
  if (Manual_Forward || Manual_Reverse || Manual_Stop) {
    manualControl();
  }

  // Call calibration routine only if Calibration_Start or Calibration_Active is set
  if (Calibration_Start || Calibration_Active) {
    calibrationRoutine();
  }

  // Call automatic wire feeding
  automaticWireFeeding();
  
}