From:
//  WNYC_10_Kemmel.nxc
//  Created by: hdrake, TabbyCat Robots      2011Jun29


task line_color_detect()
{
    int prelim_detected_roadway_color;
    unsigned char color_action_code;

    current_detected_roadway_color = 0;
    roadway_idx = 0;

    //  The following 2 lines are for debugging.  Uncomment for debugging.
//    detected_roadway_colors[roadway_idx] = current_detected_roadway_color;
//    roadway_idx++;

    previous_detected_roadway_color = current_detected_roadway_color;

    while(TRUE)
    {
        prelim_detected_roadway_color = Sensor(COLOR_SENSOR);
        if (prelim_detected_roadway_color == previous_detected_roadway_color)
        {
            Wait(2);
            continue;  //  Go to top of while loop.
        }

        if (prelim_detected_roadway_color == INPUT_YELLOWCOLOR)
        {
            Wait(yellow_detect_delay);
            prelim_detected_roadway_color = Sensor(COLOR_SENSOR)
        }

        Wait(color_detect_delay);   //  Was 4, 7
        current_detected_roadway_color = Sensor(COLOR_SENSOR);

        if (current_detected_roadway_color != prelim_detected_roadway_color)
        {
            continue;  //  Go to top of while loop.
        }

        if (current_detected_roadway_color == previous_detected_roadway_color)
        {
            continue;  //  Go to top of while loop.
        }

        //  The following if block is for debugging.  Uncomment for debugging.
/*
        if (current_detected_roadway_color != INPUT_WHITECOLOR)
        {
            detected_roadway_colors[roadway_idx] = current_detected_roadway_color;
            roadway_idx++;
        }
*/        
        color_action_code = (10 * previous_detected_roadway_color) + current_detected_roadway_color;
        previous_detected_roadway_color = current_detected_roadway_color;

        //  The following cdtrace lines are for debugging.  Uncomment for debugging.
//        cdtrace_buffer[cdtrace_idx] = 8;
//        cdtrace_idx++;
//        cdtrace_buffer[cdtrace_idx] = current_todo_queue_length;
//        cdtrace_idx++;
        if (cdtrace_idx > 59)
        {
//            break;
            cdtrace_idx = 0;
        }

        //  Note for the following switch.  This thread does not set any
        //  global variables that are used by the line_follower_pro task.
        //  All data for the line_follower_pro task must go into the
        //  roadway_todo_queue, so items are executed and variables
        //  considered in the correct order.
        switch (color_action_code)
        {
        case BLACK_GREEN_ACTION_CODE:
            //  May need to add code for dependencies on ROADWAY_TYPE and ROADWAY_SUBTYPE.
            switch (roadway_subtype)
            {
            case ROADWAY_SUBTYPE_RACING_LINE:  //  Accelerate - increment 1 speed_subscript.
                if (speed_subscript < MAX_SPEED_SUBSCRIPT)
                {
                    speed_subscript++;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
                break;

            case ROADWAY_SUBTYPE_PIT_ENTANCE:
                pit_location = PIT_STALL_LOCATION_RIGHT;
                roadway_subtype = ROADWAY_SUBTYPE_PIT_LANE;
                break;

            case ROADWAY_SUBTYPE_PIT_LANE:
                if (car_id == CAR_COLOR_GREEN)
                {
                    if (pit_location == PIT_STALL_LOCATION_RIGHT)
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_RIGHT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    else
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_LEFT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    roadway_subtype = ROADWAY_SUBTYPE_PIT_STALL;
                }
                break;

            case ROADWAY_SUBTYPE_PIT_EXIT:
                if (speed_subscript < MAX_SPEED_SUBSCRIPT)
                {
                    speed_subscript++;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    roadway_subtype = ROADWAY_SUBTYPE_RACING_LINE;
                }
                break;

            default:
                break;  //  Final (default) break for switch switch (roadway_subtype).
            }
            break;  //  break for case BLACK_GREEN_ACTION_CODE, switch (color_action_code)

        case BLACK_RED_ACTION_CODE:  //  Brake - decrement 1 speed_subscript.
            //  May need to add code for dependencies on ROADWAY_TYPE and ROADWAY_SUBTYPE.
            switch (roadway_subtype)
            {
            case ROADWAY_SUBTYPE_RACING_LINE:
                if (speed_subscript > SLOW_CURVE_SPEED_SUBSCRIPT)
                {
                    speed_subscript--;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
                break;

            case ROADWAY_SUBTYPE_PIT_ENTANCE:
                pit_location = PIT_STALL_LOCATION_LEFT;
                roadway_subtype = ROADWAY_SUBTYPE_PIT_LANE;
                break;

            case ROADWAY_SUBTYPE_PIT_LANE:
                if (car_id == CAR_COLOR_RED)
                {
                    if (pit_location == PIT_STALL_LOCATION_RIGHT)
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_RIGHT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    else
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_LEFT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    roadway_subtype = ROADWAY_SUBTYPE_PIT_STALL;
                }
                break;

            case ROADWAY_SUBTYPE_PIT_STALL:
                roadway_todo_queue[next_open_slot_idx] = STOP_IN_PIT_STALL;
                next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                roadway_subtype = ROADWAY_SUBTYPE_PIT_EXIT;
                break;

            default:
                break;  //  Final (default) break for switch switch (roadway_subtype).
            }
            break;  //  break for case BLACK_RED_ACTION_CODE, switch (color_action_code)

        case BLACK_YELLOW_ACTION_CODE:
            //  May need to add code for dependencies on ROADWAY_TYPE and ROADWAY_SUBTYPE.
            switch (roadway_subtype)
            {
            case ROADWAY_SUBTYPE_PIT_LANE:
                if (car_id == CAR_COLOR_YELLOW)
                {
                    if (pit_location == PIT_STALL_LOCATION_RIGHT)
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_RIGHT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    else
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_LEFT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    roadway_subtype = ROADWAY_SUBTYPE_PIT_STALL;
                }
                break;
            }
            break;

        case BLACK_BLUE_ACTION_CODE:
            //  May need to add code for dependencies on ROADWAY_TYPE and ROADWAY_SUBTYPE.
            switch (roadway_subtype)
            {
            case ROADWAY_SUBTYPE_PIT_LANE:
                if (car_id == CAR_COLOR_BLUE)
                {
                    if (pit_location == PIT_STALL_LOCATION_RIGHT)
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_RIGHT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    else
                    {
                        roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_STALL_LEFT;
                        next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    }
                    roadway_subtype = ROADWAY_SUBTYPE_PIT_STALL;
                }
                break;
            }
            break;

        case BLUE_GREEN_ACTION_CODE:  //  Lane change to right.
            if (pit_entrance_ahead == FALSE)
            {
                if (use_passing_lanes & USE_ANY_ALL_RIGHT_PASSING_LANES)
                {
                    roadway_todo_queue[next_open_slot_idx] = LANE_CHG_RIGHT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            else  //  i.e. (pit_entrance_ahead == TRUE)
            {
                pit_entrance_ahead = FALSE;  //  It is no longer ahead; we've
                                             //  reached it, and will soon be
                                             //  beyond it.
                if (pit_this_lap == TRUE)
                {
                    roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_LANE_RIGHT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    pit_this_lap = FALSE;
                    speed_subscript = PIT_LANE_SPEED_SUBSCRIPT;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            break;

        case BLUE_RED_ACTION_CODE:  //  Lane change to left.
            if (pit_entrance_ahead == FALSE)
            {
                if (use_passing_lanes & USE_ANY_ALL_LEFT_PASSING_LANES)
                {
                    roadway_todo_queue[next_open_slot_idx] = LANE_CHG_LEFT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            else  //  i.e. (pit_entrance_ahead == TRUE)
            {
                pit_entrance_ahead = FALSE;  //  It is no longer ahead; we've
                                             //  reached it, and will soon be
                                             //  beyond it.
                if (pit_this_lap == TRUE)
                {
                    roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_LANE_LEFT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    pit_this_lap = FALSE;
                    speed_subscript = PIT_LANE_SPEED_SUBSCRIPT;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            break;

        case BLUE_YELLOW_ACTION_CODE:  //  Lane change by going straight.
            if (pit_entrance_ahead == FALSE)
            {
                if (use_passing_lanes & USE_ANY_ALL_STRAIGHT_PASSING_LANES)
                {
                    roadway_todo_queue[next_open_slot_idx] = LANE_CHG_STRAIGHT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            else  //  i.e. (pit_entrance_ahead == TRUE)
            {
                pit_entrance_ahead = FALSE;  //  It is no longer ahead; we've
                                             //  reached it, and will soon be
                                             //  beyond it.
                if (pit_this_lap == TRUE)
                {
                    roadway_todo_queue[next_open_slot_idx] = ENTER_PIT_LANE_STRAIGHT;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                    pit_this_lap = FALSE;
                    speed_subscript = PIT_LANE_SPEED_SUBSCRIPT;
                    roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
                    next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
                }
            }
            break;

        case YELLOW_GREEN_ACTION_CODE:  //  Pit entrance ahead.
            //  May need to add code for dependencies on ROADWAY_TYPE and ROADWAY_SUBTYPE.
            pit_entrance_ahead = true;
            break;

        case YELLOW_BLUE_ACTION_CODE:  //  Sharp curve ahead.
            speed_subscript = SLOW_CURVE_SPEED_SUBSCRIPT;
            roadway_todo_queue[next_open_slot_idx] = SPEED_SUBSCRIPT_CHANGE;
//            roadway_todo_queue[next_open_slot_idx] = STOP_IN_PIT_STALL;
            next_open_slot_idx = (next_open_slot_idx + 1) % ROADWAY_TODO_QUEUE_LENGTH;
            break;

/*
        case RED_GREEN_ACTION_CODE:  //  Enter test mode.
            //  Code to enter test mode may be executed here or in the line_follower_pro task.
            take_no_action = TRUE;
            cdtrace_idx = 0;
            break;

        case GREEN_RED_ACTION_CODE:  //  Exit test mode.
            //  Code to enter test mode may be executed here or in the line_follower_pro task.
            take_no_action = FALSE;
            break;
*/
        default:
            break;
        }

        if (roadway_idx > 62)
        {
            break;
        }               
    }

    line_color_detect_task_complete = TRUE;

}  //  End  -  task  -  line_color_detect