#define RIGHT_BUMPER 10
#define LEFT_BUMPER 14
#define LEFT_ENCODER 1
#define RIGHT_ENCODER 0
#define LEFT_IR 30
#define RIGHT_IR 31
#define LEFT_MOTOR 1
#define RIGHT_MOTOR 3
#define LEFT_SPEED 30  /* speed of left motor */
#define RIGHT_SPEED 60 /* speed of right motor - adjusted to go straight*/
#define TURN_90_TIME 1.4 /*time to turn 90 degrees*/
#define TURN_180_TIME 2.2
#define TURN_5_TIME .07 /* time to turn 5 degrees*/
#define BACKUP_TIME 0.1
#define REFLECTANCE_THRESHOLD 220 /* Threshold for reflectance sensors*/
/* commands used to identify whether the robot is going towards a block
or a goal*/
#define GOAL 1
#define BLOCK 2
/* directions for orientation global */
#define NORTH 0
#define EAST 1
#define SOUTH 2
#define WEST 3
/* confidence levels for CMUCam */
#define ORANGE_CONFIDENCE 40
#define TWO_BLOCK_CONFIDENCE 80
#define BLUE_CONFIDENCE 80
/* RAMMING Speeds! */
#define LEFT_RAM_SPEED 50
#define RIGHT_RAM_SPEED 100
/* time used when searching for blocks */
#define MOVE_TO_BLOCK_TIME 0.5
/* used as a motor speed */
#define OFF 0

#define RIGHT_TURN 1
#define LEFT_TURN 0
#define SPIN 52
#define STAGE_SPEED 14

#define TICKS_PER_INCH 9

#define HIGH_TURN_SPEED 30
#define LOW_TURN_SPEED 10

#define left_drift -1
#define right_drift 2

#define tape_value 50 /* average tape value for top hats */
#define low_tape 180  /* value of top hat near white edge of tape */
#define hi_tape 220  /* value of top hat near black edge of tape */

#use "cmucamlib.ic"

/* used as to record the distance when encountering 
a block while going straight */
float distanceToObstacle = 0.0;
/* takes in the list from Dr. Hougen */
float model[] = {7.,3.,2.,3.5,6.,4.5,6.,6.,1.,7.,1.,9.5,6.5,10.,7.5,10.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,-1.,
      1.,1.,6.,3.,2.,11.,-1.,-1.,4.,5.5};
/* records the current position of the robot */
float current_x;
float current_y;
/* records the next goal position of the robot */
float next_x;
float next_y;
/* records the direction (N, S, E, or W) the robot faces */
int orientation;
/* Dectected the enemy! */
int foundOtherRobot;
/* records process ids */
int enemyProcessID;
int avoidProcessID;
/* records that the robot is currently carrying a block */
int haveABlock;
int finding_goal = 0;

int blobSize = 0;
int spinvalue = SPIN;

void main(){
    sleep(1.0);
    printf("press start\n");
    while (!start_button()) {sleep(0.2);}
    while (start_button()) {}
    /* sets all the initializations */
    current_x = model[40];
    current_y = model[41];
    
    orientation = NORTH;
    
    enable_encoder(RIGHT_ENCODER);
    enable_encoder(LEFT_ENCODER);
    init_camera();
    clamp_camera_yuv();
    printf("press start\n");
    while (!start_button()) {}
    while (start_button()) {}
    /* starts the two behavior processes */
    /* The two behaviors are commented out because multiple processes*/
    /* don't seem to work.*/
    /*avoidProcessID = start_process(avoid());*/
    /* sleeps to account for CMUCam processing time */
    sleep(0.5);
    /*enemyProcessID = start_process(detectEnemy());*.
    /* starts the main hierarchical process */
    collectBlocks();
}

/*Main process.  Moves to closest block, then moves to closest goal,
repeat until done.*/
void collectBlocks(){
    int notDone = 1;
    while(notDone){
        notDone = (int) moveToClosest(BLOCK);
        if(findBlock()){
            finding_goal = 1;
            moveToClosest(GOAL);
            findGoal();
        }
        finding_goal = 0;
    }
}

/* This just moves to the closest object of the given type.*/
float moveToClosest(int objectType){
    int init = 0;
    int quantity = 16;
    int i = 0;
    float minimalDistance = 20.0;
    next_x = 0.0;
    next_y = 0.0;
    if(objectType == GOAL){
        init += 16;
        quantity = 4;
    }
    /*Searches through the list and finds the closest object */
    for(i = init;(i < (init + quantity)) && (model[2*i] != -1.0);i++){
        float distance = abs(current_x - model[2*i]) +
          abs(current_y - model[(2*i) + 1]);
        if(distance <= minimalDistance){
            minimalDistance = distance;
            next_x = model[2*i];
            next_y = model[(2*i) + 1];
        }
    }
    /* actually moves to that closest object */
    if((next_x + next_y) != 0.)
      moveTo(next_x, next_y);
    /* returns 0 if no object is left, so that it knows it's done */
    return(next_x + next_y);
}

/* Swerves to avoid an orange block */
void swerve(){
    int xOfBlock;
    distanceToObstacle = getDistanceTraveled();
    motor(LEFT_MOTOR, OFF);
    motor(RIGHT_MOTOR, OFF);
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);
    xOfBlock = track_x;
    /*Checks to see which side the second block is on and tries to*/
    /*avoid it*/
    if(xOfBlock < 0){
        turn(30, LEFT_TURN);
        goStraight(.5);
        turn(30, RIGHT_TURN);
        goStraight(.5);
    }
    else{
        turn(30, RIGHT_TURN);
        goStraight(.5);
        turn(30, LEFT_TURN);
        goStraight(.5);
    }
    /* informs the robot that it has traveled an additional 11.0*/
    /* inches*/
    distanceToObstacle += 11.0;
}

/* Checks to see if there's an orange block in the way */
void avoid(){
    while(1){
        /* Checks to see if the size of the orange block being */
        /* detected has increased substantially, indicating the*/
        /* presence of a second block*/
        if(track_size > (blobSize + 50)){
            if(!haveABlock){
                swerve();
            }
            else{
                swerve();
            }
            blobSize = track_size;
        }
    }
    
}


/* Attempts to ram the blue object if detected */
void rammingSpeed(){
    int x_value_1;
    int x_value_2;
    track_blue();
    x_value_1 = track_x;
    sleep(0.1);
    track_blue();
    x_value_2 = track_x;
    /* uses the location of the centroid to try and ram the enemy */
    if(x_value_1 > x_value_2){
        if(x_value_2 < 0){
            motor(LEFT_MOTOR, LEFT_SPEED);
            motor(RIGHT_MOTOR, RIGHT_RAM_SPEED);
        }
        else if(x_value_2 < 40){
              motor(LEFT_MOTOR, LEFT_RAM_SPEED);
              motor(RIGHT_MOTOR, RIGHT_RAM_SPEED);
          }
          else{
              motor(LEFT_MOTOR, LEFT_RAM_SPEED);
              motor(RIGHT_MOTOR, RIGHT_RAM_SPEED);
          }
    }
    else{
        if(x_value_2 < -40){
            motor(LEFT_MOTOR, LEFT_RAM_SPEED);
            motor(RIGHT_MOTOR, RIGHT_RAM_SPEED);
        }
        else if(x_value_2 < 0){
              motor(LEFT_MOTOR, LEFT_RAM_SPEED);
              motor(RIGHT_MOTOR, RIGHT_SPEED);
          }
          else{
              motor(LEFT_MOTOR, LEFT_RAM_SPEED);
              motor(RIGHT_MOTOR, OFF);
          }
    }
    /*Move forward while we still see the enemy */
    while(!bump()  && (track_blue() >= BLUE_CONFIDENCE)){
        sleep(0.2);
    }
    /* WE Have found the Blue One and slain it */
    if(bump()){
        foundOtherRobot = 1;
        kill_process(enemyProcessID);
    }
    motor(LEFT_MOTOR, OFF);
    motor(RIGHT_MOTOR, OFF);
}

/* Continually checks for the Blue one. */
void detectEnemy(){
    if(track_blue() > BLUE_CONFIDENCE){
        rammingSpeed();
    }
    sleep(1.0);
}

/* Travels to a point in the grid by traveling only in the x and 
y axes */
void moveTo(float x_coord, float y_coord){
    if(x_coord < current_x){
        if(orientation == EAST){
            turnLeft();
        }
        if(orientation == SOUTH){
            turnAround();
        }
        if(orientation == WEST){
            turnRight();
        }
        /* Robot is now pointing NORTH */
        goStraight(current_x - x_coord);
    }
    
    if(x_coord > current_x){
        if(orientation == EAST){
            turnRight();
        }
        if(orientation == NORTH){
            turnAround();
        }
        if(orientation == WEST){
            turnLeft();
        }
        /* Robot is now pointing SOUTH */
        goStraight(x_coord - current_x);
    }
    
    if(y_coord < current_y){
        if(orientation == SOUTH){
            turnLeft();
        }
        if(orientation == WEST){
            turnAround();
        }
        if(orientation == NORTH){
            turnRight();
        }
        /* Robot is now pointing EAST */
        goStraight(current_y - y_coord);
    }
    
    if(y_coord > current_y){
        if(orientation == SOUTH){
            turnRight();
        }
        if(orientation == EAST){
            turnAround();
        }
        if(orientation == NORTH){
            turnLeft();
        }
        /* Robot is now pointing WEST */
        goStraight(y_coord - current_y);
    }
    
}

/* Just turns right 90 degrees */
void turnRight(){
    turn(90, RIGHT_TURN);
    orientation = (int) abs((float)((orientation + 1) % 4));
}

/* Turns left 90 degrees */
void turnLeft(){
    turn(90, LEFT_TURN);
    orientation = (int) abs((float)((orientation - 1) % 4));
}

/* Turns 180 degrees to the right */
void turnAround(){
    turn(180, RIGHT_TURN);
    orientation = (int) abs((float)((orientation - 2) % 4));
}

/* Registers when the bump sensors hit */
int bump(){
    return (digital(RIGHT_BUMPER) + digital(LEFT_BUMPER));
}

/* Registers when the reflectance sensors have found tape */
int foundGoal(){
    if((analog(RIGHT_IR) > REFLECTANCE_THRESHOLD) ||
       (analog(LEFT_IR) > REFLECTANCE_THRESHOLD)){
        return 1;
    }
    else return 0;
}

/* Moves forward for distance, using the encoders */
void goStraight(float distance){
    float distanceTraveled = 0.0;
    float speed = 0.55;
    int position;
    int inch = TICKS_PER_INCH;
    int forward_speed = 30;
    float inches = distance * 12.;
    
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);
    
    position= ((read_encoder(RIGHT_ENCODER) + read_encoder(LEFT_ENCODER))
               / 2)+(inch* ((int) inches));
    
    /* Checks the encoders to see if we have moved forward far enough*/
    while((((read_encoder(RIGHT_ENCODER) + read_encoder(LEFT_ENCODER)) / 2)
           < position) && (!foundGoal() || !finding_goal) && !bump())
      {
        forward(forward_speed);
    } 
    if(forward_speed<0)ao();
    else ao();
    /* Checks to see which direction we're travelling and adjusts the */
    /* current position accordingly*/
    if(orientation == NORTH){
        current_x -= distance;
    }
    else if(orientation == EAST){
          current_y -= distance;
      }
      else if(orientation == SOUTH){
            current_x += distance;
        }
        else if(orientation == WEST){
              current_y += distance;
          }
          
}

/* Moves the robot forward at a given speed, adjusting for drift*/
void forward (int VELOCITY){
    int drift;
    /* takes the difference from the encoder values */
    drift=read_encoder(LEFT_ENCODER)-read_encoder(RIGHT_ENCODER);
    
    /*Checks to see if the robot has drifted and adjusts*/
    if (drift < left_drift){
        motor(LEFT_MOTOR, VELOCITY);
        motor(RIGHT_MOTOR, OFF);
    }
    else if (drift > right_drift){
          motor(LEFT_MOTOR, OFF);
          motor(RIGHT_MOTOR, VELOCITY);
      }
      else{
          motor(LEFT_MOTOR, VELOCITY);
          motor(RIGHT_MOTOR, VELOCITY);
      }
    
}


/* reads the encoders and checks to see how far we've gone */
float getDistanceTraveled(){
    return (((float)(read_encoder(RIGHT_ENCODER)) / 6.)* 3.14);
}

/* This function takes the absolute value */
float abs(float a){
    if (a >= 0.) return a;
    return -a;
}

/* Searches for the orange blocks when really close. */
int findBlock(){
    int count = 0;
    /* if the robot can't find the block, turns to look for it*/
    while(!track_orange() && (count < 4)){
        turnRight();
        count++;
    }
    /*If the robot does find the block, attempts to capture it*/
    /* in its carrying bumper*/
    if(track_orange()){
        if(track_y > 0){
            if(track_x < -40){
                turn(10,LEFT_TURN);
                goStraight(0.25);
                turn(10,RIGHT_TURN);
                goStraight(0.25);
            }
            else{
                if(track_x < 40){
                    goStraight(0.25);
                }
                else{
                    if(track_x > 40){
                        turn(10,RIGHT_TURN);
                        goStraight(0.25);
                        turn(10,LEFT_TURN);
                        goStraight(0.25);
                    }
                }
            }
        }
        blobSize = track_size;
        haveABlock = 1;
        /*readjusts position*/
        current_x = next_x;
        current_y = next_y;
        return haveABlock;
    }
    else{
        haveABlock = 0;
        blobSize = 0;
        removeFromList();
        return haveABlock;
    }    
}

/* searches for the goal and drops off the block */
int findGoal(){
    float startTime;
    float speed = 0.55;
    int turns = 0;
    int i = 0;
    /* looks for a goal*/
    while(!foundGoal() && turns < 4){
        startTime = seconds() + 0.5;
        while((seconds() < startTime) && !foundGoal()){
            motor(LEFT_MOTOR, LEFT_SPEED);
            motor(RIGHT_MOTOR, RIGHT_SPEED);
        }
        motor(LEFT_MOTOR, OFF);
        motor(RIGHT_MOTOR, OFF);
        if(!foundGoal()){
            for(i = 1; (i<3) && (!foundGoal());i++){
                turn(30,RIGHT_TURN);
            }
            if(i > 4){
                orientation = (int) abs((float)((orientation + 1) % 4));
            }
            turns++;
        }
    }
    if(foundGoal()){
        /* adjusts and beeps*/
        stage();
        beep();
        printf("press start to continue.\n");
        while (!start_button()) {}
        while (start_button()) {}
        blobSize = 0;
        haveABlock = 0;
        finding_goal = 0;
        /* enters the goal and recalibrates position*/
        goStraight(0.5);
        current_x = next_x;
        current_y = next_y;
        
        return foundGoal();
    }
    return foundGoal();
}            

/* Removes an orange block from the list once it is discovered
   not to be there */
void removeFromList(){
    int i = 0;
    for(i = 0; (i < 16); i++){
        if((model[2*i] == next_x) && (model[(2*i) + 1] == next_y)){
            model[2*i] = -1.;
            model[(2*i)+1] = -1.;
        }
    }
    
}

/* Code taken from Team 3, Project 1*/
void turn(int degree, int right) /* uses a skid steering approach to change the robots vector
                        it will drive the right motor forward for a specific number of 
                        turns using the encoders and drive the left motor backward 
                        for the same number of turns. This makes the robot turn like a 
                        Tank. */
{
    int drift,ticks;
    int spin= spinvalue; /* number of ticks per 90 degrees */
    
    reset_encoder(RIGHT_ENCODER);
    reset_encoder(LEFT_ENCODER);
    
    ticks=degree/90*spin; /* sets the number of ticks for the spin */
    
    /*turns on motors in opposite direction until the number of ticks has been reached*/
    
    while(((read_encoder(RIGHT_ENCODER) + read_encoder(LEFT_ENCODER))/2) < ticks)  
      {
        if(right) drift=read_encoder(LEFT_ENCODER)-read_encoder(RIGHT_ENCODER);
        else drift=read_encoder(RIGHT_ENCODER)-read_encoder(LEFT_ENCODER);
        
        if(right){
            /*if (drift < -5){
                motor(RIGHT_MOTOR,-HIGH_TURN_SPEED);
                motor(LEFT_MOTOR, LOW_TURN_SPEED);
            }
            else if (drift > 5){
                  motor(RIGHT_MOTOR,-LOW_TURN_SPEED);
                  motor(LEFT_MOTOR, HIGH_TURN_SPEED);
              }
              else{*/
            motor(RIGHT_MOTOR, -HIGH_TURN_SPEED);
            motor(LEFT_MOTOR, HIGH_TURN_SPEED);
        }
        /*}*/
        else{/*
            if (drift < -5){
                motor(LEFT_MOTOR,-HIGH_TURN_SPEED);
                motor(RIGHT_MOTOR, LOW_TURN_SPEED);
            }
            else if (drift > 5){
                  motor(LEFT_MOTOR,-LOW_TURN_SPEED);
                  motor(RIGHT_MOTOR, HIGH_TURN_SPEED);
              }
              else{*/
            motor(LEFT_MOTOR, -HIGH_TURN_SPEED);
            motor(RIGHT_MOTOR, HIGH_TURN_SPEED);
        }
        /*}*/
        
        
    }
    brake(0);
    reset_encoder(LEFT_ENCODER);
    reset_encoder(RIGHT_ENCODER);
}
/* Code taken from Team 3, Project 1*/
int move(int a,int b)  /* simple motor function takes two speed values for 
    each motor */ 
{
    motor(LEFT_MOTOR,a);
    motor(RIGHT_MOTOR,b);
}

/* Code taken from Team 3, Project 1*/
int brake(int x)   /* A series of brake commands to stop the robot from 
    drifting after the motors are turned off. The momentum 
    of the robot will carry it a few extra inches. */ 
{
    if (x==1)move(-10,-10);
    else if (x==0) move(20,-20);
      else if (x==2) move(20,20);
        sleep(0.1);
    ao();
}

/* Code taken from Team 3, Project 1*/
void stage()  /* The robot must be staged on the edge of a box to ensure
                 perpendicluar allignment to drive to the next box.*/ 
{
    int l,r,bump;
    l=0;
    r=0;
    bump=0;
    
    /* the robot reads the left and right top hat sensor values and will try to
       stop the robot where they are both reading very similar values. */
    
    while(bump<1000)
      {
        if(l==1 && r==1)
          {
            ao();
            reset_encoder(LEFT_ENCODER);
            reset_encoder(RIGHT_ENCODER);
            break;
        }
        l=left_hat();
        r=right_hat();
        bump=bump+1;    /* an counter was added to avoid being stuck in an infinite loop */
    }
    
}

/* Code taken from Team 3, Project 1*/
int right_hat()  /* function for the right top hat sensor used in the stage function to 
                    control the right side of the robot and align the right sensor */
{
    if(analog(RIGHT_IR) > low_tape && analog(RIGHT_IR) < hi_tape)return(1);
    else if(analog(RIGHT_IR) < low_tape)
        {
          motor(RIGHT_MOTOR,STAGE_SPEED);
          return(0);
      }
      else if(analog(RIGHT_IR) > hi_tape)
          {
            motor(RIGHT_MOTOR,-STAGE_SPEED);
            return(0);
        }
}

/* Code taken from Team 3, Project 1*/
int left_hat() /* same function as above except for the left side */
{
    if(analog(LEFT_IR) > low_tape && analog(LEFT_IR) < hi_tape)return(1);
    else if(analog(LEFT_IR) < hi_tape)
        {
          motor(LEFT_MOTOR,STAGE_SPEED);
          return(0);
      }
      else if(analog(LEFT_IR) > hi_tape)
          {
            motor(LEFT_MOTOR,-STAGE_SPEED);
            return(0);
        }
}