#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>

#define upSpeed 255
#define downSpeed 150

const int ROTARY_DT = A0;    // DT pin of KY-040
const int ROTARY_CLK = 13;   // CLK pin of KY-040
const int ROTARY_SW = 3;    // SW pin of KY-040
const int hallSensorPin = 2;  // Replace with the actual pin connected to the Hall effect sensor
const int pwm = 11;
const int direct = A1;
const int forward = A2;
const int backward = A3;
volatile int pulseCount = 0;
const int ROW_NUM    = 4; // four rows
const int COLUMN_NUM = 4; // four columns
char keys[ROW_NUM][COLUMN_NUM] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte pin_rows[ROW_NUM] = {12, 10, 9, 8}; // connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = {7, 6, 5, 4}; // connect to the column pinouts of the keypad
Keypad myKeypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM);

LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, 16 column and 2 rows

float targetDistance = 0;
float coveredDistance = 0;
bool settingTarget = false;
int encoderPinA_prev;
int timer;
bool target;
bool moveFlag = false;

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);

  pinMode(ROTARY_DT, INPUT_PULLUP);
  pinMode(ROTARY_CLK, INPUT_PULLUP);
  pinMode(ROTARY_SW, INPUT_PULLUP);
  pinMode(hallSensorPin, INPUT_PULLUP);
	pinMode(forward, INPUT_PULLUP);
	pinMode(backward, INPUT_PULLUP);
  pinMode(pwm, OUTPUT);
  pinMode(direct, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(hallSensorPin), countPulse, FALLING);
  attachInterrupt(digitalPinToInterrupt(ROTARY_SW), toggleSetting, FALLING);
}

void loop() {
  char key = myKeypad.getKey();

  if (key != NO_KEY) 
	{
    if (key == 'A')
    {
      targetDistance = 3;
      settingTarget = false;
      pulseCount = 0;
    }
    else if (key == 'B')
    {
      targetDistance = 5;
      settingTarget = false;
      pulseCount = 0;
    }
    else if (key == 'C')
    {
      targetDistance = 9;
      settingTarget = false;
      pulseCount = 0;
    }
    else if (key == 'D')
    {
      targetDistance = coveredDistance;
      settingTarget = false;
      pulseCount = 0;
    }
    else if (!moveFlag)
    {
      if (key == '#') {
        // Submit target distance
        int encoderPinA_value = digitalRead(ROTARY_CLK);
        encoderPinA_prev = encoderPinA_value;
        if (!settingTarget)
          targetDistance = 0;
        settingTarget = !settingTarget;
        coveredDistance = 0;
      }
      else {
        // Update target distance based on key pressed
        int keyVal = key - '0';
        if (settingTarget)
          targetDistance = targetDistance * 10 + keyVal;
        //settingTarget = true;
      }
    }
  }
  int currentTime = millis();
  if (currentTime > timer + 1000)
  {
    if (settingTarget) {
      lcd.clear();
      lcd.print("Target: ");
      lcd.print(targetDistance);
      lcd.print("m");
      lcd.setCursor(0, 1);
      lcd.print("Setting target...");
    } else {
      lcd.clear();
      lcd.print("Target: ");
      lcd.print(targetDistance);
      lcd.print("m");
      lcd.setCursor(0, 1);
      lcd.print("Covered: ");
      lcd.print(coveredDistance);
      lcd.print("m");
    }
    timer = currentTime;
  }
  if (settingTarget)
  {
    handleEncoder();
    coveredDistance = 0;
  }
  else if ((coveredDistance > targetDistance) && (targetDistance > 0))
    moveFlag = false;
  else if ((coveredDistance < targetDistance) && (targetDistance < 0))
    moveFlag = false;
  else if (coveredDistance != targetDistance)
    moveFlag = true;
  else
    moveFlag = false;
  if (moveFlag)
  {
    moveMotor();
    coveredDistance = pulseCount * 0.0195;
    if (targetDistance < 0)
      coveredDistance *= -1;
  }
	else if(digitalRead(forward)==LOW)
	{
		digitalWrite(direct,LOW);
		analogWrite(pwm,upSpeed);
	}
	else if(digitalRead(backward)==LOW)
	{
		digitalWrite(direct,HIGH);
		analogWrite(pwm,upSpeed);
	}
	else
	{
		analogWrite(pwm,0);
	}
}

void moveMotor()
{
  if (targetDistance > 0)
  {
    digitalWrite(direct, LOW);
  }
  else if (targetDistance < 0)
  {
    digitalWrite(direct, HIGH);
  }
  if ((coveredDistance < targetDistance - 1))
  {
    analogWrite(pwm, upSpeed);
  }
  else if ((coveredDistance > targetDistance + 1))
  {
    analogWrite(pwm, upSpeed);
  }
  else
  {
    analogWrite(pwm, downSpeed);
  }
}

void handleEncoder() {
  int encoderPinA_value = digitalRead(ROTARY_CLK);
  if (encoderPinA_value != encoderPinA_prev) { // check if knob is rotating
    // if pin A state changed before pin B, rotation is clockwise
    if (digitalRead(ROTARY_DT) != encoderPinA_value) {
      targetDistance += 0.1;
    } else {
      // if pin B state changed before pin A, rotation is counter-clockwise
      targetDistance -= 0.1;
    }
    encoderPinA_prev = encoderPinA_value;
  }
}

void toggleSetting() {
  if (!moveFlag)
  {
    if (settingTarget) {
      settingTarget = false;
    } else {
      targetDistance = 0;
      settingTarget = true;
    }
  }
}

void countPulse() {
  // This function is called when a falling edge is detected on the hallSensorPin
  if (moveFlag)
    pulseCount++;
  else
    pulseCount = 0;
}