#use "cmucamlib.ic"

#define SERVO_MAX 3800
#define SERVO_MIN 1100
#define SERVO_DELTA 30

//Possible Actions
#define NOTHING 0
#define SING_QUIT 1
#define PLAY_NOTE 2
#define LOOK_LEFT 3
#define LOOK_RIGHT 4

//Suggestions from each behavior
int searchCommand = NOTHING;
int trackCommand = NOTHING;
int singCommand = NOTHING;

int done = 0;

void main()
{
    //Init Camera
    init_camera(); 
    //Check White Value
    clamp_camera_yuv();
    //Turn on servos   
    init_expbd_servos(1);
    
    start_process(search_behavior());
    start_process(track_red_behavior());
    start_process(sing_and_quit_behavior());
    start_process(arbitrator());
    
    while(done==0) 
      defer();
    init_expbd_servos(0);
}

//looks to the right and left repeatedly
//once per 1/10th of a second interval
void search_behavior()
{
    int lastCommand = LOOK_LEFT;
    float lastCommandTime = seconds();
    float currentTime;
    
    searchCommand = LOOK_LEFT;
    while(done==0)
      {
        currentTime = seconds();
        if(currentTime - lastCommandTime < .1)
          searchCommand = NOTHING;
        else
          {
            if(servo0 - SERVO_DELTA < SERVO_MIN)
              searchCommand = LOOK_RIGHT;
            else if(servo0 + SERVO_DELTA > SERVO_MAX)
                searchCommand = LOOK_LEFT;
              else
                searchCommand = lastCommand;
            
            lastCommand = searchCommand;
            lastCommandTime = currentTime;
        }
        defer();
    }
}

void track_red_behavior()
{
    int whereIsRed = 0;
    while(done==0)
      {
        trackRaw(200, 255, //Red (Min,max)
                 0, 50,    //Green
                 0, 50);   //Blue
        if(track_confidence > 40)
          {
            whereIsRed = track_x;
            printf("X=%d  Y=%d\n", whereIsRed, track_y);
            if(whereIsRed > 9)
              trackCommand = LOOK_RIGHT;
            else if(whereIsRed < -9)
                trackCommand = LOOK_LEFT;
              else
                trackCommand = PLAY_NOTE;
        }
        else
          trackCommand = NOTHING;
        defer();
    }
}

void sing_and_quit_behavior()
{
    while(done==0)
      {
        if(stop_button())
          singCommand = SING_QUIT;
        else
          singCommand = NOTHING;
        defer();
    }
}

void arbitrator()
{
    int selectedCommand = NOTHING;
    while(done==0)
      {
        selectedCommand = searchCommand;
        
        if(trackCommand!=NOTHING)  
          selectedCommand = trackCommand;
        
        if(singCommand!=NOTHING)
          selectedCommand = singCommand;
        
        doCommand(selectedCommand);
        defer();
    }
}

void doCommand(int command)
{
    if(command==NOTHING)
      return;
    if(command==PLAY_NOTE)
      tone(1500.0,.1);
    if(command==SING_QUIT)
    {
        servo0 = 2100;
        tone(1000.0, .1);
        tone(1500.0, .1);
        tone(2000.0, .1);
        tone(2500.0, .2);
        tone(2000.0, .1);
        tone(2500.0, .3);
        done = 1;
    }
    if(command==LOOK_RIGHT && servo0 < SERVO_MAX)
      servo0 += SERVO_DELTA;
    if(command==LOOK_LEFT && servo0 > SERVO_MIN)
      servo0 -= SERVO_DELTA;
}