// Spiral Search
// This program executes a spiral search looking for a
// color blob from a downward looking camera.
// The blob color comes from model model 0
// Blob must be 1,000 pixels or greater
// Vision screen is 356x292 (103,952 pixels)
// Center of x dimmension is 178
// Center of y dimmension is 146
// Left motor is 3, right motor is 1

#use "xbccamlib.ic"
#define myColor 0  // Define color model 
// (default: 0=orange, 1=yellow, 2=green)

#define right 1
#define left 3
int velocity=100;  // Left motor speed
int direction=1;   // Spiral direction (1=out, 0=in)
int angle_left=0;  // Blob left off center
int angle_right=0; // Blob right off center

void main() {
    init_camera();
    display_clear();
    printf("Spiral Search\n");
    sleep(1.0);
    
    track_set_minarea(1000); 
    while(!a_button()) {
        track_update();
        display_clear();
        printf("Press A button to start\n");
        printf(" Hold B button to stop\n");
        printf(" count=%d\n x=%d\n size=%d\n",
               track_count(myColor),track_x(myColor,0),track_size(myColor,0));
    }
    beep();
    sleep(1.0);
    
    track_set_minarea(1000); // Set minimum size of blob
    while(!b_button()) {
        track_update(); // Update blob data
        display_clear();
        printf(" count=%d\n x=%d\n size=%d\n",
               track_count(myColor),track_x(myColor,0),track_size(myColor,0));
        printf(" velocity=%d, direction=%d\n", velocity, direction); 
        if (track_count(myColor)>0) { 
            // Blob found
            mav(left,0); mav(right,0); // Stop
            beep(); sleep(0.1); beep(); sleep(0.1); beep();
            sleep(1.0);
            mav(left,400); // Mark the spot
            sleep(0.2);
            mav(left,0); 
            sleep(0.2);
            mav(left,-400); 
            sleep(0.2);
            mav(left,0);
            beep(); sleep(0.1); beep(); sleep(0.1); beep();
        } else { 
            // Blob not found, scan for blob 
            if (direction==1) {
                velocity=velocity+1;
                if (velocity>250) direction=0;
            } else {
                velocity=velocity-1;
                if (velocity<100) direction=1;
            }
            mav(left,velocity);
            mav(right,400);
            sleep(0.2);
        }
    }
    ao();
    beep(); 
    printf("Done!\n");
}