// Teensy 4.1 communication with Nextion display
#include <AccelStepper.h>
#include <EEPROM.h>

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

// EEPROM Addresses
#define EEPROM_ACTUAL_FEED_SPEED 0
#define EEPROM_FEED_SELECTION_SET 4
#define EEPROM_START_DELAY 5
#define EEPROM_END_DELAY 9
#define EEPROM_BURN_OFFSET 13
#define EEPROM_CALIBRATION_OFFSET 17
#define EEPROM_FEED_SPEED 21
#define EEPROM_MANUAL_SPEED 25

// Global variables
String Page = "";

// Main Page Variables
int Actual_Feed_Speed = 165;
String Calibration_Status_TXT = "Incomplete";
String Feed_Selection = "Double";

// Setting Page Variables
bool FEED_SELECTION_SET = 0;
int Start_Delay = 2; //Sec
int End_Delay = 2; // Sec

int Burn_Offset = 0;//mm
int Calibration_Offset = 4; // mm
float Feed_Speed = 15.0; // mm/sec

// Manual Page Variables
float Manual_Speed = 10.0; // mm/sec
bool Manual_Forward = 0;
bool Manual_Reverse = 0;

// Variables
int wheel_diameter = 10; // mm
bool Calibration_Status = 0;
bool Triger_Started = false;

float Calibration_Speed = 5.0; // mm/sec

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() {
  // Initialize Serial communication for debugging and display
  Serial.begin(9600); // USB debugging
  while (!Serial) {
    // Wait for Serial to connect
  }

  Serial1.begin(9600); // Nextion display communication
  Serial.println("Setup complete. Loading variables from EEPROM...");

  // Load variables from EEPROM
  Actual_Feed_Speed = EEPROM.read(EEPROM_ACTUAL_FEED_SPEED);
  FEED_SELECTION_SET = EEPROM.read(EEPROM_FEED_SELECTION_SET);
  Start_Delay = EEPROM.read(EEPROM_START_DELAY);
  End_Delay = EEPROM.read(EEPROM_END_DELAY);
  Burn_Offset = EEPROM.read(EEPROM_BURN_OFFSET);
  Calibration_Offset = EEPROM.read(EEPROM_CALIBRATION_OFFSET);
  Feed_Speed = EEPROM.read(EEPROM_FEED_SPEED);
  Manual_Speed = EEPROM.read(EEPROM_MANUAL_SPEED);

  Serial.println("Variables loaded.");

  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() {
  if (Serial1.available()) {
    String dataFromDisplay = "";
    delay(30); // Allow time for data to arrive

    while (Serial1.available()) {
      dataFromDisplay += char(Serial1.read());
    }

    processReceivedData(dataFromDisplay);

    if (Page == "Home_Page") {
      sendNextionCommand("Main_N1.val=" + String(Actual_Feed_Speed));
      if(Calibration_Status == 1){Calibration_Status_TXT = "Complete";}
      else if(Calibration_Status == 0){Calibration_Status_TXT = "Incomplete";}
      sendNextionCommand("Main_T1.txt=\"" + Calibration_Status_TXT + "\"");
      if(FEED_SELECTION_SET == 1){Feed_Selection = "Double";}
      else if(FEED_SELECTION_SET == 0){Feed_Selection = "Single";}
      sendNextionCommand("Main_T2.txt=\"" + Feed_Selection + "\"");
    }
  }

  // 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();

}


// Process received data from the Nextion display
void processReceivedData(String dataFromDisplay) {
  if (dataFromDisplay == "Home_Page") {
    Page = "Home_Page";
    Serial.println(Page);
  } else if (dataFromDisplay == "Setup1_Page") {
    Page = "Setup1_Page";
    sendNextionCommand("Setup1_SW1.val=" + String(FEED_SELECTION_SET));
    sendNextionCommand("Setup1_N1.val=" + String(Start_Delay));
    sendNextionCommand("Setup1_N2.val=" + String(End_Delay));
    Serial.println(Page);
  } else if (dataFromDisplay == "Setup2_Page") {
    Page = "Setup2_Page";
    sendNextionCommand("Setup2_N1.val=" + String(Burn_Offset));
    sendNextionCommand("Setup2_N2.val=" + String(Calibration_Offset));
    sendNextionCommand("Setup2_N3.val=" + String(Feed_Speed));
    Serial.println(Page);
  } else if (dataFromDisplay == "Manual_Page") {
    Page = "Manual_Page";
    sendNextionCommand("Manual_N1.val=" + String(Manual_Speed));
    Serial.println(Page);
  } else if (dataFromDisplay.startsWith("S1_SW")) {
    FEED_SELECTION_SET = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_FEED_SELECTION_SET, FEED_SELECTION_SET);
    Serial.println(FEED_SELECTION_SET);
  } else if (dataFromDisplay.startsWith("S1_N1")) {
    Start_Delay = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_START_DELAY, Start_Delay);
    Serial.println(Start_Delay);
  } else if (dataFromDisplay.startsWith("S1_N2")) {
    End_Delay = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_END_DELAY, End_Delay);
    Serial.println(End_Delay);
  } else if (dataFromDisplay.startsWith("S2_N1")) {
    Burn_Offset = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_BURN_OFFSET, Burn_Offset);
    Serial.println(Burn_Offset);
  } else if (dataFromDisplay.startsWith("S2_N2")) {
    Calibration_Offset = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_CALIBRATION_OFFSET, Calibration_Offset);
    Serial.println(Calibration_Offset);
  } else if (dataFromDisplay.startsWith("S2_N3")) {
    Feed_Speed = (uint8_t)dataFromDisplay[5];
    EEPROM.update(EEPROM_FEED_SPEED, Feed_Speed);
    Serial.println(Feed_Speed);
  } else if (dataFromDisplay.startsWith("M_N1")) {
    Manual_Speed = (uint8_t)dataFromDisplay[4];
    EEPROM.update(EEPROM_MANUAL_SPEED, Manual_Speed);
    Serial.println(Manual_Speed);
  } else {
    Serial.println("Unknown command: " + dataFromDisplay);
  }
}

// Function to send a command to the Nextion display
void sendNextionCommand(String command) {
  Serial1.print(command);
  sendCommandEnd();
}

// Function to send command termination sequence
void sendCommandEnd() {
  Serial1.write(0xff);
  Serial1.write(0xff);
  Serial1.write(0xff);
}