#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "common.h"

#define WOKWI

#define MAX_BUTTON 4
#define BUTTON_BOUNCING 300
#define NUM_MENU 2
#define NUM_MANUAL_MENU 5
#define NUM_AUTO_MENU 3

LiquidCrystal_I2C lcd(LCD_ADDRESS, NUM_CHAR, NUM_ROW);

bool ledon_flag = false;
long led_time;
int button_prev[MAX_BUTTON];
long button_time[MAX_BUTTON];
short iMainMenu = 0;
short iSubMenu = -1;
short iAutoMenu = 0;
short iSelectedAutoMenu = -1;
short iSelectedManualMenu = -1;
String menu[] = {"AUTOMATIC", "MANUAL"};
String manual_menu[] = {"MOTOR ON/OFF",
                        "HEATER1 ON/OFF",
                        "HEATER2 ON/OFF",
                        "HEATER3 ON/OFF",
                        "READ CALIPER"
                       };
String auto_menu[] = {"START", "SET TEMP", "SET SPEED"};

bool bProcessStart = false;
int speed_percent = 10;       // start 10% speed
int temp_degree = 30;
bool bManualMotorOn = false;

void clearLcdLine(int _lineid)
{
  String writestr = "";
  for (int i = 0; i < NUM_CHAR; i++)
  {
    writestr += " ";
  } // for (int i=0; i < NUM_CHAR; i++)
  lcd.setCursor(0, _lineid);
  lcd.print(writestr);
} // void clearLcdLine(int _lineid)

void writeLcdString(int _lineid, int _cursor, String s)
{
  lcd.setCursor(_cursor, _lineid);
  lcd.print(s);
} // void writeLcdString(String s, int _lineid, short _alignment = 0 /* 0:left, 1:mid, 2:right */)


void writeLcdString(String s, int _lineid, short _alignment = 0 /* 0:left, 1:mid, 2:right */)
{
  int len = s.length();
  int space_len = NUM_CHAR - len;
  String writestr = "";
  switch (_alignment)
  {
    case 0:
      writestr = s;
      padString(writestr.c_str());
      break;
    case 1:
      space_len = space_len / 2;
      for (int i = 0; i < space_len; i++)
      {
        writestr += " ";
      } // for (int i=0; i < space_len; i++)
      writestr += s;
      writestr = padString(writestr.c_str());
      // String debugstr = writestr;
      // debugstr.replace(" ", ".");
      // Serial.println(String(space_len) + debugstr);
      break;
  }
  lcd.setCursor(0, _lineid);
  lcd.print(writestr);
} // void writeLcdString(String s, int _lineid, short _alignment = 0 /* 0:left, 1:mid, 2:right */)

void setup()
{
  Serial.begin(115200);

  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  driveMotor(0, 0);

  pinMode(MAIN_MENU_BUTTON, INPUT_PULLUP);
  pinMode(ENTER_BUTTON, INPUT_PULLUP);
  pinMode(UP_BUTTON, INPUT_PULLUP);
  pinMode(DN_BUTTON, INPUT_PULLUP);

  button_prev[0] = digitalRead(MAIN_MENU_BUTTON);
  button_prev[1] = digitalRead(ENTER_BUTTON);
  button_prev[2] = digitalRead(UP_BUTTON);
  button_prev[3] = digitalRead(DN_BUTTON);


  for (int i = 0; i < MAX_BUTTON; i++)
  {
    // button_action[i] = false;
    button_time[i] = millis();
  }

  pinMode(LED_BUILTIN, OUTPUT);

#ifdef WOKWI
  lcd.begin(NUM_CHAR, NUM_ROW, LCD_5x8DOTS);
#else
  lcd.begin();
#endif // WOKWI
  lcd.backlight();

  writeLcdString(MACHINE_NAME, 0, CENTER_ALIGNMENT);
  // automatic mode at startup
  writeLcdString(menu[iMainMenu], 1, CENTER_ALIGNMENT);
  writeLcdString(auto_menu[iAutoMenu], 2, CENTER_ALIGNMENT);
  led_time = millis();
}

void loop()
{
  checkLedOn();
  checkButton();
  if (bProcessStart) {
    checkProcessCondition();
  } // if (bProcessStart)
}

void checkLedOn()
{
  if ((millis() - led_time) >= 500)
  {
    ledon_flag = !ledon_flag;
    digitalWrite(LED_BUILTIN, ledon_flag ? HIGH : LOW);
    led_time = millis();
  }
} // void checkLedOn()

void performButton(int _buttonid)
{
  switch (_buttonid)
  {
    case 0: // menu
      if (iSelectedAutoMenu > -1)
      {
        iSelectedAutoMenu = -1;
      } // if (iSelectedAutoMenu > -1)
      else if (iSelectedManualMenu > -1) {
        iSelectedManualMenu = -1;
      } // else if (iSelectedManualMenu > -1)
      else {
        iMainMenu++;
      }
      if (iMainMenu == NUM_MENU)
      {
        iMainMenu = 0;
        iAutoMenu = 0;
        iSubMenu = -1;
      } // if (iMainMenu == NUM_MENU)
      if ((iMainMenu == 1) && (iSubMenu == -1))
        iSubMenu = 0;
      if (iMainMenu < NUM_MENU)
        writeLcdString(menu[iMainMenu], 1, CENTER_ALIGNMENT);

      switch (iMainMenu)
      {
        case 0:
          writeLcdString(auto_menu[iAutoMenu], 2, CENTER_ALIGNMENT);
          clearLcdLine(3);
          break;
        case 1:
          writeLcdString(manual_menu[iSubMenu], 2, CENTER_ALIGNMENT);
          writeLcdString(ENTER_TOGGLE, 3, CENTER_ALIGNMENT);
          break;
      } // switch (iMainMenu)
      break;
    case 1: // enter
      switch (iMainMenu)
      {
        case 0: // Automatic
          iSelectedAutoMenu = iAutoMenu;
          switch (iSelectedAutoMenu) {
            case 0: // START
              startProcess(true);
              break;
            case 1: // set temp
              clearLcdLine(2);
              writeLcdString("     TEMP:    C", 2, LEFT_ALIGNMENT);
              displayTemp();
              break;
            case 2: // set speed
              // clearLcdLine(2);
              // writeLcdString("    SPEED:     %", 2, LEFT_ALIGNMENT);
              displaySpeed(2);
              break;
          }
          break;
        case 1: // Manual
          iSelectedManualMenu = iSubMenu;
          switch (iSelectedManualMenu) {
            case 0:
              bManualMotorOn = !bManualMotorOn;
              driveMotor(bManualMotorOn ? speed_percent : 0, 0);
              writeLcdString(bManualMotorOn ? "MOTOR ON" : "MOTOR OFF", 3, CENTER_ALIGNMENT);
              break;
          }
          break;
      } // switch (iSelectedManualMenu)
      break;
    case 2: // up
      switch (iMainMenu)
      {
        case 0:
          switch (iSelectedAutoMenu) {
            case -1:
              iAutoMenu--;
              if (iAutoMenu < 0) iAutoMenu = (NUM_AUTO_MENU - 1);
              writeLcdString(auto_menu[iAutoMenu], 2, CENTER_ALIGNMENT);
              break;
            case 1: // temp
              if (temp_degree < MAX_TEMP) {
                temp_degree++;
                displayTemp();
              } // if (temp_degree < MAX_TEMP)
              break;
            case 2: // speed
              if (speed_percent < MAX_SPEED) {
                speed_percent++;
                displaySpeed(2);
              } // if (speed_percent < MAX_SPEED)
              break;
          } // switch (iMainMenu)
          break;
        case 1:
          if (bManualMotorOn)
          {
            displaySpeed(3);
          } else {
            iSubMenu--;
            if (iSubMenu < 0) iSubMenu = (NUM_MANUAL_MENU - 1);
            writeLcdString(manual_menu[iSubMenu], 2, CENTER_ALIGNMENT);
          }
          break;
      } // switch (iMainMenu)
      break;
    case 3:
      switch (iMainMenu)
      {
        case 0:
          switch (iSelectedAutoMenu)
          {
            case -1:
              iAutoMenu++;
              if (iAutoMenu == NUM_AUTO_MENU) iAutoMenu = 0;
              writeLcdString(auto_menu[iAutoMenu], 2, CENTER_ALIGNMENT);
              break;
            case 1:
              if (temp_degree > MIN_TEMP) {
                temp_degree--;
                displayTemp();
              } // if (temp_degree > MIN_TEMP)
            case 2:
              if (speed_percent > MIN_SPEED) {
                speed_percent--;
                displaySpeed(2);
              } // if (speed_percent > MIN_SPEED)
              break;
          } // switch (iSelectedAutoMenu)
          break;
        case 1:
          iSubMenu++;
          if (iSubMenu == NUM_MANUAL_MENU) iSubMenu = 0;
          writeLcdString(manual_menu[iSubMenu], 2, CENTER_ALIGNMENT);
          break;
      } // switch (iMainMenu)
      break;

    default:
      break;
  } // switch (_buttonid)
  Serial.println(String(_buttonid) + "," +
                 String(iMainMenu) + "," +
                 String(iAutoMenu) + "," +
                 String(iSubMenu) + "," +
                 String(iSelectedAutoMenu));
} // void performButton(int _buttonid)

void checkButton()
{
  int b1 = digitalRead(MAIN_MENU_BUTTON);
  int b2 = digitalRead(ENTER_BUTTON);
  int b3 = digitalRead(UP_BUTTON);
  int b4 = digitalRead(DN_BUTTON);
  checkButtonOperation(b1, 0);
  checkButtonOperation(b2, 1);
  checkButtonOperation(b3, 2);
  checkButtonOperation(b4, 3);
} // void checkButton()

void checkButtonOperation(int _button_state, int _buttonid) {
  if ((_button_state == LOW) && (button_prev[_buttonid] == HIGH))
  {
    if ((millis() - button_time[_buttonid]) > BUTTON_BOUNCING)
    {
      button_prev[_buttonid] = LOW;
      button_time[_buttonid] = millis();
      performButton(_buttonid);
    }
  } // if ((_button_state == LOW) && (button_prev[_buttonid] == HIGH))
  else if ((_button_state == HIGH) && (button_prev[_buttonid] == LOW))
  {
    button_prev[_buttonid] = HIGH;
  } // else if ((_button_state == HIGH) && (button_prev[_buttonid] == LOW))
} // void checkButtonOperation(int _button_state, int _buttonid)

String padString(const char *s)
{
  String ret = String(s);
  int remain = NUM_CHAR - strlen(s);
  for (int i = 0; i < remain; i++)
    ret += " ";
  return ret;
} // String padString(const char *s)

void startProcess(bool _start) {
  bProcessStart = _start;
} // void startProcess(bool _start)

void checkProcessCondition() {
  if (bProcessStart)
  {

  } // if (bProcessStart)
  else {

  } // if (bProcessStart)
} // void checkProcessCondition()

void displaySpeed(int _line = 2) {
  clearLcdLine(_line);
  writeLcdString("    SPEED:     %", _line, LEFT_ALIGNMENT);
  String speed_str = String(speed_percent);
  int digit_remain = 3 - speed_str.length();
  for (int i = 0; i < digit_remain; i++)
    speed_str = " " + speed_str;
  writeLcdString(_line, 11, speed_str);
} // void displaySpeed(int _line=2)

void displayTemp() {
  String temp_degree_str = String(temp_degree);
  int digit_remain = 3 - temp_degree_str.length();
  for (int i = 0; i < digit_remain; i++)
    temp_degree_str = " " + temp_degree_str;
  writeLcdString(2, 11, temp_degree_str);
} // void displayTemp()

void driveMotor(int _m1, int _m2) {
  analogWrite(MOTOR_IN1, _m1);
  analogWrite(MOTOR_IN2, _m2);
} // void driveMotor(int _m1, int _m2)