// scanbot3.nqc
//    Scanbot program with ambient light threshold

// sensors
#define EYE   SENSOR_3
#define ANGLE SENSOR_1

// motors
#define LEFT  OUT_A
#define HEAD  OUT_B
#define RIGHT OUT_C

// other constants
#define SLOP 3
#define TURN 2

#define CENTER 53
#define SWEEP  48    // 135 degrees

#define AMBIENT_MARGIN 2  // a little extra range for ambient light

// variables
int target, max;
int ambient;

void align()
{
    int previous;
    
    OnRev(HEAD); // begin turning left
    
    do
    {
        previous = ANGLE; // save current position
        Wait(10);
    } while(ANGLE != previous); // if position changed, keep going
    
    
    Off(HEAD);
    ClearSensor(ANGLE); // make this position "0"    
}


void check()
{
    int level = EYE;
    if (level > max)
    {
        target = ANGLE;
        max = level;
    }
}


void scan()
{
    max = 0;

    if (ANGLE > CENTER)
    {
        // look left
        OnRev(HEAD);
        while(ANGLE > CENTER-SWEEP)
            check();
    }
    else
    {
        // look right
        OnFwd(HEAD);
        while(ANGLE < CENTER+SWEEP)
            check();
    }
    
    Off(HEAD);
}


void steer()
{
    int spinTime;
    
    if (max <= ambient)
    {
        Off(LEFT + RIGHT);
        return;
    }
    
    target -= CENTER;
    
    if (target > SLOP)
    {
        OnFwd(LEFT);
        OnRev(RIGHT);
        spinTime = TURN * target;
    }
    else if (target < -SLOP)
    {
        OnFwd(RIGHT);
        OnRev(LEFT);
        spinTime = -TURN * target;
    }
    else
    {
        // we're on target...go straight
        OnFwd(LEFT+RIGHT);
        return;  // no need to steer, just return now
    }

    Wait(spinTime);
    Off(LEFT+RIGHT);
}


task main()
{
    // configure sensors
    SetSensor(EYE, SENSOR_LIGHT);
    SetSensor(ANGLE, SENSOR_ROTATION);
    
    align();
    scan();
    ambient = max + AMBIENT_MARGIN;
    
    // look and steer...
    while(true)
    {
        scan();
        steer();
    }
}