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

#define CLK 2
#define DT 3
#define SW 4

int currentStateCLK;
int lastStateCLK;

Servo myservo;  // create servo object to control a servo
LiquidCrystal_I2C lcd(0x27, 16, 2);

int pos = 1;       // variable to store the servo position (In mm)
int manual_pos = 0;  // init manual position
int mode = 0;        // init mode
unsigned long lastButtonPress = 0;


void setup() {
  // Set encoder pins as inputs
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);

  lcd.init();
  lcd.backlight();

  // myservo.write() range from 0-180
  // the value controls the angular position of the servo
  Serial.begin(9600);
  Serial.println("Ready..");
  myservo.attach(9, 544, 2500);

  // initialise state of servo
  myservo.write(0);
}

void loop() {
  // get input for mode and pos
  digitalWrite(LED_BUILTIN, LOW);
  // Serial.println("mode?");
  if (Serial.available() > 0) {
    String sel = Serial.readString();
    int val = sel.toInt();
    if (val < 3) {
      mode = val;
    } else if (val >= 33 && val <= 56) {
      pos = val;
    } else {
      Serial.print("invalid value recieved:");
      Serial.println(val);
    }
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("mode:"); lcd.print(mode);
    lcd.setCursor(0, 1);
    lcd.print("pos:"); lcd.print(pos);
  }

  // Codes for manual control
  // run once when mode = 1
  if (mode == 1) {

    //lastStateCLK = digitalRead(CLK);
    digitalWrite(LED_BUILTIN, HIGH);

    // get the current state of CLK
    currentStateCLK = digitalRead(CLK);
    // If last and current state of CLK are different, then pulse occurred
    // React to only 1 state change to avoid double count
    if (currentStateCLK != lastStateCLK) {
      lastStateCLK = currentStateCLK; //remember..
      // If the DT state is different than the CLK state then
      // the encoder is rotating CW so increment
      byte data = digitalRead(DT);

      if (!data && currentStateCLK == LOW) {
        manual_pos++;
        if (manual_pos > 180)
          manual_pos = 180;
      } else if (data && currentStateCLK == LOW) {
        // Encoder is rotating CCW so decrement
        manual_pos--;
        if (manual_pos < 0)
          manual_pos = 0;
      }
    }

    // move the servo
    myservo.write(manual_pos);

    //check encoder button and exit to mode 0
    if (millis() - lastButtonPress > 50) {
      // Read the button state
      int btnState = digitalRead(SW);
      // If we detect LOW signal, button is pressed and exit manual control mode
      if (btnState == LOW) {
        // if 50ms have passed since last LOW pulse, it means that the
        // button has been pressed, released and pressed again
        Serial.println("9");
        myservo.write(0);
        mode = 0;
      }
      // Remember last button press event
      lastButtonPress = millis();
    }
  }




  if (mode == 2) {
    digitalWrite(LED_BUILTIN, HIGH);



    // exit auto mode if input is 0
    if (pos == 0) {
      myservo.write(0);
      mode = 0;
    }

    // calculate angular position of the servo, and turn it into the specified linear position
    if (pos >= 33 && pos <= 56) {
      pos = (pos - 33) * 7.6433;//what's this??
      myservo.write(pos);
    }

    // Read the button state
    if (millis() - lastButtonPress >= 50)
    {
      lastButtonPress = millis();
      int btnState = digitalRead(SW);
      // If we detect LOW signal, button is pressed and exit manual control mode
      if (btnState == LOW) {
        Serial.println("8");
        myservo.write(0);
        pos = 1;
        mode = 0;//added.. ~q
      }
    }
  }
}