//*!!Sensor,    S1,            doorsOpen, sensorTouch,      ,                    !!*//
//*!!Sensor,    S2,    personApproaching, sensorSONAR9V,      ,                  !!*//
//*!!Sensor,    S3,          doorsClosed, sensorTouch,      ,                    !!*//
//*!!Motor,  motorA,                doors, tmotorNormal,                         !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors doorsOpen            = (tSensors) S1;   //sensorTouch        //*!!!!*//
const tSensors personApproaching    = (tSensors) S2;   //sensorSONAR9V      //*!!!!*//
const tSensors doorsClosed          = (tSensors) S3;   //sensorTouch        //*!!!!*//
const tMotor   doors                = (tMotor) motorA; //tmotorNormal       //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//
// Automatic door opening
//
// Copyright Mark Crosbie August 2007
// mark@mastincrosbie.com
// http://markclego.mastincrosbie.com
//
// Open the doors when a reading is detected on the US sensor
// Halt the doors opening once the touch sensors return a reading
// Then close the door after a timeout
// Be sure not to crush anyone coming though the doors
//
// Connections:
// Port A - Legacy Mindstorms Motors
// Port 1 - doors are open touch sensor
// Port 2 - Ultrasonic sensor
// Port 3 - Doors are closed sensor
//

//////////////////////////////////////////////////////////////////////////////////////////
//
// Constants
//
//////////////////////////////////////////////////////////////////////////////////////////

const int kDoorOpenTime = 500;	// how long to keep the doors open for
const int ksonarThreshold = 10; // what reading triggers the door opening
const int kDoorCloseSpeed = 60;	// speed to close the doors at (slightly slower than opening speed)
const int kDoorOpenSpeed = -80;	// speed to open the doors at
const int kDebugLine = 5;				// line to display debug messages on

//////////////////////////////////////////////////////////////////////////////////////////
//
// States the door system can be in
//
//////////////////////////////////////////////////////////////////////////////////////////
typedef enum {
	sNoState,
	sDoorsOpening,	// state that the doors are currently opening
	sDoorsClosing,	// state that the doors are currently closing
	sDoorsOpen,				// doors are now open fully
	sDoorsClosed, 			// doors are now fully closed
} tDoorStates;

//////////////////////////////////////////////////////////////////////////////////////////
//
// Globals
//
//////////////////////////////////////////////////////////////////////////////////////////
tDoorStates currentState;	// current state
bool personState;			// was a person detected at the door?
void closeDoors();
void displayWelcomeMessage();

//////////////////////////////////////////////////////////////////////////////////////////
//
// main
//
// Display the welcome message, ensure that the doors are initially closed
// and then run the state machine loop
//
//////////////////////////////////////////////////////////////////////////////////////////

task main()
{

	int line = 1;
	bool bDoorsOpen;
	bool bDoorsClosed;

	SensorType[doorsOpen] = sensorTouch;
	SensorType[doorsClosed] = sensorTouch;

	displayWelcomeMessage();

	// first ensure the doors are closed
	if(SensorValue[doorsClosed] != 1) {
		closeDoors();
	}

	wait10Msec(2);

	currentState = sDoorsClosed;

	while(1)
	{

		//
		// Measure the current state of all our external stimuli
		// This means, what state each each sensor showing?
		//
		if(SensorValue[personApproaching] <= ksonarThreshold)
		{
			personState = true;
		} else {
			personState = false;
		}

		if(SensorValue[doorsClosed] == 1)
			bDoorsClosed = true;
		else
			bDoorsClosed = false;

		if(SensorValue[doorsOpen] == 1)
			bDoorsOpen = true;
		else
			bDoorsOpen = false;


		switch(currentState) {
			case sDoorsClosed:
					if(personState) {
						motor[doors] = kDoorOpenSpeed;	// someone is at the door - open it
						currentState = sDoorsOpening;
					}
					break;

			case sDoorsOpening:
					if(bDoorsOpen) {
						motor[doors] = 0;								// doors have opened all the way
						currentState = sDoorsOpen;			// so stop the motor
					}
					break;

			case sDoorsOpen:
					wait10Msec(kDoorOpenTime);				// wait for a period and then start
					motor[doors] = kDoorCloseSpeed;		// to close the doors again
					currentState = sDoorsClosing;
					break;

			case sDoorsClosing:
					if(personState) {
						motor[doors] = 0;								// something is trying to go in the
						wait10Msec(1);									// doors again. Stop the motor and
						motor[doors] = kDoorOpenSpeed;	// open the doors again.
						currentState = sDoorsOpening;
						break;
					} else if(bDoorsClosed) {					// otherwise wait until the door
						motor[doors] = 0;								// closed sensor indicates that the doors
						currentState = sDoorsClosed;		// are closed
					}
					break;

					default: // nop
				}

				wait10Msec(5);	// no need to murder the CPU in a tight loop
		}
}

//////////////////////////////////////////////////////////////////////////////////////////
//
// Display a welcome message indicating what ports to connect
// and wait for the middle button to be pressed on the NXT
//
//////////////////////////////////////////////////////////////////////////////////////////

void displayWelcomeMessage()
{
	int line = 1;

	nxtDisplayTextLine(line++, "Automatic doors");
	nxtDisplayTextLine(line++, "Motor - Port A");
	nxtDisplayTextLine(line++, "Door open - 1");
	nxtDisplayTextLine(line++, "Ultrasonic - 2");
	nxtDisplayTextLine(line++, "Door closed - 3");
	nxtDisplayTextLine(line++, "Press orange key");

	while(nNxtButtonPressed == kNoButton) ;

	eraseDisplay();
}

void closeDoors()
{
	motor[doors] = kDoorCloseSpeed;
	while(SensorValue[doorsClosed] == 0)
	{
		wait10Msec(1);
	}
	wait10Msec(1);
	motor[doors] = 0;
}