/* seekbump_access.nqc
 *
 * Drive towards a bright light.  As long as the light
 * sensor sees a bright light, drive forward.  Otherwise
 * try to find the light first by spinning in place.
 * After 3 seconds of spinning, try driving forward
 * to a new location and then continue spinning.
 *
 * If an obstacle is encountered during seeking, back
 * up and spin a bit.
 *
 * This version of the program uses access control
 * to coordinate the seek and bump behaviors.
 */

/*
 * The first section of source code defines the basic
 * building blocks of the program including function to
 * setup the robot, find a target, and avoid an obstacle.
 */
 
// motors and sensors
#define LEFT    OUT_A
#define RIGHT   OUT_C
#define BUMPER  SENSOR_1
#define EYE     SENSOR_2

// timing
#define BUMP_BACK_TIME  60  // 0.6 seconds
#define BUMP_SPIN_TIME  20  // 0.2 seconds
#define SEEK_MAX_TIMER  30  // 3 seconds
#define SEEK_FWD_TIME   100 // 1 second
#define SEEK_DELAY      40  // 0.4 seconds

// threshold for light detector
int threshold=0;

// margin used to determing threshold
#define MARGIN  4


// setup the sensors and start moving
void setup()
{
	// setup sensors and start driving
	SetSensor(BUMPER, SENSOR_TOUCH);
	SetSensor(EYE, SENSOR_LIGHT);
	
	// determine light sensor threshold
	calibrate();
	
	OnFwd(LEFT+RIGHT);
}


// determine proper threshold
void calibrate()
{
	int baseline = EYE;
	
	do
	{
		int delta = EYE - baseline;
		
		if (delta > MARGIN)
		{
			threshold = baseline + delta - MARGIN/2;
		}
	} while(threshold == 0);
	
	PlaySound(SOUND_UP);
	until(EYE < threshold);
	PlaySound(SOUND_DOWN);
}


// spin and move around looking for target
void find_target()
{
	// start spinning and reset timer
	PlayTone(440, 10);
	Rev(LEFT);
	ClearTimer(0);
	
	while(EYE < threshold)
	{
		if (Timer(0) > SEEK_MAX_TIMER)
		{
			// spent too long spinning...
			// move forward a bit
			Fwd(LEFT);
			Wait(SEEK_FWD_TIME);
			
			// continue spinning and reset timer
			Rev(LEFT);
			ClearTimer(0);
		}
	}
	
	// found the light, resume
	PlayTone(880, 10);
	Fwd(LEFT);
}


// back and spin to avoid obstacle
void avoid_obstacle()
{
	// back up a bit        
	OnRev(LEFT+RIGHT);
	Wait(BUMP_BACK_TIME);
	
	// spin a bit
	Fwd(LEFT);
	Wait(BUMP_SPIN_TIME);
	
	// resume
	Fwd(RIGHT);
}


/*
 * The second section of the source code uses the
 * functions defined above in two tasks - the
 * main task avoids obstacles while the seek task
 * finds the target.  Access control is used to
 * coordinate these tasks.
 */
 
task main()
{
	setup();
	SetPriority(1);
	start seek;
	
	while(true)
	{
		until(BUMPER==0);
		
		acquire(ACQUIRE_USER_1)
		{
			avoid_obstacle();
		}
	}
}


task seek()
{
	SetPriority(2);
	
	while(true)
	{
		acquire(ACQUIRE_USER_1)
		{
			Wait(SEEK_DELAY);
			while(true)
			{
				until(EYE < threshold);            
				find_target();
			}
		}
	}
}