const int motorDoorOpenPin = 2;
const int motorDoorClosePin = 3;
const int motorLockOpenPin = 4;
const int motorLockClosePin = 5;

const int insideButtonPin = 6;
unsigned long lastInsideButtonPressTime = 0;

const int outsideButtonPin = 7;
unsigned long lastOutsideButtonPressTime = 0;

const int firstLockSensorPin = 8;
const int secondLockSensorPin = 9;

const int objectSensorPin = A1;
const int objectDetectionThreshold = 300;

enum State { IDLE, OPENING, CLOSING, LOCKING, UNLOCKING, EMERGENCY };

State currentState = IDLE;

unsigned long startMillis;
unsigned long currentMillis;
const unsigned long lockOperationMaxDuration = 3000;
const unsigned long doorOpeningMovementMaxDuration = 6000;
const unsigned long doorClosingMovementMaxDuration = 7500;
const unsigned long buttonCooldown = 500;

void setup() {
	Serial.begin(115200);
	pinMode(motorDoorOpenPin, OUTPUT);
	pinMode(motorDoorClosePin, OUTPUT);
	pinMode(motorLockOpenPin, OUTPUT);
	pinMode(motorLockClosePin, OUTPUT);
	
	pinMode(insideButtonPin, INPUT_PULLUP);
	pinMode(outsideButtonPin, INPUT_PULLUP);
	pinMode(objectSensorPin, INPUT_PULLUP);
	pinMode(firstLockSensorPin, INPUT_PULLUP);
	pinMode(secondLockSensorPin, INPUT_PULLUP);
}

void loop() {
	bool insideButtonPressed = !digitalRead(insideButtonPin);
	bool outsideButtonPressed = !digitalRead(outsideButtonPin);
	bool objectDetected = analogRead(objectSensorPin) < objectDetectionThreshold;
	bool firstLockSensor = digitalRead(firstLockSensorPin);
	bool secondLockSensor = digitalRead(secondLockSensorPin);

	printSensors(doorDirection, objectDetected, firstLockSensor, secondLockSensor);

	// Get current time
  currentMillis = millis();

	switch (currentState) {
		case IDLE:
			stopMotors();
			if (insideButtonPressed) {
				currentState = CLOSING;
				startMillis = currentMillis;
			} else if (outsideButtonPressed) {
				currentState = OPENING;
				startMillis = currentMillis;
			}
			break;
		case UNLOCKING:
			unlockDoor();
			if (currentMillis - startMillis >= lockOperationMaxDuration) {
				currentState = IDLE;
				startMillis = currentMillis;
			} else if (!firstLockSensor && !secondLockSensor) {
				currentState = OPENING;
				stopLocking();
				startMillis = currentMillis;
			}
			break;
		case LOCKING:
			lockDoor();
			if (currentMillis - startMillis >= lockOperationMaxDuration) {
				currentState = IDLE;
				startMillis = currentMillis;
			} else if (firstLockSensor && secondLockSensor) {
				currentState = IDLE;
				startMillis = currentMillis;
			}
			break;
		case OPENING:
			openDoor();
			if (firstLockSensor || secondLockSensor) {
				stopDoor();
				currentState = UNLOCKING;
				startMillis = currentMillis;
			} else if (currentMillis - startMillis >= doorOpeningMovementMaxDuration) {
				currentState = IDLE;
				startMillis = currentMillis;
			}
			break;
		case CLOSING:
			closeDoor();
			if (objectDetected) {
				currentState = EMERGENCY;
				startMillis = currentMillis;
			} else if (firstLockSensor || secondLockSensor) {
				currentState = LOCKING;
				startMillis = currentMillis;
			} else if (currentMillis - startMillis >= doorClosingMovementMaxDuration) {
				currentState = IDLE;
				startMillis = currentMillis;
			}
			break;
		case EMERGENCY:
			openDoor();
			unlockDoor();
			if (currentMillis - startMillis >= 500) {
				currentState = IDLE;
				startMillis = currentMillis;
			}
	}





	// HAVENT TOUCHED THIS PART YET, FIRST TRY TO MAKE IT WORK WITH THE STATE MACHINE 

	// Inside button pressed
	if (insideButtonPressed && (currentMillis - lastInsideButtonPressTime >= buttonCooldown)) {
		if (!objectDetected && doorDirection == STOP) {
			doorDirection = CLOSING;
		} else {
			doorDirection = IDLE;
		}
		lastInsideButtonPressTime = currentMillis;
		startMillis = currentMillis;
	}

	// Outside button pressed
	if (outsideButtonPressed && (currentMillis - lastOutsideButtonPressTime >= buttonCooldown)) {
		if (firstLockSensor || secondLockSensor) {
			doorDirection = OPEN;
		} else if (!objectDetected) {
			doorDirection = CLOSING;
		} else {
			doorDirection = STOP;
		}
		lastOutsideButtonPressTime = currentMillis;
		startMillis = currentMillis;
	}

}
 

void lockDoor() {
	digitalWrite(motorLockOpenPin, LOW);
	digitalWrite(motorLockClosePin, HIGH);
}


void unlockDoor() {
	digitalWrite(motorLockOpenPin, HIGH);
	digitalWrite(motorLockClosePin, LOW);
}

void stopLocking() {
	digitalWrite(motorLockOpenPin, LOW);
	digitalWrite(motorLockClosePin, LOW);
}

void closeDoor() {
	digitalWrite(motorDoorOpenPin, LOW);
	digitalWrite(motorDoorClosePin, HIGH);
}

void openDoor() {
	digitalWrite(motorDoorOpenPin, HIGH);
	digitalWrite(motorDoorClosePin, LOW);
}

void stopDoor() {
	digitalWrite(motorDoorOpenPin, LOW);
	digitalWrite(motorDoorClosePin, LOW);
}

void stopMotors() {
	stopDoor();
	stopLocking();
}

void emergencyOpenMotorsFor0Point4SecondsThenStop() {
	stopMotors();
	openDoor();
	unlockDoor();
	delay(400);
	stopMotors();
}

void printSensors(bool doorDirection, bool objectDetected, bool firstLockSensor, bool secondLockSensor) {
	Serial.print("door direction:");
	Serial.print(doorDirection);
	Serial.print("-----");
	Serial.print("object detected:");
	Serial.print(objectDetected);
	Serial.print("-----");
	Serial.print("first lock sensor:");
	Serial.print(firstLockSensor);
	Serial.print("-----");
	Serial.print("second lock sensor:");
	Serial.println(secondLockSensor);
}