/*
  jefdepatat
  Discord Arduino coding-help 11/2/23 at 5:06 PM
  So i'm trying to make a project (an elevator).
  I'm using the HC-SR04 to measure the distance of a carrier.
  With the distance i measured i'll tell the stepper motor to go
  up or down based on what floor the carrier is.
  You get what i'm trying to do or atleast I hope so.
  Well the problem is that the stepper doesn't move smoothly
  till the if statement is completed. I'm pretty new to arduino
  so don't be too harsh on me thanks

  Button handling courtesy of:
  https://github.com/leftCoast/LC_baseTools/tree/master

*/

#include <AccelStepper.h>
#include <LiquidCrystal_I2C.h>
#include <mechButton.h>

const int NUM_LEDS = 3;
const int BTN_PIN[] = {9, 8, 7};
const int LED_PIN[] = {12, 11, 10};
const int TRIG_PIN = 6;
const int ECHO_PIN = 5;
const int STEP_PIN = 3;
const int DIR_PIN = 2;
// some arrows, for fun
byte upArrow[8] = {
  B00000,
  B00100,
  B01110,
  B01110,
  B11111,
  B11111,
  B00000
};
byte dnArrow[8] = {
  B00000,
  B11111,
  B11111,
  B01110,
  B01110,
  B00100,
  B00000
};

AccelStepper elevatorStepper (1, STEP_PIN, DIR_PIN);
LiquidCrystal_I2C lcd (0x27, 16, 2);
mechButton btnFloor1(BTN_PIN[0]);
mechButton btnFloor2(BTN_PIN[1]);
mechButton btnFloor3(BTN_PIN[2]);

int getRange() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = duration * 0.0343 / 2;
  //Serial.print("Float: ");
  //Serial.println(distance);
  int round_distance = distance + 0.5;
  return round_distance;
}

void lcdShow(int currFloor, int destFloor, int dir, bool onFloor) {
  lcd.setCursor(0, 0);
  lcd.print("Now on: ");
  lcd.setCursor(9, 0);
  lcd.print(currFloor + 1);
  lcd.setCursor(0, 1);
  if (!onFloor)  {
    lcd.print("Enroute: ");
    lcd.setCursor(9, 1);
    lcd.print(destFloor + 1);
  } else {
    lcd.print("          ");
  }
  switch (dir)  {
    case 0:
      lcd.setCursor(15, 0);
      lcd.write(byte(0));
      lcd.setCursor(15, 1);
      lcd.print(" ");
      break;
    case 1:
      lcd.setCursor(15, 0);
      lcd.print(" ");
      lcd.setCursor(15, 1);
      lcd.write(byte(1));
      break;
    case 2:
      lcd.setCursor(15, 0);
      lcd.print(" ");
      lcd.setCursor(15, 1);
      lcd.print(" ");
      break;
    default:
      break;
  }
}

void lightLED(int number) {
  for (int i = 0; i < NUM_LEDS; i++)  {
    digitalWrite(LED_PIN[i], LOW);
  }
  digitalWrite(LED_PIN[number], HIGH);
}

void moveStepper(int speed) {
  elevatorStepper.setSpeed(speed);
  elevatorStepper.runSpeed();
}

void setup() {
  //Serial.begin(9600);
  lcd.init();
  lcd.createChar(0, upArrow);
  lcd.createChar(1, dnArrow);
  lcd.clear();
  lcd.backlight();
  elevatorStepper.setMaxSpeed(1000);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  for (int i = 0; i < NUM_LEDS; i++) {
    pinMode(LED_PIN[i], OUTPUT);
  }
  // splash screen
  lcd.print("Elevator Control");
  lcd.setCursor(5, 1);
  lcd.print("v1.0.0");
  delay(2000);
  lcd.clear();
}

void loop() {
  static int destinationFloor;

  int currentFloor;
  int direction;
  bool arrived;
  bool btn1State, btn2State, btn3State;

  // to which floor should the elevator go?
  btn1State = btnFloor1.trueFalse();
  btn2State = btnFloor2.trueFalse();
  btn3State = btnFloor3.trueFalse();
  if (!btn1State) destinationFloor = 0;
  if (!btn2State) destinationFloor = 1;
  if (!btn3State) destinationFloor = 2;
  // what floor is the elevator on?
  int cmRange = getRange();
  if (cmRange < 125) {
    currentFloor = 0;
  } else if (cmRange >= 125 && cmRange < 250) {
    currentFloor = 1;
  } else if (cmRange >= 250) {
    currentFloor = 2;
  }
  // move elevator up or down, or stop
  if (destinationFloor != currentFloor) {
    arrived = false;
    if (destinationFloor > currentFloor)  {
      direction = 0;
      moveStepper(500);
    }
    if (destinationFloor < currentFloor)  {
      direction = 1;
      moveStepper(-500);
    }
  } else {
    arrived = true;
    direction = 2;
    moveStepper(0);
  }
  // indicate floor
  lightLED(currentFloor);
  lcdShow(currentFloor, destinationFloor, direction, arrived);
}