/* The following code is taken with modifications from Joseph L. Jones,
 * Anita M.  Flynn, and Bruce A. Seiger, Mobile Robots: Inspiration to
 * Implementation, Second Edition, Section 9.5, Behavior Control in IC, 
 * pages 302-306, A. K. Peters, 1999.   Modifications by Dean F. Hougen,
 * 2003.
 */

/* CRUISE: BEHAVIOR TO GO FORWARD */

int cruise_command;                     /* GLOBAL: Command to motors */
int cruise_output_flag;                 /* GLOBAL: Command active? */

void cruise(){                          
    while(1){
        cruise_command = FORWARD;
        cruise_output_flag = TRUE;      /* Command is now active */
    }
}

/* FOLLOW: BEHAVIOR TO FOLLOW A LIGHT */

int follow_command;                     /* GLOBAL: Command to motors */
int follow_output_flag;                 /* GLOBAL: Command active? */

void follow(){ 
    int left_photo, right_photo, delta; /* Left and right photocells */
    while(1){
        left_photo = analog(1);
        right_photo = analog(0);
        delta = right_photo - left_photo;
        if (abs(delta) > PHOTO_DEAD_ZONE){
            if (delta > 0)
                follow_command = LEFT_TURN;
            else
                follow_command = RIGHT_TURN;
            follow_output_flag = TRUE;
        }
        else
            follow_output_flag = FALSE;
    }
}

/* AVOID: BEHAVIOR TO AVOID NEARBY OBSTACLES USING IR DETECTION */

int avoid_command;
int avoid_output_flag;

void avoid(){
    int val;
    while(1){
        val = ir_detect();
        if (val == 0b11){               /* Both left and right IRs active */
            avoid_command = LEFT_ARC;
            avoid_output_flag = TRUE;
        }
        else if (val = 0b10){           /* Left IR active */
            avoid_command = RIGHT_ARC;
            avoid_output_flag = TRUE;
        }
        else if (val = 0b01){           /* Right IR active */
            avoid_command = LEFT_ARC;
            avoid_output_flag = TRUE;
        }
        else
            avoid_output_flag = FALSE;
    }
}

/* ESCAPE: BEHAVIOR TO TURN AWAY FROM COLLISIONS DETECTED BY BUMPERS */

int escape_command;
int escape_output_flag;

void escape(){
    while(1){
        bump_check();
        if (bump_left && bump_right){
            escape_output_flag = TRUE;
            escape_command = BACKWARD;
            sleep(0.2);
            escape_command = LEFT_TURN;
            sleep(0.4);
        }
        else if (bump_left){
            escape_output_flag = TRUE;
            escape_command = RIGHT_TURN;
            sleep(0.4);
        }
        else if (bump_right){
            escape_output_flag = TRUE;
            escape_command = LEFT_TURN;
            sleep(0.4);
        }
        else if (bump_back){
            escape_output_flag = TRUE;
            escape_command = LEFT_TURN;
            sleep(0.2);
        }
        else
            escape_output_flag = FALSE;
    }
}

/* MAIN: START ALL PROCESSES */

void main(){
    start_process(motor_driver());
    start_process(cruise());
    start_process(follow());
    start_process(avoid());
    start_process(escape());
    start_process(arbitrate());
}

/* ARBITRATE: CHOOSE BEHAVIOR */

void arbitrate(){
    while(1){
        if (cruise_output_flag == TRUE)
            motor_input = cruise_command;
        if (follow_output_flag == TRUE)
            motor_input = follow_command;
        if (avoid_output_flag == TRUE)
            motor_input = avoid_command;
        if (escape_output_flag == TRUE)
            motor_input = escape_command;
        sleep(tick);
    }
}