#define armCenterPower  33
#define rotatePower 40

#define ultrasonicThreshold 80
#define quitAfterShot true

bool running;
bool found;
bool centered;

long sDest;
bool sStop;



sub wakeup() {
     PlayFileEx("t-deploy.rso", 6, false);
     SetSensorLowspeed (IN_4);

     RotateMotorEx(OUT_B,100,80,100,false,true);
     
     PlayFileEx("t-autosearch.rso", 6, false);
     
     running=true;
     found = false;
}

sub stopMotor() {
    Off(OUT_A);
}

sub resumeMotor() {
    RotateMotorEx(OUT_A, rotatePower, sDest - MotorTachoCount(OUT_A) ,100, false, sStop);
}

sub rotateMotor(int destangle, bool brake) {
    sDest = destangle;
    sStop = brake;

    TextOut(0,30, "P: ", true);
    TextOut(0, 10, "D: ", false);
    NumOut(20,30, MotorTachoCount(OUT_A),false);
    TextOut(20,20, "to", false);
    NumOut(20,10, sDest,false);

    resumeMotor();

}



task holdMotor() {
     while(running || !centered) {
                   //PlayToneEx(262,400,3,FALSE);
                    RotateMotorEx(OUT_B, armCenterPower,1,100,false,true);
                    RotateMotorEx(OUT_B, armCenterPower,-1,100,false,true);
     }
     
     PlayFileEx("t-disabled.rso", 6, false);
     Wait(500);
     RotateMotorEx(OUT_B, 50, -45,100,false, false);
}

task watchForEnd() {
     until(ButtonPressed(BTNCENTER,false)==1);
     running=false;
}

task bobHead() {
     until(running);
     
     centered=false;
     
     while(running) {

                    
                    until(!found);
                    rotateMotor(90,true);
                    Wait(100);

                    until(!found);
                    rotateMotor(-90,true);
                    Wait(100);
     }
     
     rotateMotor(0, true);
     centered=true;
}



sub fire() {
    OnFwd(OUT_C, 100);
    Wait(400);
    OnRev(OUT_C, 100);
    Wait(400);
    Coast(OUT_C);
}

task search() {
     until(running);

     while(running) {
                    until( SensorUS(IN_4) < ultrasonicThreshold);
                    PlayToneEx(400,400,3,FALSE);

                    stopMotor();
                    
                    found=true;

                    PlayFileEx("t-active.rso", 6, false);
                    Wait(800);
                    
                    fire();
                    
                    Wait(500);

                    resumeMotor();
                    
                    found=false;

                    if(quitAfterShot) running=false;

     }
}


task main() {
     Precedes (holdMotor, watchForEnd, bobHead, search);

     wakeup();

}