#define DEBUG 1 // Debug mode ON = 1
// #define PI 3.14159265
#if DEBUG == 1
    #define Debug(x) Serial.print(x)
    #define Debugln(x) Serial.println(x)
#else
    #define Debug(x) 
    #define Debugln(x) 
#endif


int RPM = 100;           //  motor shaft RPM (Revolution / minute) 
const int StepsPerRevolution = 200;
int speed = RPM * StepsPerRevolution/60;  //  steps / second
const int acceleration = 5000;  // Sets the acceleration in steps/s²
int steps = 0;
int Task = 1;            // 1 = Set Speed To Position; 2 = Move To Position; 3 = Spin Continuously 
const int Debounce = 50;    // kapcsolók lekérdezési frekvenciája
int DebCounter = 0;


#include <ESP_FlexyStepper.h>

ESP_FlexyStepper stepper1;
// ESP_FlexyStepper stepper2; 
// float stepmax = 600;



#include <ContinuousStepper.h> // https://github.com/bblanchon/ArduinoContinuousStepper

const uint8_t stepPin = 1;
const uint8_t dirPin = 2;
const uint8_t enaPin =  3;

ContinuousStepper<StepperDriver> stepper;


#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);


//Rotary Encoder Button
#include "AiEsp32RotaryEncoder.h"
      /*
      connecting Rotary encoder
      Rotary encoder side    MICROCONTROLLER side  
      -------------------    ---------------------------------------------------------------------
      CLK (A pin)            any microcontroler intput pin with interrupt -> in this example pin 32
      DT (B pin)             any microcontroler intput pin with interrupt -> in this example pin 21
      SW (button pin)        any microcontroler intput pin with interrupt -> in this example pin 25
      GND - to microcontroler GND
      VCC                    microcontroler VCC (then set ROTARY_ENCODER_VCC_PIN -1) 
      ***OR in case VCC pin is not free you can cheat and connect:***
      VCC                    any microcontroler output pin - but set also ROTARY_ENCODER_VCC_PIN 25 
                              in this example pin 25
      */

#define ROTARY_ENCODER_A_PIN 7
#define ROTARY_ENCODER_B_PIN 8
#define ROTARY_ENCODER_BUTTON_PIN 9
#define ROTARY_ENCODER_VCC_PIN -1 /* 27 put -1 of Rotary encoder Vcc is connected directly to 3,3V; else you can use declared output pin for powering rotary encoder */

      //depending on your encoder - try 1,2 or 4 to get expected behaviour
      //#define ROTARY_ENCODER_STEPS 1
      //#define ROTARY_ENCODER_STEPS 2
#define ROTARY_ENCODER_STEPS 4

AiEsp32RotaryEncoder rotaryEncoder = AiEsp32RotaryEncoder(ROTARY_ENCODER_A_PIN, ROTARY_ENCODER_B_PIN, ROTARY_ENCODER_BUTTON_PIN, ROTARY_ENCODER_VCC_PIN, ROTARY_ENCODER_STEPS);
bool circleValues = false;  // circleValues true|false (when encoder max go to min and vice versa)
        
      void IRAM_ATTR readEncoderISR()
      {
        rotaryEncoder.readEncoder_ISR();
      }
//ENCODER Header END


void setup() {
// Initialize LCD
  lcd.init();
  lcd.backlight();   
  lcd.setCursor(0, 0);
  lcd.print("PIZZA FLEET 2024"); 
  lcd.setCursor(0, 1);
  lcd.print("pizzafleet.com");

  stepper1.connectToPins(1, 2);   // ESP_FlexyStepper
  //stepper2.connectToPins(23, 22);
  stepper1.setSpeedInStepsPerSecond(speed);
  stepper1.setAccelerationInStepsPerSecondPerSecond(800);
  stepper1.setDecelerationInStepsPerSecondPerSecond(800);
  //  stepper2.setSpeedInStepsPerSecond(60);

  stepper1.startAsService(1);

  pinMode(enaPin,OUTPUT);  
  stepper.begin(stepPin, dirPin);  // ContinuousStepper
  stepper.spin(speed);   // rotate at X steps per seconds
  digitalWrite(enaPin, LOW); 

  Serial.begin(115200);

//we must initialize rotary encoder
  rotaryEncoder.begin();
  rotaryEncoder.setup(readEncoderISR);
  //set boundaries and if values should cycle or not
  //in this example we will set possible values between 0 and 1000;
  //rotaryEncoder.setBoundaries(-1000, 1000, circleValues); //minValue, maxValue, circleValues true|false (when max go to min and vice versa)

  /*Rotary acceleration introduced 25.2.2021.
   * in case range to select is huge, for example - select a value between 0 and 1000 and we want 785
   * without accelerateion you need long time to get to that number
   * Using acceleration, faster you turn, faster will the value raise.
   * For fine tuning slow down.
   */
  //rotaryEncoder.disableAcceleration(); //acceleration is now enabled by default - disable if you dont need it
  rotaryEncoder.setAcceleration(100); //or set the value - larger number = more accelearation; 0 or 1 means disabled acceleration
  rotaryEncoder.reset(RPM*-1);




  Serial.println("");
  Serial.println("Hello, XIAO ESP32-S3!");
  Serial.println("PIZZA FLEET 2024 StepperTester");
    Serial.print("DEBUG = ");
    Serial.println(DEBUG);

  delay(1000);
  lcd.clear();
  lcd.print("SET SPEED TO POS");
  lcd.setCursor(0, 1);
  lcd.print("RPM: ");
  lcd.setCursor(7, 1);
  lcd.print(RPM);
  lcd.print("   ");
}


void rotary_loop()
{
  //dont do anything unless value changed
  if (rotaryEncoder.encoderChanged())
  {
    if (Task == 1) {
    // Set speed to position
      RPM = rotaryEncoder.readEncoder()*-1;
      
      lcd.setCursor(0, 1);
      lcd.print("RPM: ");
      lcd.setCursor(7, 1);
      lcd.print(RPM);
      lcd.print("   ");
    }  

    if (Task == 2) {
      // Set postion in steps
      steps = rotaryEncoder.readEncoder();

      lcd.setCursor(0, 1);      
      lcd.print("STEPS: ");
      lcd.setCursor(7, 1);
      lcd.print(steps*-1);
      lcd.print("   ");    
    }

    if (Task == 3) {
      RPM = rotaryEncoder.readEncoder()*-1;
      speed = RPM * StepsPerRevolution/60;
      stepper.spin(speed); // hujja
   
      lcd.setCursor(0, 0);
      lcd.print("RPM:       ");
      lcd.setCursor(7, 0);
      lcd.print(RPM);
      lcd.print("        ");
      lcd.setCursor(0, 1);
      lcd.print(speed); 
      lcd.print(" steps/sec    ");
    
      Serial.println(RPM);
    }
  }
  if (rotaryEncoder.isEncoderButtonClicked())
  {
    // ide jöhetnek funkciok az encoder nyomo gombjára
      if (Task == 1) {
        stepper1.setSpeedInStepsPerSecond(speed);
        rotaryEncoder.reset(0);
  
        lcd.setCursor(0, 0);
        lcd.print("MOVE TO POSITION");
        lcd.setCursor(0, 1);
        lcd.print("STEPS: ");
        lcd.setCursor(7, 1);
        lcd.print(steps);
        lcd.print("   ");


      }

      if (Task == 2) {
        rotaryEncoder.reset(0);
        RPM = 0;
        steps = 0;
        speed = RPM * StepsPerRevolution/60;
        stepper.spin(speed);

        lcd.setCursor(0, 0);
        lcd.print("RPM:                ");
        lcd.setCursor(7, 0);
        lcd.print(RPM);
        lcd.print("   ");
        lcd.setCursor(0, 1);
        lcd.print(speed); 
        lcd.print(" steps/sec    ");
    
       Serial.println(RPM);


      }

    if (Task == 3) {
      stepper.stop();
      RPM = abs(RPM);
      rotaryEncoder.reset(RPM*-1);

      lcd.clear();
      lcd.print("SET SPEED TO POS");
      lcd.setCursor(0, 1);
      lcd.print("RPM: ");
      lcd.setCursor(7, 1);
      lcd.print(RPM);
      lcd.print("   ");
    
      Serial.println(RPM);

      Task = 0;
    }

    Task ++;

    Serial.print("Task: ");
    Serial.println(Task);

  }
}


void loop() {
  /*
  if (Task == 1) {
   // Set speed to position
  }  
  */
  
  if (Task == 2) {
   // Move to position
   
   stepper1.setTargetPositionInSteps(steps);
  }  

  if (Task == 3) {
    stepper.loop();
  }
  
  DebCounter ++;       //Gomb feldolgozás eleje
  if (DebCounter > Debounce) {  
    rotary_loop();
    DebCounter = 0;
  }
}