/*
* Track Line.ic
*
* This program tracks a black line using IR sensors
* in ports 0 and 1.
*/


#use "botball.ic"


//Moves the bot forward
void forwardon(float distance, int port1, int port2, int speed)
{
    mrp(port1, speed, (long)(distance*(129.275)));
    mrp(port2, speed, (long)(distance*(129.275)));
}            

//Makes motors continue spinning until motors finish rotations
void forwardoff(int port1, int port2)
{
    bmd(port1);
    bmd(port2);
}

//Rotations is (number of rotations) * three hundred
void spinningpieon(int port, float rotations)
{
    //Rotates a spinning wheel
    mrp(port, 1199, (long) (rotations * 300.0));
}

//If direction is "1" then turn right
//If direction is "-1" then turn left
//Turns the bot
void turn(float distance, int port1, int port2, int speed, int direction)
{
    if(direction == 1)
      {
        mrp(port1, speed, (long)(distance*(129.275)));
        mrp(port2, speed, -((long)(distance*(129.275))));
        bmd(port1);
        bmd(port2);
    }
    if(direction == -1)
      {
        mrp(port1, speed, -((long)(distance*(129.275))));
        mrp(port2, speed, (long)(distance*(129.275)));
        bmd(port1);
        bmd(port2);
    }
}

//Checks IR readings for the ones we want
int IRcheck(int port)
{
    int check;
    check = analog(port);
    if(check >= 200)
      {
        return 1;
    }
    else
      {
        return 0;
    }
}

//Put IR sensors in ports 0 and 1
//Port 0 is the sensor in front of the wheel

//Line tracking function
void line()
{
    int endloop;
    endloop = 0;
    //Bot moves forward until IR sensor finds something
    forwardon(20.0, 1, 2, 800);
    while(IRcheck(2) == 0 && IRcheck(3) == 0)
      {
        msleep(1L);
    }
    ao();
    /* Now that it's found something, it moves forward
     * until the sensor no longer sees the line, then 
     * corrects until boths sensors are on the line
     */
    while(endloop == 0)
      {
        forwardon(20.0, 1, 2, 800);
        while(IRcheck(2) == 1)
          {
            msleep(1L);
        }
        ao();
        while(IRcheck(2) == 0)
          {
            turn(.5, 1, 2, 999, 1);
        }
        if(IRcheck(3) == 1 && IRcheck(2) == 1)
          {
            int x;
            x = 0;
            forwardon(300000.0, 1, 2, 999);
            //Keeps repeating above
            while(IRcheck(3) == 1 && IRcheck (2) == 1)
              {
                if(x == 1)
                  {
                    forwardon(300000.0, 1, 2, 999);
                    x = 0;
                }  
                msleep(1L);
                if(IRcheck(3) == 0 || IRcheck(2) == 0)
                  {
                    ao();
                    while(IRcheck(2) == 0)
                      {
                        x = 1;
                        turn(.5, 1, 2, 999, 1);
                    }
                    while(IRcheck(3) == 0)
                      {
                        x = 1;
                        turn(.5, 1, 2, 999, -1);
                    }
                    
                }
            }
            ao();
            endloop = 1;
        }    
    }   
}

//I hate assembly line code
void bline()
{
    forwardon(22.0, 1, 2, 999);
    forwardoff(1, 2);
    turn(4.0, 1, 2, 999, -1);
    forwardon(46.0, 1, 2, 999);
    forwardoff(1, 2);
    
}

void spinningpieoff(int port)
{
    bmd(port);
}



void main()
{
    bline();
    line();
    ao();
}
//Done!