//#use "cmucamlib.ic"
///////////////////////////////////////////////////////////
// Project 3.01 : Hybrid Deliberative/Reactive Demonstration
//team 4
//author: Rahul Kotamaraju
//date: 04-28-2003
//-------------------------------------------------------------
//Phase 1:code to make the robot travel from current location (given)
//to the destination location (also given) in the form of XY 
//coords.
////////////////////////////////////////////////////////////

#define X 0
#define Y 1
#define LEFT 0
#define RIGHT 1
#define LEFT_MOTOR 0
#define RIGHT_MOTOR 1
#define NUM_BLOCKS 16
#define NUM_DESTINATIONS 4
#define TURN_TIME 1000L
#define EAST 0
#define WEST 1
#define NORTH 2
#define SOUTH 3
#define NO_TURN 0
#define TURN90 1
#define TURN180 2
#define STRAIGHT -1
#define ONEFOOTTICKS 119.
#define TURNTICKS 70
#define LEFT_ENCODER 0
#define RIGHT_ENCODER 1
#define STRAIGHT_SPEED 60
#define WAIT 1000L
#define LOW_SPEED 20
#define LEFT_IR 2
#define RIGHT_IR 3
#define LEFT_IR_THRESH
#define RIGHT_IR_THRESH
#define OPEN 0
#define LOCK 1
#define RELEASE 2
#define OPEN_CAGE 0
#define RELEASE_CAGE 3700 
#define LOCK_CAGE 2100
#define BLOCK 0
#define DEST 1
#define LIR 2
#define RIR 3
#define L_THRESHOLD 175
#define R_THRESHOLD 165
#define ALIGN_SPEED 13
#define ALIGN_SPEED_HI 28
#define ALIGN_THRESH 60.

//global variables related to the WORLD MODEL
float blockList[16][2]; //details of the orange blocks
float destList[4][2];
float currBotPos[2];     //current location of the bot
int currBotDir; //curr bot orientation 
float x[21],y[21];
int aligned=0;
int sensedTape = 0;
//////////////////////////////
//align bot with goal square
//////////////////////////////
int alignStatus[2];
int alignSpeed[2];
int threshold[2] = {L_THRESHOLD, R_THRESHOLD};

int readSensor(int s) {
    //read light sensor and apply threshold
    int j = analog(s + 2);
    if(s)
      printf("%d\n", j);
    if(j > threshold[s])
      return 1;
    return 0;
}

void alignMotor(int m) {
    //compenstae for weaker right-side motor
    alignSpeed[m] = ALIGN_SPEED;
    while(1) {  //loop until thread is killed
        //drive motor while not over tape
        while(alignStatus[m] == 0) {
            motor(m, alignSpeed[m]);
            defer();  //thread sleeps for ~50ms
            alignStatus[m] = readSensor(m);
        }
        off(m);
        //burst motor backwards briefly
        motor(m, -ALIGN_SPEED_HI);
        msleep(100L);
        off(m);
        //set other motor to higher speed
        alignSpeed[1 - m] = ALIGN_SPEED_HI;
        //idle motor while sensor is aligned
        while(alignStatus[m] == 1) {
            defer();  //thread sleeps for ~50ms
            alignStatus[m] = readSensor(m);
        }
        //return other motor to normal speed
        alignSpeed[1 - m] = ALIGN_SPEED;
    }
}

/*int L_Align_Enc, R_Align_Enc;

void readEncAlign ()
{
    
    int LE = read_encoder(LEFT_ENCODER);
    int RE = read_encoder(RIGHT_ENCODER);
    
    if (L_Align_Mode == FORWARD)
      L_Align_Enc += LE;
    else
      L_Align_Enc -= LE;
    
    
      R_Align_Enc
}
*/

int alignAxle() {
    int proc[2], i = 0;
    beep();
    printf("Align\n");
    alignStatus[LEFT_MOTOR] = 0;
    alignStatus[RIGHT_MOTOR] = 0;
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);
    /*
    L_Align_Enc = 0;
    R_Align_Enc = 0;
*/
    //start left motor thread
    proc[0] = start_process(alignMotor(LEFT_MOTOR));
    //start right motor thread
    proc[1] = start_process(alignMotor(RIGHT_MOTOR));
    //block while not aligned and not timed out
    while(!alignStatus[LEFT_MOTOR] || !alignStatus[RIGHT_MOTOR]) {
        msleep(10L);
        defer();
    }
    kill_process(proc[0]);
    kill_process(proc[1]);
    alloff();
    beep();
    //msleep(1000L);
    //return boolean indicating success of alignment
    //if(i == MAX_TIME)
    //  return 0;
    return 1;
}

/////////////////////////////////////
//sub to accept WM details
/////////////////////////////////////
void acceptInputWM()
{
    int i;
    //WORLD MODEL INPUT
    //BLOCKS
    numTotalBlocks=16;
    x[0] = 4.;y[0] = 3.;
    x[1] = 2.;y[1] = 2.;
    x[2] =  -1.;y[2] =-1.;
    x[3] =  -1.;    y[3] =  -1.;
    x[4] =  -1.;    y[4] =  -1.;
    x[5] =  -1.;    y[5] =  -1.;
    x[6] = -1.;    y[6] = -1.;
    x[7] = -1.;    y[7] = -1.;
    x[8] = -1.;    y[8] = -1.;
    x[9] = -1.;    y[9] = -1.;
    x[10] = -1.;   y[10] =-1.;
    x[11] = -1.;   y[11] =-1.;
    x[12] = -1.;   y[12] = -1.;
    x[13] = -1.;   y[13] = -1.;
    x[14] = -1.;   y[14] = -1.;
    x[15] = -1.;   y[15] = -1.;
    //DESTINATIONS
    x[16] = 3.;   y[16] = 3.;
    x[17] = -1.;   y[17] = -1.;
    x[18] = -1.;   y[18] = -1.;
    x[19] = -1.;   y[19] = -1.;
    //BOT START POSITION
    x[20] = 4.;   y[20] = 4.;
    //end WM INPUT
    
    for (i=0;i<=15;i++){
        if ((x[i] < 0.) || (y[i] < 0.)){
            blockList[i][X]=9999.;
            blockList[i][Y]=9999.;
            numTotalBlocks--;
        }
        else{
            blockList[i][X]=x[i];
            blockList[i][Y]=y[i];        
        }
    }
    for (i=0;i<=3;i++){
        if ((x[i+16] < 0.) || (y[i+16] < 0.)){
            destList[i][X]=9999.;
            destList[i][Y]=9999.;
        }
        else{
            
            destList[i][X]=x[i+16];
            destList[i][Y]=y[i+16];        
        }
    }
    currBotPos[X]=x[20];
    currBotPos[Y]=y[20];
    currBotDir = SOUTH;
}


float nextTargetBlock[2];
/////////////////////////////////////////////
// closestBlock(): finds  the  
// closest block based on current bot pos
/////////////////////////////////////////////
void findClosestBlock()
{
    int i,closest;
    float x1,x2,y1,y2,min=9999.;
    x1 = currBotPos[X];
    y1 = currBotPos[Y];
    for (i=0;i<16;i++){
        x2 = blockList[i][X];
        y2 = blockList[i][Y];
        if (min > calculateDistance(x1,y1,x2,y2)){
            closest = i;
            nextTargetBlock[X]=blockList[closest][X];
            nextTargetBlock[Y]=blockList[closest][Y];
            min = calculateDistance(x1,y1,x2,y2);
        }
    }
    //remove this block from the list by making 
    // its position infinitely far away from arena
    blockList[closest][X]=9999.;
    blockList[closest][Y]=9999.;
    
    if (min > 20.) doneFlag = 1; //the closest block is far away 
    //which means all blocks have been reached
}
float nextTargetDest[2];
////////////////////////////////////////////////
// closestDest(): finds the  
// closest destination based on current bot pos
////////////////////////////////////////////////
void  findClosestDest()
{
    int i,closest;
    float x1,x2,y1,y2,min=9999.;
    x1 = currBotPos[X];
    y1 = currBotPos[Y];
    for (i=0;i<4;i++){
        x2 = destList[i][X];
        y2 = destList[i][Y];
        if (min > calculateDistance(x1,y1,x2,y2)){
            closest = i;
            nextTargetDest[X]=destList[closest][X];
            nextTargetDest[Y]=destList[closest][Y];        
            min = calculateDistance(x1,y1,x2,y2);
        }
    }
    
}

///////////////////////////////////////////
//abs function from Justins code project 2
///////////////////////////////////////////
float abs(float a)
{
    if(a < 0.)
      return -a;
    return a;
}
//////////////////////////////////////////////
//function to calc distance b/w two xy coords
/////////////////////////////////////////////
float calculateDistance(float x1,float y1,float x2,float y2)
{    
    return sqrt(sqr(x1-x2) + sqr(y1-y2));
}


///////////////////////////////////////
//funciotn to return square of a number
//////////////////////////////////////
float sqr(float a)
{
    return a*a;
}


////////////////////////////////////////////////////////////
// turn(<int>, <int>):sub to turn robot 0/90/180 deg (or 
// turn time)based on the given  direction.
// input : int direction, int angle
// ouput : void (motor actions to turn the bot
////////////////////////////////////////////////////////////
void turn(int direction, int turnAmount)
{
    int ticks = 0;
    //JGF
    int leftStopped = 0;
    int rightStopped = 0;
    int totalTicks = TURNTICKS * turnAmount;
    
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);
    ticks=readEncoders();
    while ((leftTicks < totalTicks) || (rightTicks < totalTicks)){
        ticks = readEncoders();
        if (direction == LEFT){
            if ((leftTicks - 1) > rightTicks){
                motor(RIGHT_MOTOR,STRAIGHT_SPEED);
                motor(LEFT_MOTOR,0);
            }
            else if ((rightTicks) - 1 > leftTicks){
                  motor(RIGHT_MOTOR,0);
                  motor(LEFT_MOTOR,-STRAIGHT_SPEED);
              }
              else
                {
                  motor(RIGHT_MOTOR,STRAIGHT_SPEED);
                  motor(LEFT_MOTOR,-STRAIGHT_SPEED);
              }
        }
        else if (direction == RIGHT){
              if ((leftTicks - 1) > rightTicks){
                  motor(RIGHT_MOTOR,-STRAIGHT_SPEED);
                  motor(LEFT_MOTOR,0);
              }
              else if ((rightTicks - 1) > leftTicks){
                    motor(RIGHT_MOTOR,0);
                    motor(LEFT_MOTOR,STRAIGHT_SPEED);
                }
                else
                  {
                    motor(RIGHT_MOTOR,-STRAIGHT_SPEED);
                    motor(LEFT_MOTOR,STRAIGHT_SPEED);
                }
          }
    }
    ao();
    msleep(WAIT);
    //printf("%d,%d,%d\n",leftTicks,rightTicks,totalTicks);
}
/*/////////////////////////////////////////////////////////////////
drive: drive the bot forward based on the distance
input: <int> distance : the no of feet to cover along x/y axis
return: <void>
*///////////////////////////////////////////////////////////////////
void drive(float distance)
{
    int ticks = 0;
    int totalTicks;
    totalTicks = (int) (ONEFOOTTICKS * distance);
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);    
    //printf("Drive %d feet\n",distance);
    
    ticks = readEncoders();
    
    while ((leftTicks < totalTicks) || (rightTicks < totalTicks) && !doneFlag)
      {
        ticks = readEncoders();
        //printf("l=%d,r=%d\n",leftTicks,rightTicks);
        if ((leftTicks - 1) > rightTicks){
            motor(LEFT_MOTOR,STRAIGHT_SPEED*70/100);
            motor(RIGHT_MOTOR,STRAIGHT_SPEED); //(int)right_speed); //getSpeed(leftTicks,rightTicks));
            //beep();
            while(leftTicks > rightTicks && leftTicks < totalTicks) {
                readEncoders();
            }
            //beep();
        }
        else if ((rightTicks - 1) > leftTicks){
              motor(LEFT_MOTOR,STRAIGHT_SPEED);
              motor(RIGHT_MOTOR,STRAIGHT_SPEED*70/100); //getSpeed(leftTicks,rightTicks));
              while(rightTicks > leftTicks && rightTicks < totalTicks) {
                  readEncoders();
              }
          }
          else
            {
              motor(LEFT_MOTOR,STRAIGHT_SPEED);
              motor(RIGHT_MOTOR,STRAIGHT_SPEED);
          }
    }
    ao();
    msleep(WAIT);
}

/*/////////////////////////////////////////////////////////////////
read_encoders: read the left and right wheel encoders to check the distance travelled
input <void>
return <int> the reading on the slot
*///////////////////////////////////////////////////////////////////
int leftTicks, rightTicks,L_Enc,R_Enc;
int readEncoders()
{
    leftTicks = read_encoder(LEFT_ENCODER);
    rightTicks = read_encoder(RIGHT_ENCODER);
    L_Enc = leftTicks;
    R_Enc = rightTicks;
    return (leftTicks + rightTicks)/2;
}

/*
LOOKUP TABLE indicating the DIRECTION (St/Left/Right) to turn.

along x is the desired direction the robot must turn
along y is the current direction the robot is facing
*/
/////////////////////////
int turnDirection[4][4];
/////////////////////////
/*
Left = 0
Right = 1
Straight = -1

         |  E  |  W  | N  | S
      -------------------------
      E  |  S  |  R  | L  | R
      -------------------------
      W  |  R  |  S  | R  | L
      -------------------------
      N  |  R  |  L  | S  | R
      -------------------------
      S  |  L  |  R  | R  | S
*/
//turnDirection[CURRENT, DESIRED] = TURN DIRECTION
void directionLookUp()
{
    turnDirection[EAST][EAST] = STRAIGHT;
    turnDirection[EAST][WEST] = RIGHT;
    turnDirection[EAST][NORTH] = LEFT;
    turnDirection[EAST][SOUTH] = RIGHT;
    
    turnDirection[WEST][EAST] = RIGHT;
    turnDirection[WEST][WEST] = STRAIGHT;
    turnDirection[WEST][NORTH] = RIGHT;
    turnDirection[WEST][SOUTH] = LEFT;
    
    turnDirection[NORTH][EAST] = RIGHT;
    turnDirection[NORTH][WEST] = LEFT;
    turnDirection[NORTH][NORTH] = STRAIGHT;
    turnDirection[NORTH][SOUTH] = RIGHT;
    
    turnDirection[SOUTH][EAST] = LEFT;
    turnDirection[SOUTH][WEST] = RIGHT;
    turnDirection[SOUTH][NORTH] = RIGHT;
    turnDirection[SOUTH][SOUTH] = STRAIGHT;
}//directionLookUp  
/*
LOOKUP TABLE indicating the ANGLE (0/90/180) to turn.

along x is the desired angle the robot must turn
along y is the current direction the robot is facing
*/

////////////////////////
int turnAngle[4][4];
////////////////////////
/*
this value can be multiplied with the TURN_TIME const to
tell the robot to turn (0/90/180 degrees)
NO TURN  = 0
TURN 90  = 1
TURN 180 = 2

         |  E  |  W  | N  | S
      -------------------------
      E  |  0  |  2  | 1  | 1
      -------------------------
      W  |  2  |  0  | 1  | 1
      -------------------------
      N  |  1  |  1  | 0  | 2
      -------------------------
      S  |  1  |  1  | 2  | 0
*/
void angleLookUp()
{
    turnAngle[EAST][EAST] = NO_TURN;
    turnAngle[EAST][WEST] = TURN180;
    turnAngle[EAST][NORTH] = TURN90;
    turnAngle[EAST][SOUTH] = TURN90;
    
    turnAngle[WEST][EAST] = TURN180;
    turnAngle[WEST][WEST] = NO_TURN;
    turnAngle[WEST][NORTH] = TURN90;
    turnAngle[WEST][SOUTH] = TURN90;
    
    turnAngle[NORTH][EAST] = TURN90;
    turnAngle[NORTH][WEST] = TURN90;
    turnAngle[NORTH][NORTH] = NO_TURN;
    turnAngle[NORTH][SOUTH] = TURN180;
    
    turnAngle[SOUTH][EAST] = TURN90;
    turnAngle[SOUTH][WEST] = TURN90;
    turnAngle[SOUTH][NORTH] = TURN180;
    turnAngle[SOUTH][SOUTH] = NO_TURN;
}//angleLookUp
/*/////////////////////////////////////////////////////////////////////
navigate(<int>, <int>, <int>, <int>, <int>, <int>):
given start xy position and direction the robot is facing and the stop 
xy position and direction the robot must face this routine gives motor
commands that navigate the robot.
INPUT: start state and stop state of the robot
/////////////////////////////////////////////////////////////////////*/
void navigate(float x1, float y1, int d1, float x2, float y2)
{
    //printf("x1=%fy1=%fx2=%dy2=%d\n",x1,y1,x2,y2);
    if ((x1 > x2) && (y1 > y2))//destination is NE
      {
        //printf("Destination NE\n");
        turn(turnDirection[d1][NORTH],turnAngle[d1][NORTH]);
        drive(abs(x1-x2));
        turn(turnDirection[NORTH][EAST],turnAngle[NORTH][EAST]);
        drive(abs(y1-y2));
        currBotDir=EAST;        
    }
    else if ((x1 > x2) && (y1 < y2)) //destination is NW
        {
          //printf("Destination NW\n");
          turn(turnDirection[d1][NORTH],turnAngle[d1][NORTH]);
          drive(abs(x1-x2));
          turn(turnDirection[NORTH][WEST],turnAngle[NORTH][WEST]);
          drive(abs(y1-y2));
          currBotDir=WEST;        
      }
      else if ((x1 > x2) && (y1 == y2)) //destination is N
          {
            //printf("Destination N\n");
            turn(turnDirection[d1][NORTH],turnAngle[d1][NORTH]);
            drive(abs(x1-x2));
            currBotDir=NORTH;        
        }
        
        else if ((x1 < x2) && (y1 > y2)) //destination is SE
            {
              //printf("Destination SE\n");
              turn(turnDirection[d1][SOUTH],turnAngle[d1][SOUTH]);
              drive(abs(x1-x2));
              turn(turnDirection[SOUTH][EAST],turnAngle[SOUTH][EAST]);
              drive(abs(y1-y2));
              currBotDir=EAST;
          }
          else if ((x1 < x2) && (y1 < y2)) //destination is SW
              {
                //printf("Destination SW\n");
                turn(turnDirection[d1][SOUTH],turnAngle[d1][SOUTH]);
                drive(abs(x1-x2));
                turn(turnDirection[SOUTH][WEST],turnAngle[SOUTH][WEST]);
                drive(abs(y1-y2));
                currBotDir=WEST;
            }
            else if ((x1 < x2) && (y1 == y2)) //destination is S
                {
                  //printf("Destination S\n");
                  turn(turnDirection[d1][SOUTH],turnAngle[d1][SOUTH]);
                  drive(abs(x1-x2));
                  currBotDir=SOUTH;
              }
              
              else if ((x1 == x2) && (y1 > y2)) //destination is E
                  {
                    //printf("Destination E\n");
                    turn(turnDirection[d1][EAST],turnAngle[d1][EAST]);
                    drive(abs(y1-y2));
                    currBotDir=EAST;
                }
                else if ((x1 == x2) && (y1 < y2)) //destination is W
                    {
                      //printf("Destination W\n");
                      turn(turnDirection[d1][WEST],turnAngle[d1][WEST]);
                      drive(abs(y1-y2));
                      currBotDir=WEST;
                  }
                  else if ((x1 == x2) && (y1 == y2)) //STAY!!!
                      {
                        printf("Destination NONE\n");
                    }
                    //UPDATE POSITION OF THE BOT    
                    currBotPos[X]=x2;
    currBotPos[Y]=y2;
    //end UPDATE ...
    
}
int cageStatus = 0;
//////////////
//cage control
///////////////
void cage(int status){
    if (status == OPEN)
      {
        servo5 = OPEN_CAGE;
    }
    else if (status == LOCK)
        {
          servo5 = LOCK_CAGE;
      }
      else if (status == RELEASE)
          {
            servo5 = RELEASE_CAGE;
        }
        msleep(WAIT);
    
}
/////////////////////////////////////
//sub to acquire the block
///////////////////////////////////
void releaseBlock(float a, float b){
    float c = b-a;
    
    if (currBotPos[X] > 4.)
      navigate(currBotPos[X],currBotPos[Y],currBotDir,currBotPos[X]-a,currBotPos[Y]);
    else
      navigate(currBotPos[X],currBotPos[Y],currBotDir,currBotPos[X]+a,currBotPos[Y]);
    if (currBotDir==EAST) currBotPos[Y]-=c;
    if (currBotDir==WEST) currBotPos[Y]+=c;
    if (currBotDir==SOUTH) currBotPos[X]+=c;
    if (currBotDir==NORTH) currBotPos[X]-=c;
}


///////////////////////////////////
//path plan
///////////////////////////////////
void planPath(int seek)
{
    if (seek == BLOCK){
        //find the closest block
        findClosestBlock();
        //goto the closest block
        navigate(currBotPos[X],currBotPos[Y],currBotDir,nextTargetBlock[X],nextTargetBlock[Y]);
    }
    else if (seek == DEST)
        {
          //find the closest destination
          findClosestDest();
          //goto the closest destination
          navigate(currBotPos[X],currBotPos[Y],currBotDir,nextTargetDest[X],nextTargetDest[Y]);    
      }
}
///////////////////////////////////
//func to play success tune
//////////////////////////////////
void playTune()
{
    tone(554.0, 0.103125);
    tone(554.0, 0.103125);
    tone(554.0, 0.103125);
    tone(554.0, 0.103125);
    tone(554.0, 0.103125);
    tone(493.0, 0.103125);
    tone(493.0, 0.103125);
    tone(493.0, 0.20625);
    tone(440.0, 0.103125);
    tone(415.0, 0.103125);
    tone(440.0, 0.103125);
    tone(440.0, 0.103125);
    tone(554.0, .9375);
    tone(493.0, .6375);
    tone(554.0, 0.103125);
    tone(493.0, 0.103125);
    tone(440.0, .9375);
    
}
void correctError(){
    drive(0.5);
    if (currBotDir==EAST) currBotPos[Y]-=0.5;
    if (currBotDir==WEST) currBotPos[Y]+=0.5;
    if (currBotDir==SOUTH) currBotPos[X]+=0.5;
    if (currBotDir==NORTH) currBotPos[X]-=0.5;
}

void backUp() {
    motor(LEFT_MOTOR, -ALIGN_SPEED);
    motor(RIGHT_MOTOR, -ALIGN_SPEED);
    while(!readSensor(LEFT_MOTOR) && !readSensor(RIGHT_MOTOR))
      ;
    ao();
}

void release() {
    //backUp();
    //drive(0.5);
    cage(RELEASE);
    releaseBlock(.005,.0);
    alignAxle();
    releaseBlock(.5,1.25);
    playTune();
    cage(OPEN);
}

/*/////////////////////////////
main function
//////////////////////////////*/
int numBlocksContained, numTotalBlocks;
int doneFlag=0;
void main()
{
    //intialization of robot
    angleLookUp();
    directionLookUp();
    enable_encoder(LEFT_ENCODER);
    enable_encoder(RIGHT_ENCODER);
    init_expbd_servos(1);
    cage(OPEN);
    acceptInputWM();
    //end intialization
    printf("PRESS START\n");
    while(!start_button());
    printf("numblks=%d\n",numTotalBlocks);
    msleep(WAIT);
    numBlocksContained=1;
    while (numBlocksContained <= numTotalBlocks  && !doneFlag)
      {
        printf("currBot x=%f y=%f\n",currBotPos[X],currBotPos[Y]);
        msleep(WAIT);
        //plan the path for next move
        planPath(BLOCK);
        correctError();
        cage(LOCK);
        printf("currBot x=%f y=%f\n",currBotPos[X],currBotPos[Y]);
        msleep(WAIT);
        //end of logic for caging the block
        planPath(DEST);
        printf("currBot x=%f y=%f\n",currBotPos[X],currBotPos[Y]);
        msleep(WAIT);
        release();
        numBlocksContained++;
    }
    printf("DONE!\n");
}