#include <LiquidCrystal.h>
#include <Servo.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

// Variables
Servo servo;
int speed = 0;		   // current vehicle speed
int pos = 0;		   // current servomotor position
int ledState = LOW;	   // the current state of the output pin
int buttonState;	   // the current reading from the input pin
int lastButtonState = LOW; // the previous reading from the input pin
float startTime;	   // when motion is detected and  doors fully open
unsigned long lastDebounceTime = 0; // the last time the output pin was toggled
unsigned long debounceDelay =
    10; // the debounce time; increase if the output flickers
bool motionDetected = false;

// Constants
const int closed_pos = 0, open_pos = 130, servo_delay = 15;
const int max_speed = 10, start_moving_speed = 3;
const float printMotionTime = 2500;
const unsigned int calibrationTime = 60;

// Pins
const int sensorPin = 6;
const int buttonPin = 7;
const int ledPinRed = 8;
const int servoPin = 9;
const int motorPin = 10;
const int ledPinGreen = 13;

void setup()
{
	// Set up the LCD's number of columns and rows:
	lcd.begin(16, 2);
	pinMode(sensorPin, INPUT);
	pinMode(buttonPin, INPUT);
	pinMode(ledPinGreen, OUTPUT);
	pinMode(ledPinRed, OUTPUT);
	pinMode(motorPin, OUTPUT);

	Serial.begin(9600);
	servo.attach(servoPin, 500, 2500);
	servo.write(closed_pos);

	for (int i = 0; i < calibrationTime; i++) {
		lcd.setCursor(0, 0);
		lcd.print("Calibrating");
		lcd.setCursor(5, 1);
		lcd.print("sensor...");
		delay(1000);
	}
	lcd.clear();

	// Make sure vehicle is not moving when the simulation begins
	speed = map(analogRead(A0), 0, 999, 0, 115);
	speed -= speed;
}

void print_movement()
{
	lcd.setCursor(0, 0);
	lcd.print("Someone at the");
	lcd.setCursor(5, 1);
	lcd.print("door!");
}

void print_speed()
{
	// Conversion from RMP to km/h
	speed = map(analogRead(A0), 0, 999, 0, 115);

	if (speed >= start_moving_speed)
		digitalWrite(motorPin, HIGH);
	else
		digitalWrite(motorPin, LOW);

	lcd.setCursor(0, 0);
	lcd.print("Speed: ");
	lcd.print(speed);
	lcd.print(" km/h   ");
}

void print_door_state()
{
	lcd.setCursor(0, 1);
	lcd.print("Door: ");
	if (!ledState)
		lcd.print("Closed");
	else
		lcd.print("Opened");
}

void open_door()
{
	for (int i = pos; i <= open_pos; i++, pos++) {
		if (speed >= max_speed) {
			ledState = LOW;
			close_door();
			break;
		}

		// Tell servo to go to position in variable 'pos'
		servo.write(pos);
		delay(servo_delay);

		if (motionDetected) {
			print_movement();
		} else {
			print_speed();
		}
	}
	startTime = millis();
}

void close_door()
{
	for (int i = pos; i >= closed_pos; i--, pos--) {
		if (digitalRead(sensorPin) == HIGH && speed < max_speed) {
			ledState = HIGH;
			motionDetected = true;
			lcd.clear();
			open_door();
			break;
		}

		// Tell servo to go to position in variable 'pos'
		servo.write(pos);
		delay(servo_delay);
		print_speed();
	}
}

void loop()
{
	// Set the LED:
	if (ledState) {
		digitalWrite(ledPinRed, LOW);
		digitalWrite(ledPinGreen, HIGH);
	} else {
		digitalWrite(ledPinGreen, LOW);
		digitalWrite(ledPinRed, HIGH);
	}

	if (motionDetected) {
		if (millis() - startTime < printMotionTime) {
			print_movement();
			return;
		}

		lcd.clear();
		motionDetected = false;
		Serial.println(pos);
		return;
	}

	print_speed();
	print_door_state();

	if (ledState != LOW && speed >= max_speed) {
		ledState = LOW;
		close_door();
		return;
	}

	// Read the state of the switch into a local variable
	int reading = digitalRead(buttonPin);

	// If the switch changed, due to noise or pressing:
	if (reading != lastButtonState) {
		// Reset the debouncing timer
		lastDebounceTime = millis();
	}

	if ((millis() - lastDebounceTime) > debounceDelay) {

		// If the button state has changed:
		if (reading != buttonState) {
			buttonState = reading;

			// Only toggle the LED if the new button state is HIGH
			if (buttonState == HIGH)
				if (!ledState && speed < start_moving_speed) {
					ledState = HIGH;
					open_door();
				}

				else {
					ledState = LOW;
					close_door();
				}
		}
	}

	// Save the reading, it'll be the lastButtonState:
	lastButtonState = reading;
}