/*
  Forum: https://forum.arduino.cc/t/while-loop-not-executing-the-second-time/1159017
  Wokwi: https://wokwi.com/projects/373323514455873537

*/

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

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

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();
  lcd.print("Start");
  // myservo.write() range from 0-180
  // the value controls the angular position of the servo
  Serial.begin(9600);
  Serial.println("Start");
  myservo.attach(9, 544, 2500);

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

void loop() {
  stateMachine();
}



void stateMachine() {
  switch (mode) {
    case 0: // Get Input Mode
      getInputMode();
      break;
    case 1: // 1: Manual
      handleRotaryEncoder();
      if (buttonReleased()) {
        Serial.println("From Manual to Select Mode");
        myservo.write(0);
        set_mode(0);
      }
      break;
    case 2 :
      getPosData();
      break;
    case 3: // 2: Auto
      if (buttonReleased()) {
        Serial.println("From Auto to Select Mode");
        myservo.write(0);
        set_mode(0);
      }
      break;
    default:
      mode = 0;
      break;
  }
}

void getInputMode() {
  static char buf[4];
  static int count = 0;
  char c;
  digitalWrite(LED_BUILTIN, LOW);
  // Serial.println("mode?");
  if (Serial.available()) {
    c = Serial.read();
    if (c >= '0' && c <= '9') {
      buf[count] = c;
      count++;
      buf[count] = '\0'; // End of string array
    }
    if (c == '\n') {
      if (count > 0) {
        mode = buf[0] - 48;
        set_mode(mode);
      }
      count = 0;
    }
  }
  switch (mode) {
    case 1 :  // Prepare mode 1
      Serial.println("Switch to Manual Mode");
      manual_pos = 0;
      digitalWrite(LED_BUILTIN, HIGH);
      break;
    case 2 :  // Prepare mode 3
      Serial.println("Get Data for Auto Mode");
      lcd_print(0, 1, "Input 33 .. 56", false);
      break;
    default: // Do Nothing
      break;

  }
}

void lcd_print(int x, int y, char * txt, bool clr) {
  if (clr) {
    lcd.clear();
  }
  lcd.setCursor(x, y);
  lcd.print(txt);
}

void lcd_print(int x, int y,  int pos) {
  char buff[8];
  sprintf(buff, "%3d", pos);
  lcd.setCursor(x, y);
  lcd.print(buff);
}

void set_mode(int newMode) {
  mode = newMode;
  lcd_print(0, 0, "Mode: ", true);
  lcd_print(8, 0, mode);
}



void handleRotaryEncoder() {
  static int lastClk = HIGH;
  int newClk = digitalRead(CLK);
  int old_pos = manual_pos;
  if (newClk != lastClk) {
    // There was a change on the CLK pin
    lastClk = newClk;
    int dtValue = digitalRead(DT);
    if (newClk == LOW && dtValue == HIGH) {
      manual_pos++;
      if (manual_pos > 180) {
        manual_pos = 180;
      }
    }
    if (newClk == LOW && dtValue == LOW) {
      manual_pos--;
      if (manual_pos < 0) {
        manual_pos = 0;
      }
    }
  }
  if (manual_pos != old_pos) {
    myservo.write(manual_pos);
    old_pos = manual_pos;
    lcd_print(0, 1, "pos = ", false);
    lcd_print(6, 1, manual_pos);
  }
}


boolean buttonReleased() {
  static unsigned long lastButtonPress = 0;
  static boolean buttonPressed = true;
  static byte lastState   = HIGH;
  byte recentState = digitalRead(SW);
  if (recentState != lastState) {
    // Remember last button press event
    lastButtonPress = millis();
    lastState = recentState;
  }
  if (millis() - lastButtonPress > 30 && lastState != buttonPressed) {
    buttonPressed = lastState;
    if (buttonPressed) {
      return true;
    }
  } else {
    return false;
  }
}

void getPosData() {
  static char buf[4];
  static int count = 0;
  char c;
  if (Serial.available()) {
    c = Serial.read();
    if (c >= '0' && c <= '9') {
      buf[count] = c;
      count++;
      buf[count] = '\0'; // End of string array
    }
    if (c == '\n' || count > 2) {
      int v = atoi(buf);
      if (v >= 33 && v <= 56) {
        set_mode(3);
        lcd_print(0, 1, "NewPos =        ", false);
        lcd_print(9, 1, buf, false);
        pos = v;
        //pos = (pos - 33) * 7.6433;  ???
        myservo.write(pos);
      }
      if (v == 0) {
        myservo.write(0);
        set_mode(0);
      }; // Return to Input Mode
      count = 0;
    }
  }
}