// The define directive substitutes everything after the second space everywhere
// the label between the first and second spaces appears.  The substitution is a
// direct text substitution and it occurs immediately before compiling the code.
// You can use this for constants, function calls, calibration equations, etc.

// Amount of time to let the robot run.
#define RUNTIME 30.0
// Direction control flags.
#define LEFT 0
#define RIGHT 1
// Motor power level definitions.
#define HIGH 80
#define MID 60
#define LOW 40

int irRead[5] = {0,0,0,0,0}; // Array used to store the IR sensor readings.
int hit = -1; // Variable used to indicate where a light source has been detected
int oldHit = hit;

int timeup = 0; // Flag used to signal when time has expired.

// Variables used to store the process ID's of the three main processes.
int timerPID;
int lightLocatorPID;
int controlMotorsPID;

// The timer function waits for a specified length of time and then switches the
// timeup flag.
void timer( float t ){
    float startTime = seconds();
    while( seconds() - startTime < t ){  // While time has not expired.
        defer();  // Relinquish control of the processor.
    }
    timeup = 1; // Once time has expired, change the flag, and exit the function.
} // timer( float t )


// The lightLocator function sets the variable hit to an integer representing
// the IR sensor which is detecting a light.
// -1 indicates that no hit was detected.
// 0 through 4 indicate which sensor has a hit.
void lightLocator()
{
    int i;        // A counter index.
    int minRead;  // The lowest IR reading.
    int maxRead;  // The highest IR reading.
    int maxIndex; // The index (sensor number) of the highest IR reading.
    
    while(!timeup){
        // Reset tracking variables.
        minRead = 256;
        maxRead = -1;
        maxIndex = -1;    
        // Loop through each sensor to obtain the readings.
        for(i=0; i<=4; i++){
            irRead[i] = 255 - analog(i); // Store the readings in the array.
            
            if( irRead[i]<minRead ){ // Keep track of the minimum reading. 
                minRead = irRead[i];
            }
            if( irRead[i]>maxRead ){ // Keep track of the maximum reading.
                maxIndex = i;
                maxRead = irRead[i];
            }
        }
        
        // Process the readings to see if a meaningful hit was found.
        if( ((float)(maxRead-minRead))/((float)minRead) > 1.0 ){
            hit = maxIndex;
        }
        else{ hit = -1; } // If there was not a great enough difference between the
        // highest and lowest readings, use the -1 flag to indicate no hit.
        
    }
    
} // lightLocator()


// Turns the robot quickly in the specified direction and speed.
void turn(int dir, int speed)
{
    if(dir == LEFT){
        motor(1, speed);
        motor(2, -speed);
    }
    else{  // Need to turn RIGHT.
        motor(1, -speed);
        motor(2, speed);
    }
} // turn(int dir, int speed)

// Makes the robot drive forward at the specified power level.
void goForward(int power){
    motor(1, power);
    motor(2, power);
} // goForward(int power)


// The controlMotors function controls the motors based on the value of "hit"
// which is continuously updated by lightLocator().
void controlMotors()
{
    while(!timeup){ // As long as time is left, continue updating the motor commands.
        if ( hit != oldHit){ // Only need to update if the hit has changed.
            oldHit = hit;    // If it has changed, update oldHit and command motors.
            if(hit == -1){ alloff();          defer(); }
            if(hit == 0) { turn(LEFT, HIGH);  defer(); }
            if(hit == 1) { turn(LEFT, LOW);   defer(); }
            if(hit == 2) { goForward(MID);    defer(); }
            if(hit == 3) { turn(RIGHT, LOW);  defer(); }
            if(hit == 4) { turn(RIGHT, HIGH); defer(); }
        }
    }
    // Once time has expired kill the engines and end the function.
    alloff();
} // controlMotors()

// Main simply starts processes for the controlling functions above.
void main()
{
    while(1){
        printf("Press start to  chase light.\n");
        while( !start_button() ){  } // Do nothing until start is pressed
        
        printf("Chasing!!\n");  
        reset_system_time();  // Reset the system time after you start the program.
        timeup = 0;           // Reset the timeup flag.
        
        timerPID = start_process( timer( RUNTIME ) );
        lightLocatorPID = start_process( lightLocator() );
        controlMotorsPID = start_process( controlMotors() );
        
        while(!timeup && !stop_button() ){  // Simply defer and let the processes
                                            // called above handle the action until
                                            // time expires.
            
            // Add debugging commands here as needed.
            defer(); 
        }
        
        // Once time has expired or the stop button is pressed, immediately
        // kill all processes and make sure the motors are stopped.
        kill_process(controlMotorsPID);
        kill_process(lightLocatorPID);
        kill_process(timerPID);
        alloff();
        beep();
    }
    
} // main()