// Stepper Esp32 CXZaxis_buttons_v6 switch display to task 0 
// 2024-01-24 Denna kod fungerar med CXZ axis EZ buttons Semaphore & limit Switch X & Z axis distans per revolution, 
// att göra :  , fastaccelstepper
// När detta fungerar fixa Free RTOS ? kolla innan om fastaccelstepper funfgerar utan FreRTOS ?
// remove / comment out ALL serial print later 
//  https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html
// accelstepper example  https://www.pjrc.com/teensy/td_libs_AccelStepper.html 
// https://github.com/RalphBacon/ESP32-Dual-Core-Programming/blob/master/ESP32_DUALCORE_BLINK.ino
// https://docs.espressif.com/projects/esp-idf/en/v4.3/esp32/api-reference/system/freertos.html
// https://www.freertos.org/features.html

#include <Wire.h>
#include <AccelStepper.h>
#include <LiquidCrystal_I2C.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <ezButton.h> // Include the ezButton library

LiquidCrystal_I2C lcd(0x27, 16, 2);

// 
TaskHandle_t CSpindleTask; // defining task for C spindle 
TaskHandle_t XZjoystickTask; // defining task for XZjoystick 
SemaphoreHandle_t Semaphore; // defining the semaphore handle to update LCD 

// X & Z axis 
AccelStepper stepperX(1, 27, 26); // X axis STEP pin , DIR pin
AccelStepper stepperZ(1, 19, 18); // Z axis STEP pin , DIR pin
bool resetXButtonState = false;
bool resetZButtonState = false;
ezButton resetXButton(14); // reset X button display 
ezButton resetZButton(15); // reset Z button display 
const int Analog_X_pin = 13; // analog joystic in , const int WAS const byte
const int Analog_Z_pin = 12;
int Analog_X = 0; // analog joystic 
int Analog_Z = 0;
int Analog_X_AVG = 0; // average joystic value 
int Analog_Z_AVG = 0;

// C  spindle for accellstepper remove for fastaccelstepper 
const int CspindleMAXspeed = 2000; // C spindle MAX speed 
const int CspindleMINspeed = 25; // C spindle MIN speed 
const int CstartStopPin = 34; // C start stop toggle pin input
int CdefaultSpeed = 500;  // C spindle default speed, update speed in CspindleSetSpeed();
bool Crunning = false; // // Current state of the motor
bool updateLCD = false;  // C spindle update display at button press
bool limitSwitchButtonUpdateLCD = false;  // C spindle update display awhen limit switch is triggered
AccelStepper stepperC(1, 33, 25); // C spindle, STEP pin   , DIR pin 
ezButton incSpeedButton(32); // C spindle pin increase speed
ezButton decSpeedButton(35); // C spindle pin decrease speed
ezButton startStopButton(CstartStopPin); // C spindle Combined start/stop button on joystic push

// C spindle define limit switch pin to stop C spindle fast 
const int limitSwitchButtonPIN = 5;


// Define LCD timeout values
const unsigned long MENU_TIMEOUT= 1500; // Menu time out value WAS 3 seconds
unsigned long MENU_lastButtonPressMillis; // Menu reference value 
const unsigned long LCD_refreshrate = 10; // LCD refresh value WAS 100 mS
unsigned long LCD_lastButtonPressMillis; // LCD reference value 
int menuIndex = 0; // Define menu variables start at mainmenu = 0 , Submenu1 = 1
// char Xbuf[16]; //16 char long on a 16 display buffer for printing on the LCD
// char Zbuf[16]; //buffer for printing on the LCD
float XbufCall; // WWAS double
float ZbufCall;

// pre declarations 
void InitialValues();  // calibration for X & Z axis analog joystic 
void ReadAnalog();  // for X & Z axis read analog joystic 
void CspindleOnOFF(); // for C spindle, toggles spindle on or off 
void CspindleSetSpeed(); // for C spindle set speed wwhen spindle is off 


//////  CORE 0 //////  C SPINDLE TASK loop , no main loop is used. 
void CSpindleTaskLoop(void *pvParameter) { // WAS parameter
      // code from main loop here !!!

	while (1) { 

   //  PUT display here 
  // upate LCD if millis is larger than refreshrate write function here for corect distance for ballscrew movement.
  while (millis() - LCD_lastButtonPressMillis > LCD_refreshrate ){ // MUST HAVE THIS ON WOKWI
    
    // take semaphore ot makseure no  one writes when reading to same 
  xSemaphoreTake(Semaphore, portMAX_DELAY);

   //snprintf(Xbuf, 16, "%.2f", XbufCall );  // trying string to print to format , but slower 
   //lcd.printstr(Xbuf); // 200 steps / rev , ballscrew 4mm pitch / rev . 4/200 = 0,02
  lcd.setCursor(2, 0);
  lcd.print(XbufCall);
  lcd.setCursor(2, 1);
  lcd.print(ZbufCall); 
   
    xSemaphoreGive(Semaphore);

    LCD_lastButtonPressMillis = millis(); // reset value when end of while loop
  }


    vTaskDelay(pdMS_TO_TICKS(1)); // inside while loop,  WAS 5
	}


} // END of CORE 0  CSpindleTaskLoop



//// CORE 1 ////  XZ joystick TASK loop , no main loop is used 
void XZjoystickTaskLoop(void *pvParameter) { // WAS parameter
	while (1) { 
      // code from main loop here !!!
  resetXButton.loop(); // must call X button loop to check if button is pressed
  resetZButton.loop(); // must call Z button loop to check if button is pressed
  incSpeedButton.loop(); // must call loop to check if button is pressed
  decSpeedButton.loop(); // must call loop to check if button is pressed
  startStopButton.loop(); // must call loop to check if button is pressed

  CspindleOnOFF();  // for C spindle, toggles spindle on or off
  
  if (Crunning == false && (incSpeedButton.isPressed() || decSpeedButton.isPressed())){
      CspindleSetSpeed(); // this sets C spindle speed 
    } // END of C spindle set speed   


    // Check if X axis reset button is pressed if so update LCD with 0 value on all used columns
  if (resetXButton.isPressed())  {  // resetXButton display updates to 0 value travel
   xSemaphoreTake(Semaphore, portMAX_DELAY);
    lcd.setCursor(2, 0);
    lcd.print("        "); // erase old numbers 
   xSemaphoreGive(Semaphore);
    stepperX.setCurrentPosition(0); // Reset X axis position
  }

    // Check if Y axis reset button is pressed
   if (resetZButton.isPressed()) {   // resetZButton display updates to 0 value travel
   xSemaphoreTake(Semaphore, portMAX_DELAY);
   lcd.setCursor(2, 1);
    lcd.print("        "); // erase old numbers 
    xSemaphoreGive(Semaphore);
    stepperZ.setCurrentPosition(0); // Reset Z axis position
  }



  

  ReadAnalog(); // this runs X & Z axis stepper  

//   xSemaphoreTake(Semaphore, portMAX_DELAY); // placeholders take handle to updated shared resources 
//   xSemaphoreGive(Semaphore); // placeholder give handle after updated shared resources 

   //  vTaskDelay(pdMS_TO_TICKS(1)); // time spent in blocked state inside while loop,  WAS 1


  }


//vTaskDelay(pdMS_TO_TICKS(10)); // inside while loop,  WAS 5

} // END of  XZjoystickTaskLoop


void setup() {
  Serial.begin(115200);
  Serial.println("\nVINCES Lathe\n");

  // LCD initialization
  lcd.init();
  lcd.backlight();
  delay(1000);

  //C spindle
  incSpeedButton.setDebounceTime(30); //was 50 
  decSpeedButton.setDebounceTime(30); 
  startStopButton.setDebounceTime(90);  // was 90
  stepperC.setMaxSpeed(CspindleMAXspeed); // C spindle MAX speed for accelstepper
  stepperC.setAcceleration(2000); // C spindle accelstepper must have acceleration
  
  // C spindle limit switch button 
  pinMode(limitSwitchButtonPIN, INPUT_PULLUP);

  // Joystick X & Z 
  pinMode(Analog_X_pin, INPUT); // analog joystick X axis in 
  pinMode(Analog_Z_pin, INPUT);
  InitialValues(); // calibrate joystic values 
  resetXButton.setDebounceTime(15); //was 50 EZ button debounce for X reading 
  resetZButton.setDebounceTime(15);
  delay(3000);
  stepperX.setMaxSpeed(20000); // was 1200 
  stepperZ.setMaxSpeed(20000);
  stepperX.setAcceleration(50000); // was 2000 Acceleration, this may not be needed ? 
  stepperZ.setAcceleration(50000);
  
  // LCD print ready message 
  lcd.setCursor(0, 0);
  lcd.print("  VINCES Lathe");
  lcd.setCursor(0, 1);
  lcd.print("Ready to Rumble");
  delay(5000);
  lcd.clear();

  // LCD print static start data for C & X & Z axis, to make LCD update faster 
  lcd.setCursor(0, 0);
  lcd.print("X ");
  lcd.setCursor(0, 1);
  lcd.print("Z ");
  lcd.setCursor(10, 0);
  lcd.print("C OFF ");
  lcd.setCursor(10, 1);
  lcd.print("C ");
  lcd.print(CdefaultSpeed);

// Simple flag, up or down
	Semaphore = xSemaphoreCreateMutex();

// set up Free RTOS core 0 here 
  	xTaskCreatePinnedToCore(
			CSpindleTaskLoop, /* Function to implement the task */
			"CSpindleTask", /* Name of the task */
			8000, /* Stack size in words WAS 5000*/ 
			NULL, /* Task input parameter */
			0, /* Priority of the task */ // 
			&CSpindleTask, /* Task handle. WAS NULL */
			0); /* Core where the task should run */


// set up Free RTOS core 1 here 
  	xTaskCreatePinnedToCore(
			XZjoystickTaskLoop, /* Function to implement the task */
			"XZjoystickTask", /* Name of the task */
			20000, /* Stack size in words WAS 20000*/
			NULL, /* Task input parameter */
			0, /* Priority of the task */ // 
			&XZjoystickTask, /* Task handle. WAS NULL */
			1); /* Core where the task should run */


// Start the scheduler
vTaskStartScheduler();

}// END of setup

void loop() {

 vTaskDelete(NULL);  // this is needed for core 1 function 

}   // END of MAIN loop

// for C spindle, toggles spindle OnOff 
void CspindleOnOFF(){
  
  if (startStopButton.isPressed() ) {
    Crunning = !Crunning; // toggling on off 
    stepperC.setSpeed(CdefaultSpeed);
    limitSwitchButtonUpdateLCD = false; // toggle alarm switch 
    
    
    if (Crunning == true) { 

    xSemaphoreTake(Semaphore, portMAX_DELAY);

    lcd.setCursor(10, 0);
    lcd.print("C ON  ");
    
    xSemaphoreGive(Semaphore);

    // remove all serial P
    Serial.println("\nC Spindle ON\n");
    }
    // C spindle stop 
    else if (Crunning == false ) { 

    xSemaphoreTake(Semaphore, portMAX_DELAY);

    lcd.setCursor(10, 0);
    lcd.print("C OFF ");

     xSemaphoreGive(Semaphore);

    //   // remove all serial P
    Serial.println("\nC Spindle OFF\n");
    } 


  }
  

  // C spindle start 
   if (Crunning == true  && (digitalRead(limitSwitchButtonPIN) == HIGH) ) { 
    stepperC.runSpeed(); // C spindle run motor forever
    limitSwitchButtonUpdateLCD = false;
   

  }
  // C spindle stop 
 if (Crunning == true && (digitalRead(limitSwitchButtonPIN) == LOW )) { 
    stepperC.stop();
    //Crunning = false; // double write ??
  }

  // C spindle stop 
  else if (Crunning == false || (digitalRead(limitSwitchButtonPIN) == LOW )) { 
    stepperC.stop();
    Crunning = false; // double write ??
  }



  // this prints Alarm when switch is LOW  
  if ((limitSwitchButtonUpdateLCD == false && (digitalRead(limitSwitchButtonPIN) == LOW ) )){
   xSemaphoreTake(Semaphore, portMAX_DELAY);

    lcd.setCursor(10, 0);
    lcd.print("ALARM");

     xSemaphoreGive(Semaphore);
    // remove all serial P
    Serial.println("\nC Spindle ALARM\n");
    limitSwitchButtonUpdateLCD = true; // only write once  ! 
  }
  // this prints OFF when comming from Alarm state
  if ((limitSwitchButtonUpdateLCD == true && (digitalRead(limitSwitchButtonPIN) == HIGH ) )){
    
       xSemaphoreTake(Semaphore, portMAX_DELAY);


    lcd.setCursor(10, 0);
    lcd.print("      ");
    lcd.setCursor(10, 0);
    lcd.print("C OFF ");

     xSemaphoreGive(Semaphore);
    // remove all serial P
    Serial.println("\nC Spindle OFF from ALARM state\n");
  }

} // END of void CspindleOnOFF()

// for C spindle set speed when spindle is off 
void CspindleSetSpeed() {
     
  if (incSpeedButton.isPressed()) {
    if (CdefaultSpeed < CspindleMAXspeed) {
      CdefaultSpeed += 75;
      // remove all serial P
      Serial.println("incSpeedButton pressed\n");
      Serial.println(CdefaultSpeed);
    }
    // can not exceed CspindleMAXspeed value
    if (CdefaultSpeed > CspindleMAXspeed){ 
      CdefaultSpeed = CspindleMAXspeed;
    }
    updateLCD = true; // write new number to display 
  }
  else if (decSpeedButton.isPressed() ) {
    if (CdefaultSpeed > CspindleMINspeed) {
      CdefaultSpeed -= 25;
      // remove all serial P
      Serial.println("decSpeedButton pressed\n");
      Serial.println(CdefaultSpeed);
      updateLCD = true;  // write new number to display 
    }
  }

    // write C spindle speed number to display 
    if (updateLCD) {
      updateLCD = false; // toggle to false so display does not update again 

     xSemaphoreTake(Semaphore, portMAX_DELAY);

      lcd.setCursor(10, 1);
      lcd.print("C     "); // clear data with spaces
      lcd.setCursor(12, 1);
      lcd.print(CdefaultSpeed);

    xSemaphoreGive(Semaphore);


    }
} // END of void CspindleSetSpeed()

// read X & Z joystick and update LCD with axis movements
void ReadAnalog() {
  Analog_X = analogRead(Analog_X_pin);
  Analog_Z = analogRead(Analog_Z_pin);

  if (abs(Analog_X - Analog_X_AVG) > 350) { // set threshold value starting with 1
    stepperX.setSpeed(0.6 * (Analog_X - Analog_X_AVG)); // WAS 5 *  // check what value get printed to speed
    XbufCall = (stepperX.currentPosition() * 0.001 ); // check ballscrew stepper motor and driver settings
  } 
    else {
    stepperX.setSpeed(0);
  }
  if (abs(Analog_Z - Analog_Z_AVG) > 350) {
    stepperZ.setSpeed(0.6 * (Analog_Z - Analog_Z_AVG)); // WAS 5 *
    ZbufCall = (stepperZ.currentPosition() * 0.00125 );
  } 
    else {
    stepperZ.setSpeed(0);
  }


  // upate LCD if millis is larger than refreshrate write function here for corect distance for ballscrew movement.
  while (millis() - LCD_lastButtonPressMillis > LCD_refreshrate ){
 

  //  xSemaphoreTake(Semaphore, portMAX_DELAY);

// X void here with buffer , call it in core 0
    // snprintf(buf, 8, "%d", stepperMaxSpeed); //the decimal value of the variable is in the buf
    // lcd.printStr(10, 2, buf); //set the cursor to 2nd line 10th pixel and print the buffer
 
   //XbufCall = snprintf(Xbuf, 16, "%f", (stepperX.currentPosition() * 0.02 ));
   // XbufCall = snprintf(Xbuf, 16, "%f", (stepperX.currentPosition() * 0.02 ) );
   // XbufCall = (stepperX.currentPosition() * 0.001 ); // check ballscrew stepper motor and driver settings
   // ZbufCall = (stepperZ.currentPosition() * 0.00125 );
  
    // snprintf(Zbuf, 16, "%d", (stepperZ.currentPosition() * 0.025 ));
    
   // snprintf(Xbuf, 16, "%.2f", (stepperZ.currentPosition() * 0.025 ) ); //the decimal value of the variable is in the buf
   // lcd.setCursor(2, 0);
   // lcd.printstr(Xbuf); //set the cursor to 2nd line 10th pixel and print the buffer
   // Serial.print("xbuffcall: ");
   // Serial.println((XbufCall));
  

   //  lcd.setCursor(2, 0); // update lcd on core 0 
   //  lcd.print(stepperX.currentPosition() * 0.02 ); // 200 steps / rev , ballscrew 4mm pitch / rev . 4/200 = 0,02
   //  lcd.setCursor(2, 1);
   //  lcd.print(stepperZ.currentPosition() * 0.025 ); // 200 steps / rev , ballscrew 5mm pitch / rev . 5/200 = 0,025
   
  //  xSemaphoreGive(Semaphore);

    LCD_lastButtonPressMillis = millis(); // reset value when end of while loop
  }
stepperX.runSpeed(); // this runs X axis stepper 
stepperZ.runSpeed();  // this runs Z axis stepper 
} // End of  void ReadAnalog()

// //this shall be in CORE 0 
// void XLCDupdate(){


// lcd.setCursor(2.0);
// lcd.print(Xbuff);


// }

// X & Z axis joystic calibrationthis only run once at setup 
void InitialValues() {



  lcd.setCursor(0, 0);
  lcd.print("  Do not touch  ");
  lcd.setCursor(0, 1);
  lcd.print("    Joystic     ");

  // Set the values to zero before averaging
  float tempX = 0;
  float tempZ = 0;

  // Read the analog 50 times, then calculate an average.
  for (int i = 0; i < 50; i++) {
    tempX += analogRead(Analog_X_pin);
    tempZ += analogRead(Analog_Z_pin);
  }

  Analog_X_AVG = tempX / 50;
  Analog_Z_AVG = tempZ / 50;
  delay(2000);
  // print ingo to serial remove before production
  Serial.print("AVG_X: ");
  Serial.println(Analog_X_AVG);
  Serial.print("AVG_Z: ");
  Serial.println(Analog_Z_AVG);
  Serial.println("Calibration finished");

  lcd.setCursor(0, 0);
  lcd.print("    Joystick    ");
  lcd.setCursor(0, 1);
  lcd.print("   Calibrated   ");
} // END of void InitialValues