/*
 * Extinguisher Bot
 *
 * Matt Ferlo & Jordan Taggart
 *
 * CS154 Robotics, Spring 2004
 *
 */

int CANDLE_FOUND = 0;
int NO_CANDLE = 1;
int COULDNT_REFIND_CANDLE = 2;
int BUMPED = 3;

int LMotor = 2;
int RMotor = 3;

int LLight = 5;
int RLight = 6;

int LFbump = 8;
int RFbump = 7;

int LFSbump = 10;
int RFSbump = 11;

int LSbump = 14;
int RSbump = 15;

int Fan = 1;

void forward(float time, int speed)
{
    int i;
    
    float end_time = seconds() + time;
    
    while (1)
    {
        if (stop_button())
        {
            ao();
            break;
        }
        
        if (seconds() >= end_time)
            break;
        
        motor(RMotor, 10 * speed);
        motor(LMotor, 5 * speed);
        
        // Left Front is bumped -- back up and turn right
        if (digital(LFbump))
        {
            bk(RMotor);
            bk(LMotor);
            msleep(50L);
            ao();
            right(60);
        }
        
        // Right front is bumped -- back up and turn left
        if (digital(RFbump))
        {
            bk(RMotor);
            bk(LMotor);
            msleep(50L);
            ao();
            left(30);
        }
        
        if (digital(LFSbump))
            right(10);
        
        
        if (digital(RFSbump))
            left(10);
        
        if (digital(LSbump))
            right(20);
        
        if (digital(RSbump))
            left(20);
    }
    ao();
}


void right(int degrees)
{
    int i;
    for (i = 0; i < degrees; i++)
    {
        fd(LMotor);
        bk(RMotor);
        msleep(10L);
        if (digital(RFSbump) || digital(RSbump))
        {
            forward(0.4, -10);
        }
    }
    
    ao();
}

void left(int degrees)
{
    int i;
    for (i = 0; i < degrees; i++)
    {
        fd(RMotor);
        bk(LMotor);
        
        msleep(10L);
        if (digital(LFSbump) || digital(LSbump))
        {
            forward(0.4, -10);
        }
    }
    
    ao();
}

void navigate()
{
    int i;
    int res;
    
    while (1)
    {
        printf("Navigating\n");
        forward(3.0, 10);
        
        printf("Looking for the candle\n");
        res = find_candle();
        if (res == CANDLE_FOUND)
            return;
        else if (res == NO_CANDLE)
            ;
        else if (res == COULDNT_REFIND_CANDLE)
        {
            if (find_candle() == CANDLE_FOUND)
                return;
        }
        else if (res == BUMPED)
        {
            ;
        }
        else
        {
            printf("WTF!\n");
            return;
        }
    }
}

int find_candle()
{
    int bump = 0;
    int lmin = 255;
    int lcur;
    
    int threshhold = 70;
    
    int i;
    
    fd(LMotor);
    bk(RMotor);
    
    for (i = 0; i < 370; i++)
    {
        if (digital(RSbump) || digital(RFSbump))
        {
            ao();
            bump = 1;
            break;
        }
        
        lcur = analog(LLight);
        if (lcur < lmin)
            lmin = lcur;
        msleep(10L);
    }
    
    ao();
    
    if (bump)
    {
        int dist = i;
        bk(LMotor);
        fd(RMotor);
        for (i = 0; i < dist; i++)
        {
            lcur = analog(LLight);
            if (lcur <= lmin + 20)
                break;
            msleep(10L);
        }
        
        ao();
        
        if (lcur > threshhold)
            return BUMPED;
        
        return CANDLE_FOUND;
    }
    
    if (lmin > threshhold)
    {
        printf("L: %d\n", lmin);
        return NO_CANDLE;
    }
    
    
    lmin += 20;
    
    bk(LMotor);
    fd(RMotor);
    
    for (i = 0; i < 370; i++)
    {
        lcur = analog(LLight);
        
        if (lcur <= lmin)
            break;
        
        msleep(10L);
    }
    
    ao();
    
    if (i == 370)
    {
        printf("Couldn't re-find bright spot!\n");
        return COULDNT_REFIND_CANDLE;
    }
    
    printf("Found (L: %d)\n", lmin);
    
    return CANDLE_FOUND;
}


void reacquire_candle()
{
    int lmin = 255;
    int lcur;
    int i;
    int right_turn_amt;
    
    motor(LMotor, -50);
    motor(RMotor, 50);
    
    for (i = 0; i < 30; i++)
    {
        if (digital(LSbump) || digital(LFSbump))
        {
            off(LMotor);
            off(RMotor);
            break;
        }
        msleep(10L);
    }
    
    motor(LMotor, 50);
    motor(RMotor, -50);
    
    for (i = 0; i < 60; i++)
    {
        if (digital(RSbump) || digital(RFSbump))
        {
            off(LMotor);
            off(RMotor);
            break;
        }
        
        lcur = analog(LLight);
        if (lcur < lmin)
            lmin = lcur;
        msleep(10L);
    }
    
    right_turn_amt = i;
    
    motor(LMotor, -50);
    motor(RMotor, 50);
    
    for (i = 0; i < right_turn_amt; i++)
    {
        if (analog(LLight) <= lmin + 5)
            break;
        
        msleep(10L);
    }
    
    off(LMotor);
    off(RMotor);
}

void extinguish()
{
    int i;
    
    while (analog(LLight) > 25)
    {
        forward(1.0, 2);
        reacquire_candle();
    }
    
    while (1)
    {
        fd(Fan);
        
        if (analog(LLight) > 15)
        {
            reacquire_candle();
        }        
        sleep(5.0);

        
        if (analog(LLight) > 15)
        {
            motor(LMotor, 10);
            motor(RMotor, 14);
            
            for (i = 0; i < 30; i++)
            {
                if ((digital(LFbump) || digital(RFbump)))
                {
                    off(LMotor);
                    off(RMotor);
                }
                msleep(20L);
            }

            off(LMotor);
            off(RMotor);
        }
        
        if (analog(LLight) > 125)
        {
            off(Fan);
            sleep(2.0);
            
            reacquire_candle();
            
            fd(Fan);
            sleep(5.0);
            
            if (analog(LLight) > 125)
            {
                ao();
                tone(400.0, 0.2);
                tone(440.0, 0.2);
                break;
            }
        }
    }
}

void main()
{
    printf("\n");
    
    while (! start_button())
        ;
    
    reset_system_time();
    
    navigate();
    extinguish();
    
}