// 2024-01-06 Denna kod fungerar med CXZ axis EZ buttons , ingen freertos , fastaccelstepper, limit  switch
// att göra : fixa update display , timeout X & Z axis
// 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 

#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);

// 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 target_speed = 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
AccelStepper stepperC(1, 33, 25); // C spindle, STEP pin   , DIR pin 
ezButton incSpeedButton(32); // C spindle increase speed
ezButton decSpeedButton(35); // C spindle decrease speed
ezButton startStopButton(CstartStopPin); // C spindle Combined start/stop button on joystic push

// 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 = 100; // 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

// 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 
// this is for LCD timeout menu
//void MenuLogic(); // button action sets menuIndex to 1 or 0 , with timeout and refresh rate
//void showMenu();  // writes to LCD what is set in menuLogic


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(30);
  stepperC.setMaxSpeed(CspindleMAXspeed); // C spindle MAX speed for accelstepper
  stepperC.setAcceleration(2000); // C spindle accelstepper must have acceleration
  
  // 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(1200);
  stepperZ.setMaxSpeed(1200);
  stepperX.setAcceleration(2000); // Acceleration, this may not be needed ? 
  stepperZ.setAcceleration(2000);
  
  // 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(target_speed);

}

void loop() {
    //Ez buttons
  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
  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
 
     // 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
          lcd.setCursor(2, 0);
          lcd.print("        "); // erase old numbers 
          //lcd.print(target_speed);
      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
          lcd.setCursor(2, 1);
          lcd.print("        ");
          //lcd.print(target_speed);
      stepperZ.setCurrentPosition(0); // Reset Z axis position
      }

  
  

  ReadAnalog(); // this runs X & Z axis stepper  
  CspindleOnOFF();  // for C spindle, toggles spindle on or off 

if (Crunning == false && (incSpeedButton.isPressed() || decSpeedButton.isPressed())){
   CspindleSetSpeed(); // this sets C spindle speed 
    Serial.println("\nCtoggle status\n");
    Serial.println(Crunning);
  }

  // // Move thius Z & X in to timeout function then only update position in ReadAnalog() void
  //  lcd.setCursor(0, 0);
  //  lcd.print("X: ");
  //  lcd.print(stepperX.currentPosition());
  //  lcd.setCursor(0, 1);
  //  lcd.print("Z: ");
  //  lcd.print(stepperZ.currentPosition());

}   // END of MAIN loop

 // for C spindle, toggles spindle OnOff 
void CspindleOnOFF(){
 
    // C spindle running if Crunning is true else false stop 
   // must be before the button toggling of Crunning
    if (Crunning == true) {
     stepperC.runSpeed(); // C spindle run motor forever
    }
    else if (Crunning == false) { 
      stepperC.stop();
    } 
    
 //delay (50);

    // if startStopButton is pressed , falling toggle or rising toggle Crunning 
    if (startStopButton.isPressed()) {
      Crunning = !Crunning; //toggle Crunning for ON  true  OFF false
      stepperC.setSpeed(target_speed);
      lcd.setCursor(10, 0);
      lcd.print("C OFF ");
      Serial.println("\nC Spindle OFF\n");
    }

    if (startStopButton.isReleased()) {
      Crunning = !Crunning; //toggle Crunning for ON  true  OFF false
      stepperC.setSpeed(target_speed);
      lcd.setCursor(10, 0);
      lcd.print("C ON  ");
      Serial.println("\nC Spindle ON\n");
    }
} 

 // for C spindle set speed wwhen spindle is off 
void CspindleSetSpeed() {

        
  if (incSpeedButton.isPressed()) {
    if (target_speed < CspindleMAXspeed) {
              target_speed += 75;
              Serial.println("\incSpeedButton pressed\n");
              Serial.println(target_speed);
    }
    // can not exceed CspindleMAXspeed value
    if (target_speed > CspindleMAXspeed){ 
        target_speed = CspindleMAXspeed;
    }
    updateLCD = true; // write new number to display 
  }
  else if (decSpeedButton.isPressed() ) {
    if (target_speed > CspindleMINspeed) {
        target_speed -= 25;
        Serial.println("\decSpeedButton pressed\n");
        Serial.println(target_speed);
        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 
      lcd.setCursor(10, 1);
      lcd.print("C     "); // clear data with spaces
      lcd.setCursor(12, 1);
      lcd.print(target_speed);
      //updateLCD = false; 
    }


}



// read X & Z joystick 
void ReadAnalog() {
  Analog_X = analogRead(Analog_X_pin);
  Analog_Z = analogRead(Analog_Z_pin);

  if (abs(Analog_X - Analog_X_AVG) > 1) { // set threshold value starting with 1
    stepperX.setSpeed(5 * (Analog_X - Analog_X_AVG)); // WAS 5 *
  } 
    else {
    stepperX.setSpeed(0);
  }
  if (abs(Analog_Z - Analog_Z_AVG) > 1) {
    stepperZ.setSpeed(5 * (Analog_Z - Analog_Z_AVG)); // WAS 5 *
  } 
    else {
    stepperZ.setSpeed(0);
  }


  // upate LCD if millis is larger than refreshrate.
  while (millis() - LCD_lastButtonPressMillis > LCD_refreshrate ){
          
       lcd.setCursor(2, 0);
       lcd.print(stepperX.currentPosition());
       lcd.setCursor(2, 1);
       lcd.print(stepperZ.currentPosition());

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

  
stepperX.runSpeed(); // was in main loop 
stepperZ.runSpeed(); // was in main loop
} // End of  void ReadAnalog()

// this only run once at setup 
// X & Z axis joystic calibration 
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 
  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