//Computer Science Club
//Fire Find Procedure


//analog(0) right et sensor
//analog(1) left et sensor
//analog(2) front tophat
//analog(3) right tophat
//analog(4) left tophat 
//motor 0 is right
//motor 2 is left

void main(){        
    moveforwardroom();     
    rotatefull();    
    beep();
}

void moveforwardroom(){
    float ticksPerSecond = (1000.0 / (6.35 * 3.14));
    long distance = (long)ticksPerSecond * ((long)frontSonar() / 20L);
    mrp(0, 1000, distance);
    mrp(2, 1000, distance);
    while(get_motor_done(0) == 0 ){
    }    
}

void rotatefull(){    
    int speed = 1000;
    long distance = 2803L;
    mrp(0, speed, distance);
    mrp(2, speed, -1L * distance);
    
    while(get_motor_done(0) == 0){
        
        if(seeflame() == 1){
            ao();
            findflame();
            break;    
        }
    }
}

int seeflame(){
    if ( (frontT() < 230) || (rightT() < 230) || (leftT() < 230) )
      return 1;
    return 0;
}

void findflame(){
    while(0 == 0){
        if(leftT() < frontT())
          turnleftflame();
        if(rightT() < frontT())
          turnrightflame();
        fd(0);
        fd(2);
        if(frontT() < 50)
          break;
    }
    pointatflame();
    ao();
}

void pointatflame(){
    int range = 30; 
    motor(0, 66);
    motor(2, -66);
    while ( leftT() < rightT() - range) {}
    motor(0, -66);
    motor(2, 66);
    while ( rightT() < leftT() - range){}
}         


void turnleftflame(){
    motor(0, 66);
    motor(2, -66);
    while(leftT() < frontT()){
    }    
}

void turnrightflame(){
    motor(0, -66);
    motor(2, 66);
    while(rightT() < frontT()){
    }
}

int frontT(){
    return analog(2);
}

int rightT(){
    return analog(3);
}

int leftT(){
    return analog(4);
}

int frontSonar(){
    return sonar(8);
}