/* -----------------------------------
   Game Play alpha
   by Rodney Graham, Jamie Hope
   29 January 2003 
   ----------------------------------- */

/* sensor ports */
/* 

DISTANCE 2

**ACCELOROMETER 7

IR_FRONT 8
IR_RIGHT 9
IR_BACK 10
IR_LEFT 11 */

#define START_LIGHT_PORT 31

#define LEFT_LIGHT 3
#define CENTER_LIGHT 4
#define RIGHT_LIGHT 5
#define CLAW_LIGHT 16

#define CENTER_TOUCH 12
#define LEFT_TOUCH 13
#define RIGHT_TOUCH 14
#define CLAW_TOUCH 15

/* motor ports */
#define LEFT_MOTOR 0
#define RIGHT_MOTOR 2
#define TAIL_MOTOR 4

/* servo ports */
#define CLAW_SERVO 0
#define ARM_SERVO 1
#define TAIL_SERVO 2

/* directional constants */
#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3

#define FRONT 0   /* arms */
#define REAR 1

#define LOWER -1
#define RAISE 1

persistent int bGreen, bWhite, tBlack[3], tWhite[3];   /* color thresholds */
persistent int posGee, negGee, accOffset;  /* accelerometer constants and thresholds */

int team, currSide;  /* team color and current location (side color) */

/* WHITE = 1, BLACK = 0 */

void main() {
    
    printf("Game Play Alpha press start\n");
    wait_start();
    
    calibrate();   /* calibrate sensors to lighting levels */
    
    start_machine(START_LIGHT_PORT);   /* align with start light and go! */
    printf("Go!!!\n");
    
    playGame();
}

/* calibrates sensors */
void calibrate() {
    int NUM = 8, i;
    float dark[4], bright[4];
    
    printf("Calibration\n");
    wait(2.0);
    printf("proceed: Start  exit: Stop\n");
    while (wait_button()) {
        while (!press_start())
          printf("Left Light blk  value: %d\n", (int)(dark[0] = averageValues(LEFT_LIGHT, NUM)));
        while (!press_start())
          printf("Left Light wh   value: %d\n", (int)(bright[0] = averageValues(LEFT_LIGHT, NUM)));
        while (!press_start())
          printf("Center Light blkvalue: %d\n", (int)(dark[1] = averageValues(CENTER_LIGHT, NUM)));
        while (!press_start())
          printf("Center Light wh value: %d\n", (int)(bright[1] = averageValues(CENTER_LIGHT, NUM)));
        while (!press_start())
          printf("Right Light blk value: %d\n", (int)(dark[2] = averageValues(RIGHT_LIGHT, NUM)));
        while (!press_start())
          printf("Right Light wh  value: %d\n", (int)(bright[2] = averageValues(RIGHT_LIGHT, NUM)));
        while (!press_start())
          printf("Claw Light grn  value: %d\n", (int)(dark[3] = averageValues(CLAW_LIGHT, NUM)));
        while (!press_start())
          printf("Claw Light wh   value: %d\n", (int)(bright[3] = averageValues(CLAW_LIGHT, NUM)));
        
        for (i = 0; i < 4; i++) {
            float margin = bright[i] - dark[i];
            if (margin < 0.0)
              printf("NEG MARGIN ERRORRECALIBRATE\n");
            
            if (i < 3) {
                tBlack[i] = (int)(dark[i] + (margin * 0.3));
                tWhite[i] = (int)(bright[i] - (margin * 0.3));
            }
            else {
                bGreen = (int)(dark[i] + (margin * 0.3));
                bWhite = (int)(bright[i] - (margin * 0.3));

            }
        }        
}

/* controls operation during actual game play */
void playGame() {
    int i, tableColor;
    float timeBase;
    
    initializeServos();

    /* determine team color and orient robot */
    while ((tableColor = getColor(LEFT_LIGHT)) == -1) {
        if (i = 0)
            inch(FORWARD);
        inch(FORWARD);
        if (seconds() > 7.0) {  /* TIMEOUT -- cannot resolve start color */
            tableColor = 0;     /* guess black team?  -- need better response!*/
            break;
        }
	  i++;
    }
    currSide = team = tableColor;

    if (tableColor) {
        while (!pressed(LEFT_TOUCH)) { 
            turn(LEFT); 
            if (seconds() > 4.0)  /* TIMEOUT -- touch sensor blocked */
                break;
        }
    }
    else {
        while (!pressed(RIGHT_TOUCH)) { 
            turn(RIGHT);
            if (seconds() > 4.0)  /* TIMEOUT -- touch sensor blocked */
                break;
         }
    }
    
    start_accel(2);  /*** arguments?? ***/
   
    timeBase = seconds();
    while ((seconds() - timeBase) < 10.0 ) { followWall(); }
    
    /* test balls until team ball is found */
    while (obtainBall(0) != team) {
        removeBall();  /* remove any opponent balls found */
    }
    
    findIsland();   
    depositBall();
    
    /* protect lava island until final seconds of competition */
    stop();
    while (seconds() < 52.0);
    
    anarchy(); /* clear any balls present on middle plateau */
}

/* directs robot to lava island */ 
void findIsland() {
    int white = 1;
    float timeBase, BALL_DIST = 3.0;
    
    /* first, find wall to follow to opposite end of board */
    printf("findIsland( %d )\n", currSide);    
    while (getTilt() == 0) {
        timeBase = seconds();
        while (!pressed(CENTER_TOUCH)) {
            printf("findIsland()    find wall\n");
            if ((seconds() - timeBase) > 3.0) {
                printf("findIsland()    timeout check\n");
                move(BACKWARD);
                turn(LEFT + !currSide);
                timeBase = seconds();
            }
            move(FORWARD);
        }
        
        if (getTilt() != 0) { break; } /* prevent unnecessary reorientation */
        
        move(BACKWARD);
        if (pressed(LEFT_TOUCH)) {
            if (currSide) {
                while (!pressed(RIGHT_TOUCH)) { turn(RIGHT); }
                move(FORWARD);
                wait(0.5);
            }
            else {
                turn(RIGHT);
                wait(0.25);
                move(FORWARD);
                wait(0.5);
            }
        }
        else {
            if (pressed(RIGHT_TOUCH)) {
                if (currSide) {
                    turn(LEFT);
                    wait(0.25);
                    move(FORWARD);
                    wait(0.5);
                }
                else {
                    while(!pressed(LEFT_TOUCH)) { turn(LEFT); }
                    move(FORWARD);
                    wait(0.5);
                }
            }
            else {
                if (currSide) {
                    turn(LEFT);
                    wait(0.15);
                    move(FORWARD);
                    wait(0.25);
                }
                else {
                    turn(RIGHT);
                    wait(0.15);
                    move(FORWARD);
                    wait(0.25);
                }
            }
        }
    }
    
    while(getTilt() == -1) { turn(LEFT + currSide); }
    
    /* now, align with wall in order to execute wall following algorithm */
    printf("findIsland()    align wall\n");
    timeBase = seconds();
    while (!pressed(LEFT_TOUCH + currSide)) {
        if ((seconds() - timeBase) > 3.0) { /* TIMEOUT -- failed to align self with wall */
            printf("findIsland()    align timeout\n");
            move(BACKWARD);
            turn(LEFT + !currSide);         /* turn away from wall to increase intercept angle */
        }
        move(FORWARD);
        wait(0.5);
        turn(LEFT + currSide);
    }
    /* follow wall until opposite end of board is reached */
    while (!pressed(CENTER_TOUCH)) { followWall(); }
    
    /* turn to orient self with appropriate line */
    printf("findIsland()    align line\n");    
    move(BACKWARD);
    turn(LEFT + !currSide);
    while (getColor(CENTER_LIGHT) != white) {
        inch(LEFT + !currSide); 
    }
    
    /* finally, follow line to lava island and turn around*/
    while (!pressed(CENTER_TOUCH)) { followLine(white); }
    move(BACKWARD);
    wait(0.25);
    stop();

    printf("findIsland()    align island\n");
    turn(LEFT + !currSide);
    while ((getColor(CENTER_LIGHT) != white) || (getColor(LEFT_LIGHT + !currSide) == white)) {
        inch(LEFT + !currSide); 
    }
    
    while (getDistance() > BALL_DIST) { inch(FORWARD); }
    stop();
}

/* remove possible opponent ball from lava island and deposit team ball */
void depositBall() {
    int white = 1;
    float BALL_DIST = 3.0, DIST_TO_WALL = 15.0, timeBase;
    
    /* attempt to remove opponent ball from lava island */
    printf("depositBall( %d ) remove\n", currSide);    
    stop();
    moveArm(REAR, LOWER);
    spinTail();
    timeBase = seconds();
    while ((seconds() - timeBase) < 5.0) { 
        inch(BACKWARD);
        wait(0.3);
    }
    stopTail();
    moveArm(REAR, RAISE);
        
    /* prepare to deposit team ball by orienting front with lava island */
    printf("depositBall( %d ) orient\n", currSide);
    turn(LEFT + !currSide);
    wait(0.3);
    while (getColor(LEFT_LIGHT + currSide) != white) { turn(LEFT + currSide); }
    while (getColor(CENTER_LIGHT) != white) { inch(LEFT + currSide); }
    while (!pressed(CENTER_TOUCH) && getDistance() > BALL_DIST) { inch(FORWARD); }
    stop();
    
    /* deposit team ball!!!! */
    printf("depositBall( %d ) deposit\n", currSide);    
    moveArm(FRONT, LOWER);
    release();
    timeBase = seconds();
    while((seconds() - timeBase) < 1.0) {
	  inch(FORWARD);
 	  wait(0.3);
    }
    
    /* arm is left in lowered position to protect ball */
}

/* reeks havoc!!! */
void anarchy() {
    
    printf("anarchy( %d )\n", currSide);
    moveArm(FRONT, RAISE);
    turn(LEFT + !currSide);
    wait(0.25);
    
    while (getTilt() != 1) { move(FORWARD); }
    while (getTilt() == 1) { 
        move(FORWARD);
    }
    
    turn(LEFT + team);
    wait(0.25);
    
    while(!pressed(CENTER_TOUCH)) { move(FORWARD); }
    move(BACKWARD);
    wait(2.0);
}

/* find nearest ball and grasp with front claw */
int obtainBall(int attempts) {
    float BALL_DIST_1 = 6.0, BALL_DIST_2 = 4.5, timeBase, timeDelta;
    
    printf("obtainBall( %d,  attempts %d )\n", currSide, attempts);
    stop();
    
    printf("obtainBall()    inch forward\n");
   /** while (getDistance() > BALL_DIST_1) { inch(FORWARD); } **/
    while (!pressed(CENTER_TOUCH)) {
        moveArm(FRONT, LOWER);
    	  release();
        while (!pressed(CLAW_TOUCH)) {
            printf("obtainBall()    obtain\n");            
           /*  while (getDistance() > BALL_DIST_2) { inch(FORWARD); } */
		inch(FORWARD);
        }
	  grasp();
        moveArm(FRONT, RAISE);
        if (pressed(CLAW_TOUCH))
            break;
    }
    
    if (pressed(CENTER_TOUCH)) {  /* wall reached, try to pickup adjacent two ball */
        if (attempts == 0) {
            printf("obtainBall()    **back\n");
            while (getTilt() == 0 ) { move(BACKWARD); }  /* move backward till on incline */
            wait(0.5);
            
            printf("obtainBall()    **turn\n");
            timeDelta = seconds();
            while (getTilt() != 0) { turn(LEFT + currSide); }  /* turn until perpendicular with side wall */
            timeDelta = seconds() - timeDelta;        
            
            printf("obtainBall()    **forward\n");
            move(FORWARD);   /*** how much? ***/
            
            printf("obtainBall()    **turn\n");
            timeBase = seconds();   /* return to original orientation (i.e. parallel with side wall) */
            while ((seconds() - timeBase) < timeDelta) { turn(LEFT + !currSide); } 
            
            printf("obtainBall()    **forward\n");
            move(FORWARD);  /* move closer to balls */
            wait(0.35);
            
            return (obtainBall(1));  /* now, attempt to obtain ball */
        }
    }    
    return (getBallColor());
}

/* drop ball, currently held, over edge of board */
void removeBall() {
    float BALL_DIST = 1.0, timeBase, timeDelta;
    
    printf("removeBall( %d )\n", currSide);
    stop();
    moveArm(FRONT, RAISE);
    
    printf("removeBall()    back tilt\n");
    while (getTilt() == 0) { move(BACKWARD); }
    wait(0.5);
    
    printf("removeBall()    turn no tilt\n");
    timeDelta = seconds();
    while (getTilt() != 0) { turn(LEFT + currSide); }  /* turn until perpendicular with side wall */
    timeDelta = seconds() - timeDelta;
    
    printf("removeBall()    forward\n");
    move(FORWARD);   /*** how much? ***/
    
    /* rotate to face side wall */
    printf("removeBall()    turn wall\n");
    turn(LEFT + currSide);
    while (getTilt() != 0) { turn(LEFT + currSide); }
    
    while (getDistance() > BALL_DIST/2.0) { move(FORWARD); }  /* position arm over edge of board*/
    stop();
    
    printf("removeBall()    release!\n");
    release();  /* drop ball on floor! */
    
    printf("removeBall()    reorient\n");
    inch(BACKWARD);    
    timeBase = seconds();   /* return to original orientation (i.e. parallel with side wall) */
    while ((seconds() - timeBase) < timeDelta) { turn(LEFT + currSide); } 
    
    move(FORWARD);  /* move closer to balls */
    wait(0.5);
    stop();
}

/* initiates wall following algorithm */
void followWall() {
    int sensor;
    float lastPress;
    
    printf("followWall()\n");
    if (pressed(LEFT_TOUCH))
      sensor = LEFT_TOUCH;
    else
      sensor = RIGHT_TOUCH;
    
    /* algorithm: turn away from wall and move foward while sensor depressed
       turn toward wall and move foward while sensor not depressed */
    lastPress = seconds();
    while (pressed(sensor)) {
        if ((seconds() - lastPress) > 3.0) { /* TIMEOUT -- robot jammed */
            printf("followWall()    jammed\n");
            move(BACKWARD);                  /* attempt to free robot */
            wait(1.0);
            turn(LEFT + !(sensor - LEFT_TOUCH));
            wait(0.25);
            lastPress = seconds();
        }
        
        printf("followWall()    away\n");
        turn(LEFT + !(sensor - LEFT_TOUCH)); /* turn away from wall */
        wait(0.5);
        move(FORWARD);
        wait(0.75);
    }
    while (!pressed(sensor)) {
        printf("followWall()    toward\n");
        turn(LEFT + (sensor - LEFT_TOUCH)); /* turn toward wall */
        wait(0.5);
        move(FORWARD);
        wait(0.75);
    }
}

/* initiates line following algorithm */
void followLine(int lineColor) {
    int tColors[6];
    float entryTime;
    
    int i;
    for (i = LEFT_LIGHT; i <= RIGHT_LIGHT; i++) {
        entryTime = seconds();
        while ((tColors[i] = getColor(i)) == -1) {
            if ((seconds() - entryTime) > 0.1) { /* TIMEOUT -- sensor cannot resolve color */
                printf("followLine()    color?\n");
                tColors[i] = lineColor;         /* assign line color as default; minimizes movement */
                break;
            }
        }
    }
    
    /* algorithm: see 6.270 Course Notes */
    printf("followLine()\n"); 
    if (tColors[LEFT_LIGHT] != lineColor) {
        if (tColors[CENTER_LIGHT] != lineColor) {
            if (tColors[RIGHT_LIGHT] != lineColor) {
		    printf(" ! ! !\n");
                turn(LEFT + team);
            }
            else {
		    printf(" ! ! =\n");
                turn(RIGHT);
            }
        }
        else {
            if (tColors[RIGHT_LIGHT] != lineColor) {
		    printf(" ! = !\n");
                move(FORWARD);
                wait(0.25);
            }
            else {
		    printf(" ! = =\n");
                inch(RIGHT);
            }
        }
    }
    else {
        if (tColors[CENTER_LIGHT] != lineColor) {
            if (tColors[RIGHT_LIGHT] != lineColor) {
		    printf(" = ! !\n");
                turn(LEFT);
            }
            else {
		    printf(" = ! =\n");
                move(FORWARD);
            }
        }
        else {
            if (tColors[RIGHT_LIGHT] != lineColor) {
		    printf(" = = !\n");
                inch(LEFT);
            }
            else {
		    printf(" = = =\n");
                inch(FORWARD);
            }
        }
    }
}

/* initialize positions of servos */
void initializeServos() {
    grasp();
    moveArm(FRONT, RAISE);
    moveArm(REAR, RAISE);
    enable_servos();
}

/* raises or lowers specified arm */
void moveArm(int arm, int dir) {
    int MAX[2] = { 3500, 4000 }, MIN[2] = { 1200, 2700 };  /*** PLACEHOLDERS ***/
    if (arm == FRONT)
       grasp();
    if (dir == RAISE)
       servo(ARM_SERVO + arm, MAX[arm]);
    else if (dir == LOWER)
       servo(ARM_SERVO + arm, MIN[arm]);   
}

/* close claw */
void grasp() {
    int POSITION = 500; 
    servo(ARM_SERVO, 0);
    servo(CLAW_SERVO, POSITION);
}

/* open claw */
void release() {
    int POSITION = 3000; 
    servo(CLAW_SERVO, POSITION);
}

/* start spinning of tail tip */
void spinTail() { motor(TAIL_MOTOR, -100); }

/* stop spinning of tail tip */
void stopTail() { off(TAIL_MOTOR); }

/* moves or turns robot by small amount */
void inch(int dir) {
    /* printf("inch( dir %d )\n", dir); */
    if (dir == FORWARD)
      move(FORWARD);
    else {
        if (dir == BACKWARD)
          move(BACKWARD);
        else {
            if (dir == LEFT)
              turn(LEFT);
            else {
                if (dir == RIGHT)
                  turn(RIGHT);
            }
        }
    }
    wait(0.1);
    stop();    
}

/* initiates turning of robot in specified direction at default speed */
void turn(int dir) { altTurn(dir, 5); }

/* initiates turning of robot in specified direction at specified speed */
/* dir: LEFT, RIGHT; speed: 0 - 5 */
void altTurn(int dir, int speed) {
    if (dir == LEFT) {
        motor(LEFT_MOTOR, -(speed * 20));
        motor(RIGHT_MOTOR, (speed * 20));
    }
    else {
        if (dir == RIGHT) {
            motor(LEFT_MOTOR, (speed * 20));
            motor(RIGHT_MOTOR, -(speed * 20));
        }
    }
    wait(0.25);
    
}

/* initiates moving of robot in specified direction at default speed */ 
void move(int dir) { altMove(dir, 5); }

/* initiates moving of robot in specified direction at specified speed */
/* dir: FORWARD, BACKWARD; speed: 0 - 5 */
void altMove(int dir, int speed) {
    if (dir == FORWARD) {
        motor(LEFT_MOTOR, (speed * 20));
        motor(RIGHT_MOTOR, (speed * 20));
    }
    else {
        if (dir == BACKWARD) {
            motor(LEFT_MOTOR, -(speed * 20));
            motor(RIGHT_MOTOR, -(speed * 20));
        }
    }
    wait(0.25);
}

/* stops movement of robot body */
void stop() {
    off(LEFT_MOTOR);
    off(RIGHT_MOTOR);
}

/* returns distance to object nearest front of robot */
float getDistance() {
    int DISTANCE = 2, NUM = 10;    
    return (-3.16 + 950.0/(8.58 + averageValues(DISTANCE, NUM)));
}

/* returns 1 if robot is inclined upward, -1 if inclined downward and 0 otherwise */
int getTilt() {
    int i, duty, pulse = 0;
    
    for (i = 0; i < 16; i++) 
      pulse += (pw/10);
    duty = pulse/16;
    
    if (duty > posGee)
      return 1;
    else {
        if (duty < negGee)
          return -1;
        else
          return 0;
    }
}

/* returns 1 if touch sensor is depressed and 0 otherwise */
int pressed(int sensor) {
    digital(sensor);         /* initial sensor reading invalid */
    return digital(sensor);
}

/* returns color detected by specified sensor or -1 if 
   color cannot be resolved */
int getColor(int port) {
    int NUM = 8;
    
    float level = averageValues(port, NUM);
    
    if ((int)level < tWhite[port - LEFT_LIGHT]) {
        /* printf("WHITE\n"); */
        return 1;    /* WHITE = 1 */
    }
    else {
        if ((int)level > tBlack[port - LEFT_LIGHT]) {
            /* printf("BLACK\n"); */
            return 0;  /* BLACK = 0 */
        }
        else {
            /* printf("Unknown\n"); */
            return -1; /* unable to resolve color */
        }
    }
}

/* returns color detected by light sensor in ball holder
   or -1 if color cannot be resolved */
int getBallColor() {
    int NUM = 8;
    
    float level = averageValues(CLAW_LIGHT, NUM);
    
    if ((int)level < bWhite) {
        /* printf("WHITE\n"); */
        return 1;    /* WHITE = 1 */
    }
    else {
        if ((int)level > bGreen) {
            /* printf("GREEN\n"); */
            return 0;  /* GREEN = 0 */
        }
        else {
            /* printf("Unknown"); */
            return -1; /* unable to resolve color */
        }
    }
}

/* returns average value for specified port */
float averageValues(int port, int sampleSize) {
    int i, sum = 0;
    
    analog(port);    /* initial sensor reading invalid */
    for (i = 0; i < sampleSize; i++) {
        sum += analog(port);
        wait(0.001);
    }
    
    return ((float)sum/(float)sampleSize);
}

/* replaces sleep() function */
void wait(float sec) {
    float timeBase = seconds();
    while ((seconds() - timeBase) < sec);
}

/* General HB Functions */
int wait_button() {
    while(!start_button() && !stop_button());
    if (start_button()) {
        while (start_button() || stop_button());
        return 1;
    }
    else {
        while (start_button() || stop_button());
        return 0;
    }
}

void wait_start() { while (!press_start()); }

void wait_stop() { while (!press_stop()); }

int press_start() {
  if (!start_button()) return 0;
  while (start_button());
  return 1;
}

int press_stop() {
  if (!stop_button()) return 0;
  while (stop_button());
  return 1;
}