#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Stepper.h>
#include <Servo.h>

const int stepsPerRevolution = 200;


Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position

// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

LiquidCrystal_I2C lcd(0x27, 20, 4);

// Rotary Encoder Connections
#define CLK_PIN 2
#define DT_PIN 3
#define SW_PIN 4


// Variables
volatile int encoderPos = 0;
int counter = 0;
int counterPressed = 0;
int counterLenght = 0;
int counterMultiplier = 0;
boolean buttonState = false;
boolean lastButtonState = false;
boolean rotationDirection = false; // false = clockwise, true = counterclockwise

int WheelD = 1;



void setup() {

    myservo.attach(6); 

  // set the speed at 60 rpm:
  myStepper.setSpeed(60);
  // initialize the serial port:
  Serial.begin(9600);

  // Rotary Encoder setup
  pinMode(CLK_PIN, INPUT_PULLUP);
  pinMode(DT_PIN, INPUT_PULLUP);
  pinMode(SW_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(CLK_PIN), updateEncoder, CHANGE);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Set Length (in):");
  lcd.setCursor(0, 1);
  lcd.print(counterLenght);

}

void loop() {

  // Read the button state
  buttonState = digitalRead(SW_PIN);

  // Check if the button has been pressed
  if (buttonState != lastButtonState) {
    if (buttonState == LOW) {
      // Button is pressed
      counterPressed++;
    }
    lastButtonState = buttonState;
  }
    Serial.print("counter Pressed:");
    Serial.println(counterPressed); 

// Read the rotary encoder position
  int newCounter = encoderPos;



    if (counterPressed == 0){ 
          Serial.println("Zero");
        // Update the counter if it has changed
        if (newCounter != counterLenght) {
          counterLenght = newCounter;


            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Set Length(in):");
            lcd.setCursor(0, 1);
            lcd.print("0");


            if(counterLenght <= -1){
            counterLenght = 0;
            encoderPos = 0;
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Can not be Neg.");
            delay(1000); // Delay to display the message
        
            }

          lcd.setCursor(0, 1);
          lcd.print("      "); // Clear the counter display
          lcd.setCursor(0, 1);
          lcd.print(counterLenght/2);
        }
        delay(500); // Delay to display the message
        Serial.println(counterLenght/2);

}



    if (counterPressed == 1){
          Serial.println("One");
          lcd.setCursor(0, 0);
          lcd.print("Set Multiplier:");

          // Update the counter if it has changed
          if (newCounter != counterMultiplier) {

            counterMultiplier = newCounter;

              if(counterMultiplier <= -1){
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Can not be Neg.");
              delay(1000); // Delay to display the message
              counterMultiplier = 0;
              encoderPos = 0;
              }

            lcd.setCursor(0, 1);
            lcd.print("      "); // Clear the counter display
            lcd.setCursor(0, 1);
            lcd.print(counterMultiplier/2);
          }
            delay(500); // Delay to display the message
            Serial.println(counterMultiplier/2);
    }


void(* resetFunc) (void) = 0;




    if (counterPressed == 2){
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Cutting");
              lcd.setCursor(0, 1);
              lcd.print(counterLenght/2);
              lcd.setCursor(6, 1);
              lcd.print(counterMultiplier/2);
          Serial.println("Cutting");
          Serial.println(counterLenght/2);

          for (int i = 1; i <= (counterMultiplier/2); i++){
          pos = 0;

          for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
            // in steps of 1 degree
            myservo.write(pos);              // tell servo to go to position in variable 'pos'
            delay(15);                       // waits 15ms for the servo to reach the position
          }

                      // step one revolution  in one direction:
          Serial.print("i:");
          Serial.println(i);
          Serial.print("Length (in):");
          Serial.println(counterLenght/2);

          int FinalLenght = counterLenght/2;
          // Lenght to Steps
          int WheelCirc = 3.14 * WheelD;
          int Angle = (FinalLenght/(2*3.14*.5))*360;
          int NumSteps = Angle/1.8;
          int Revolutions = NumSteps/stepsPerRevolution;

          Serial.print("Angle:");
          Serial.println(Angle);
          Serial.print("# OF Steps:");
          Serial.println(NumSteps);
          myStepper.step(NumSteps);
          Serial.print("# OF Cuts:");
          Serial.println(counterMultiplier/2);
          //delay(1000);

                    for (pos = 90; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
            myservo.write(pos);              // tell servo to go to position in variable 'pos'
            delay(15);                       // waits 15ms for the servo to reach the position
          }

          delay(1000);

              if(i == (counterMultiplier/2)){
                Serial.println("Stop");
                          for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
            // in steps of 1 degree
            myservo.write(pos);              // tell servo to go to position in variable 'pos'
            delay(15);                       // waits 15ms for the servo to reach the position
          }
                while(1);
                //resetFunc(); 
              }
          } 

    }

}





void updateEncoder() {
  if (digitalRead(CLK_PIN) == digitalRead(DT_PIN)) {
    encoderPos++;
  } else {
    encoderPos--;
  }
}