#include <LiquidCrystal_I2C.h>

// Pin Definitions
#define PARK_PIN 17
#define DIR_PIN 4
#define STEP_PIN 16
#define RSW_PIN 5
#define LED_PIN 15
#define MSW_PIN 23
#define ENCODER_CLK_PIN 18
#define ENCODER_DT_PIN 19
#define TEMPERATURE_SENSOR_PIN 2

// LCD Object
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Constants
const float BETA = 3950;
const int MAX_TEMPERATURE = 78;
const int MAX_SPEED = 100;
const int MIN_SPEED = 0;

// Global Variables
int deg = 0;
int trg = 0;
int mode = 0;
int rspd = 50;
int spd = 105;
float celsius = 20;

void setup() {
  lcd.init();
  lcd.backlight();
  
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(MSW_PIN, INPUT_PULLUP);
  pinMode(PARK_PIN, INPUT_PULLUP);
  pinMode(RSW_PIN, INPUT_PULLUP);
  pinMode(ENCODER_CLK_PIN, INPUT);
  pinMode(ENCODER_DT_PIN, INPUT);
  
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK_PIN), readEncoder, FALLING);
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT_PIN);

  if (mode == 0) {
    trg += (dtValue == HIGH) ? 1 : -1;
  } 
  else if (mode == 1) {
    rspd += (dtValue == HIGH) ? 1 : -1;
    rspd = constrain(rspd, MIN_SPEED, MAX_SPEED);
  }
}

void updateTemperature() {
  int analogValue = analogRead(TEMPERATURE_SENSOR_PIN);
  celsius = 1 / (log(1 / (4095. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;

  if (celsius > MAX_TEMPERATURE) {
    lcd.setCursor(0, 0);
    lcd.print("HIGH TEMPERATURE");
    lcd.setCursor(0, 1);
    lcd.print(celsius);
    lcd.print(" Celsius");
  }
}

void moveMotorToTarget() {
  while (trg != deg) {
    digitalWrite(DIR_PIN, trg > deg ? HIGH : LOW);
    digitalWrite(STEP_PIN, HIGH);
    delay(spd);
    digitalWrite(STEP_PIN, LOW);
    deg += (trg > deg) ? 1 : -1;
  }
}

void parkMotor() {
  lcd.clear();
  lcd.print("Parking...");

  moveMotorToTarget();
  
  lcd.clear();
}


void handleMode0() {
  updateTemperature();

  if (trg < 0) {trg = 359;}
  if (trg >= 360) {trg = 0;}

  if (digitalRead(RSW_PIN) == LOW) {
    lcd.clear();
    lcd.print("Moving to: ");
    lcd.print(trg);
    lcd.print(char(223));
    digitalWrite(LED_PIN, HIGH);

    moveMotorToTarget();

    lcd.clear();
    digitalWrite(LED_PIN, LOW);
  }

  if (digitalRead(PARK_PIN) == LOW) {
    parkMotor();
  }

  lcd.setCursor(0, 0);
  lcd.print("Current: ");
  lcd.print(deg);
  lcd.println(char(223));
  lcd.setCursor(0, 1);
  lcd.print("Target: ");
  lcd.print(trg);
  lcd.println(char(223));
  delay(100);
}

void handleMode1() {
  if (millis() / 500 % 2 == 0) {
    lcd.clear();
  }

  spd = map(rspd, 0, MAX_SPEED, 200, 10);
  
  lcd.setCursor(0, 0);
  lcd.print("Speed: ");
  lcd.print(rspd);
  lcd.print("%");
  lcd.setCursor(0, 1);
  lcd.print("Raw: ");
  lcd.print(spd);
  lcd.print("ms");
  delay(100);
}

void handleMode2() {
  updateTemperature();

  lcd.setCursor(0, 0);
  lcd.print("Temp: ");
  lcd.print(celsius);
  lcd.setCursor(0, 1);
  lcd.print(millis());
  delay(100);
}

void loop() {
  if (digitalRead(MSW_PIN) == LOW) {
    mode++;
    lcd.clear();
  }

  if (mode >= 3) {
    mode = 0;
  }

  switch (mode) {
    case 0:
      handleMode0();
      break;
    case 1:
      handleMode1();
      break;
    case 2:
      handleMode2();
      break;
  }
}