//************************************************
// Navbot(w recenter_pos).nqc
// Author: Volga Aksoy
// Date: 11/12/2002
//
// This program implements an AI for the Navbot, which uses an IRRD-T sensor.
// The AI allows the Navbot to start and finish as if it went on the same path
// (defined by line along its starting direction) while it avoids obstacles
// along its path.
//************************************************

//sensor and motor + direction assignments
#define LEFT_STOP   SENSOR_1
#define RIGHT_STOP  SENSOR_3
#define DISTANCE    SENSOR_2
#define LEFT_DRIVE  OUT_A
#define RIGHT_DRIVE OUT_C
#define RADAR_TURN  OUT_B
#define FORWARD     OUT_FWD
#define REVERSE     OUT_REV

//motion + radar macros
#define GO_FORWARD  SetDirection(LEFT_DRIVE+RIGHT_DRIVE, FORWARD); On(LEFT_DRIVE+RIGHT_DRIVE)
#define GO_REVERSE  SetDirection(LEFT_DRIVE+RIGHT_DRIVE, REVERSE); On(LEFT_DRIVE+RIGHT_DRIVE)
#define SPIN_LEFT   SetDirection(RIGHT_DRIVE, FORWARD); SetDirection(LEFT_DRIVE, REVERSE); On(LEFT_DRIVE+RIGHT_DRIVE)
#define SPIN_RIGHT  SetDirection(LEFT_DRIVE, FORWARD); SetDirection(RIGHT_DRIVE, REVERSE); On(LEFT_DRIVE+RIGHT_DRIVE)
#define TURN_LEFT   Off(LEFT_DRIVE); SetDirection(RIGHT_DRIVE, FORWARD); On(RIGHT_DRIVE)
#define TURN_RIGHT  Off(RIGHT_DRIVE); SetDirection(LEFT_DRIVE, FORWARD); On(LEFT_DRIVE)
#define STOP_MOTION Off(LEFT_DRIVE+RIGHT_DRIVE)
#define TURN_RADAR_LEFT   OnRev(RADAR_TURN)
#define TURN_RADAR_RIGHT  OnFwd(RADAR_TURN)

//events
#define LEFT_STOPPED    0
#define RIGHT_STOPPED   1

//direction for radar positions (45 only means - between forward and full)
#define DIR_FORWARD     0
#define DIR_LEFT        1
#define DIR_LEFT_45     2
#define DIR_LEFT_PROX   3
#define DIR_RIGHT       4
#define DIR_RIGHT_45    5
#define DIR_RIGHT_PROX  6

//time to wait for radar to turn back to forward from left or right position
#define WAIT_FULL       17
//time to wait for radar to turn x degrees (used in proximity check)
#define WAIT_45         9
#define WAIT_PROX       5
#define WAIT_TURN_45    270
#define WAIT_SPIN_45    140

//macros for turning radar
#define SET_FORWARD_FROM_FULL   Wait(WAIT_FULL); Off(RADAR_TURN)
#define SET_FORWARD_FROM_45     Wait(WAIT_45); Off(RADAR_TURN)

//defines how close an object has to be to be "too close" 5->24 inches
#define RADAR_DIST_THRESH    30

//defines how much bot turns each time in degrees
#define TURN_DEG_INC    45

//major tasks bot does
#define ROAMING         0
#define CENTER_DIR      1
#define CENTER_POS      2

//trig convertion for 45 degrees (x100 precision - root 2 = ~1.41)
#define SIN_45          71
//threshold for determining if bot is in center line threshold ( > SIN_45/2)
#define CENTER_POS_THRESH   (SIN_45/2)+5

//Wait for radar to measure distance
#define WAIT_MEASURE    10

int radarDir = -1;  //direction unknown (will be reset at ResetRadarDir()
int curDir = 0;
int lastRead;
int closest = -1;
int farthest = 101;
int farthestDir = -1;
int recenterDir = 0;    //0 means bot is looking at initial direction
                        //+ve means bot is looking left
                        //-ve means bot is looking right
int recenterPos = 0;    //0 means bot is on center line
                        //+ve means bot is on left of center line
                        //-ve means bot is on right of center line
int curTask = ROAMING;  //start by "roaming"
                        
task main ()
{
    //initializations
    SetPower(OUT_B, OUT_FULL);
    SetPower(OUT_A, OUT_FULL);
    SetPower(OUT_C, OUT_FULL);

    SetSensor(LEFT_STOP, SENSOR_TOUCH);
    SetSensor(RIGHT_STOP, SENSOR_TOUCH);
    SetSensor(DISTANCE, SENSOR_LIGHT);

    SetEvent(LEFT_STOPPED, LEFT_STOP, EVENT_TYPE_PRESSED);
    SetEvent(RIGHT_STOPPED, RIGHT_STOP, EVENT_TYPE_PRESSED);

    start Roam;
}

task Roam()
{
    ResetRadarDir();
    PlaySound(SOUND_DOUBLE_BEEP);
    Wait(50);

    while(1)
    {
            GO_FORWARD;

            //direction is reset, thus calculate centerline pos
            CalcPos();
            
            if(radarDir != DIR_LEFT && radarDir != DIR_RIGHT)
            {
                LookLeft(); //Used for resetting radar direction
            }
            ScanProx();       //scan for obstacles

            switch(curTask)
            {
            case ROAMING:
                break;
            
            case CENTER_DIR:
                STOP_MOTION;
                ScanRecenterDir(); //see if you can turn back to old direction
                break;
                
            case CENTER_POS:
                STOP_MOTION;
                ScanRecenterPos(); //see if you can turn back to old direction
                break;
            }

        //if(recenterDir == 0)  curTask = CENTER_POS;
        //if(recenterPos == 0)  curTask = CENTER_DIR;
        if( recenterDir == 0 &&
            recenterPos < CENTER_POS_THRESH &&
            recenterPos > -CENTER_POS_THRESH )
        {
            curTask = ROAMING;
        }
        
    }   //end while
}

sub ResetRadarDir()
{
    TURN_RADAR_LEFT;
    until(LEFT_STOP == 1){}
    TURN_RADAR_RIGHT;
    Wait(WAIT_FULL);
    Off(RADAR_TURN);

    radarDir = DIR_FORWARD;
}

sub ScanProx()
{
    LookLeftProx();
    Measure();
    if(closest < lastRead)
        closest = lastRead;

    if(farthest > lastRead)
    {
        farthest = lastRead;
        farthestDir = DIR_LEFT_45;
    }

    LookForward();
    Measure();
    if(lastRead == 0)   //if forward is empty 
        lastRead = -1;  //give priority to forward direction
    
    if(closest < lastRead)
        closest = lastRead;

    if(farthest > lastRead)
    {
        farthest = lastRead;
        farthestDir = DIR_FORWARD;
    }

    LookRightProx();
    Measure();
    if(closest < lastRead)
        closest = lastRead;

    if(farthest > lastRead)
    {
        farthest = lastRead;
        farthestDir = DIR_RIGHT_45;
    }
    
    //Scan Complete - react to obstacles

    if(closest > RADAR_DIST_THRESH) // means object is closer enough
    {
        if(closest == 100)
        {
            STOP_MOTION;
            PlaySound(SOUND_DOWN);
            PlaySound(SOUND_DOWN);
        }
        else if (farthest > RADAR_DIST_THRESH) //if the farthest object is also close
        {   //then go backwards
            PlaySound(SOUND_LOW_BEEP);
            GO_REVERSE;
            Wait(100);
            SPIN_LEFT;
            Wait(WAIT_SPIN_45*4);
            STOP_MOTION;
            recenterDir -= TURN_DEG_INC*4;
            curTask = CENTER_DIR;   //switch task to recenter heading
        }
        else if (farthestDir == DIR_LEFT_45)
        {
            SPIN_LEFT;
            Wait(WAIT_SPIN_45);
            STOP_MOTION;
            recenterDir += TURN_DEG_INC;
            curTask = CENTER_DIR;   //switch task to recenter heading
        }
        else if (farthestDir == DIR_RIGHT_45)
        {
            SPIN_RIGHT;
            Wait(WAIT_SPIN_45);
            STOP_MOTION;
            recenterDir -= TURN_DEG_INC;
            curTask = CENTER_DIR;   //switch task to recenter heading
        }
        else if (farthestDir == DIR_FORWARD)
        {
            //keep going forward
        }
    }

    //if bot is close enough to the center line and is not heading forward
    if(recenterPos < CENTER_POS_THRESH &&  recenterPos > -CENTER_POS_THRESH
        && recenterDir != 0)
    {
        curTask = CENTER_DIR;   //switch task to recenter heading
    }
    
    //reset vars
    closest = -1;
    farthest = 101;

    //bot reaction complete
}

void ScanAndTurnLeft45()
{
    LookLeft45();
    Wait(WAIT_MEASURE);
    if(DISTANCE < RADAR_DIST_THRESH)   //if left is empty then turn left 45 deg
    {
        LookLeft();
        Wait(WAIT_MEASURE);
        if(DISTANCE < RADAR_DIST_THRESH)   //if left is empty then turn left 45 deg
        {
            SPIN_LEFT;
            Wait(WAIT_SPIN_45);
            STOP_MOTION;
            recenterDir += TURN_DEG_INC;   //bot has turned closer to initial direction
        }
    }
}

void ScanAndTurnRight45()
{
    LookRight45();
    Wait(WAIT_MEASURE);
    if(DISTANCE < RADAR_DIST_THRESH)   //if right is empty then turn left 45 deg
    {
        LookRight();
        Wait(WAIT_MEASURE);
        if(DISTANCE < RADAR_DIST_THRESH)   //if right is empty then turn left 45 deg
        {
            SPIN_RIGHT;
            Wait(WAIT_SPIN_45);
            STOP_MOTION;
            recenterDir -= TURN_DEG_INC;   //bot has turned closer to initial direction
        }
    }
}

sub ScanRecenterDir()
{
    PlaySound(SOUND_FAST_UP);
    if(recenterDir == 0)
    {
        curTask = CENTER_POS;
        return;
    }
    else if (recenterDir < 0)  //has bot previously turned right?
    {
        ScanAndTurnLeft45();
    }
    else if (recenterDir > 0)  //has bot previously turned left?
    {
        ScanAndTurnRight45();
    }

//    if(recenterDir == 0) curTask = CENTER_POS;
}

sub ScanRecenterPos()
{
    PlaySound(SOUND_DOWN);
    if(     recenterPos < CENTER_POS_THRESH
        &&  recenterPos > -CENTER_POS_THRESH) //if bot close enough to line
    {
        curTask = CENTER_DIR;   //let bot center its heading
        return;     //don't do anything
    }
    else if(recenterPos < 0)    //if bot is on right-side of the initial line
    {
        if(recenterDir == TURN_DEG_INC) //if bot is heading 45 degrees to left
            return;
        else if(recenterDir < TURN_DEG_INC) //if bot is heading < 45 deg left
        {
            ScanAndTurnLeft45();
        }
        else if(recenterDir > TURN_DEG_INC) //if bot is heading > 45 deg left
        {
            ScanAndTurnRight45();
        }
    }
    else if(recenterPos > 0)
    {
        if(recenterDir == -TURN_DEG_INC) //if bot is heading 45 degrees to right
            return;
        else if(recenterDir < -TURN_DEG_INC) //if bot is heading < 45 deg right
        {
            ScanAndTurnLeft45();
        }
        else if(recenterDir > -TURN_DEG_INC) //if bot is heading > 45 deg right
        {
            ScanAndTurnRight45();
        }
    }
}

sub CalcPos()
{
    recenterDir = recenterDir % 360;    //eliminate 360x
    if(recenterDir == 0)
    {
        //no change - bot going forward
    }
    else if(recenterDir % 180 == 0)     //eliminate 180x
    {
        //no change - bot going backwards
    }
    else if(recenterDir % 90 == 0)      //eliminate 90x
    {
        if(recenterDir > 0)         recenterPos += 100;
        else if(recenterDir < 0)    recenterPos -= 100;
    }
    else if(recenterDir % 45 == 0)      //eliminate 45x
    {
        if(recenterDir > 0)         recenterPos += SIN_45;
        else if(recenterDir < 0)    recenterPos -= SIN_45;
    }
}

void LookLeft()
{
    TURN_RADAR_LEFT;
    until(LEFT_STOP == 1){}
    Off(RADAR_TURN);
    radarDir = DIR_LEFT;
}

void LookRight()
{
    TURN_RADAR_RIGHT;
    until(RIGHT_STOP == 1){}
    Off(RADAR_TURN);
    radarDir = DIR_RIGHT;
}

void LookLeft45()
{
    switch(radarDir)
    {
    case DIR_FORWARD:
        TURN_RADAR_LEFT;
        Wait(WAIT_45);
        break;

    case DIR_LEFT:
        TURN_RADAR_RIGHT;
        Wait(WAIT_FULL-WAIT_45);
        break;

    case DIR_LEFT_45:
        break;

    case DIR_LEFT_PROX:
        TURN_RADAR_LEFT;
        Wait(WAIT_45-WAIT_PROX);
        break;

    case DIR_RIGHT:
        TURN_RADAR_LEFT;
        Wait(WAIT_FULL+WAIT_45);
        break;

    case DIR_RIGHT_45:
        TURN_RADAR_LEFT;
        Wait(WAIT_45+WAIT_45);
        break;

    case DIR_RIGHT_PROX:
        TURN_RADAR_LEFT;
        Wait(WAIT_45+WAIT_PROX);
        break;
    }

    Off(RADAR_TURN);
    radarDir = DIR_LEFT_45;
}

void LookRight45()
{
    switch(radarDir)
    {
    case DIR_FORWARD:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45);
        break;

    case DIR_LEFT:
        TURN_RADAR_RIGHT;
        Wait(WAIT_FULL+WAIT_45);
        break;

    case DIR_LEFT_45:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45+WAIT_45);
        break;

    case DIR_LEFT_PROX:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45+WAIT_PROX);
        break;

    case DIR_RIGHT:
        TURN_RADAR_LEFT;
        Wait(WAIT_FULL-WAIT_45);
        break;

    case DIR_RIGHT_45:
        break;

    case DIR_RIGHT_PROX:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45-WAIT_PROX);
        break;
    }

    Off(RADAR_TURN);
    radarDir = DIR_RIGHT_45;
}

void LookLeftProx()
{
    switch(radarDir)
    {
    case DIR_FORWARD:
        TURN_RADAR_LEFT;
        Wait(WAIT_PROX);
        break;

    case DIR_LEFT:
        TURN_RADAR_RIGHT;
        Wait(WAIT_FULL-WAIT_PROX);
        break;

    case DIR_LEFT_45:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45-WAIT_PROX);
        break;

    case DIR_LEFT_PROX:
        break;

    case DIR_RIGHT:
        TURN_RADAR_LEFT;
        Wait(WAIT_FULL+WAIT_PROX);
        break;

    case DIR_RIGHT_45:
        TURN_RADAR_LEFT;
        Wait(WAIT_45+WAIT_PROX);
        break;

    case DIR_RIGHT_PROX:
        TURN_RADAR_LEFT;
        Wait(WAIT_PROX+WAIT_PROX);
        break;
    }

    Off(RADAR_TURN);
    radarDir = DIR_LEFT_PROX;
}

void LookRightProx()
{
    switch(radarDir)
    {
    case DIR_FORWARD:
        TURN_RADAR_RIGHT;
        Wait(WAIT_PROX);
        break;

    case DIR_LEFT:
        TURN_RADAR_RIGHT;
        Wait(WAIT_FULL+WAIT_PROX);
        break;

    case DIR_LEFT_45:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45+WAIT_PROX);
        break;

    case DIR_LEFT_PROX:
        TURN_RADAR_RIGHT;
        Wait(WAIT_PROX+WAIT_PROX);
        break;

    case DIR_RIGHT:
        TURN_RADAR_LEFT;
        Wait(WAIT_FULL-WAIT_PROX);
        break;

    case DIR_RIGHT_45:
        TURN_RADAR_LEFT;
        Wait(WAIT_45-WAIT_PROX);
        break;

    case DIR_RIGHT_PROX:
        break;
    }

    Off(RADAR_TURN);
    radarDir = DIR_RIGHT_PROX;
}

void LookForward()
{
    switch(radarDir)
    {
    case DIR_LEFT:
        TURN_RADAR_RIGHT;
        Wait(WAIT_FULL);
        break;

    case DIR_LEFT_45:
        TURN_RADAR_RIGHT;
        Wait(WAIT_45);
        break;

    case DIR_LEFT_PROX:
        TURN_RADAR_RIGHT;
        Wait(WAIT_PROX);
        break;

    case DIR_RIGHT:
        TURN_RADAR_LEFT;
        Wait(WAIT_FULL);
        break;

    case DIR_RIGHT_45:
        TURN_RADAR_LEFT;
        Wait(WAIT_45);
        break;

    case DIR_RIGHT_PROX:
        TURN_RADAR_LEFT;
        Wait(WAIT_PROX);
        break;

    case DIR_FORWARD:
        break;

    default:
        break;
    }

    Off(RADAR_TURN);
    radarDir = DIR_FORWARD;
}

void Measure()
{
    Wait(WAIT_MEASURE);
    lastRead = DISTANCE;
}