/* -----------------------------------
   Phoenix
   by Rodney Graham, Jamie Hope
   30 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

#define DRIVE_POS 1300

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) */

void main() {

    printf("Phoenix\n");
    wait_start();

   /*  calibrate(); */

    initializeServos();
    removeBall();

    ao();
    printf("end Phoenix\n");
}

void removeBall() {
    int i = 0;
    float timeBase;

    printf("...removing ball");
   /* while (getColor(LEFT_LIGHT) == 0) {
	inch(LEFT);
    } */
 
    moveArm(REAR, DRIVE_POS);
    driveTail();
    timeBase = seconds();	
    while ((seconds() - timeBase) < 8.0) {
	move(BACKWARD);
      wait(0.5);
      if (i % 6 == 0) {
          inch(LEFT);
          move(BACKWARD); 
      }
      
    }
    move(FORWARD);
    wait(2.5);
    stop();
    moveArm(REAR, LOWER);
    wait(1.0);
    spinTail();
    timeBase = seconds();
    while ((seconds() - timeBase) < 14.0) {
      inch(BACKWARD);
      wait(0.5);
    }
	

    moveArm(REAR, RAISE);
    move(FORWARD);
    wait(3.0);
   /* turn(RIGHT);
    wait(2.0);
    move(FORWARD);
    wait(2.0);
    turn(RIGHT);
    wait(2.0);
    move(FORWARD); */
    moveArm(REAR, DRIVE_POS);
    spinTail();
    wait(4.0);
    celebrate();
}

void celebrate() {
    int i, pid[4];
    float timeBase = seconds();

    pid[0] = start_process(shakeHead(timeBase));
   /* pid[1] = start_process(wagTail(timeBase + 1.0)); */
    pid[2] = start_process(dance(timeBase + 1.0));
    
    wait(23.0);
    
    for (i = 0; i < 2; i++) {
        kill_process(pid[i]);
    }
}

void dance(float timeBase) {
    int dir = 0;
    
    while (1) {
      if ((getTime(timeBase) % 6) == 0) {
          if (dir == 1) {
		  move(FORWARD);
		  spinTail();
              dir = 0;
	    }
	    else {
              move(BACKWARD);
		  driveTail();
  		  dir = 1;
          }
       }
       wait(1.0);
     }
}
 
void shakeHead(float timeBase) {
    int dir =1;

    while (1) {
	if ((getTime(timeBase) % 3) == 0) {
	    moveArm(FRONT, dir);
	    dir = -dir;
	}
    }
    wait(1.0);
}

void wagTail(float timeBase) {
    int dir = 1;

    while (1) {
	if ((getTime(timeBase) % 3) == 0) {
	    moveArm(REAR, dir);
	    if (dir == 1)
             dir = 3;
          else
             dir = 1;
 	}
    }
    wait(1.0);
}

void moveMouth(float timeBase) {
     int dir = 1;

     while (1) {
	if ((getTime(timeBase) % 2) == 0) {
          if (dir == 1)
		  release();
	    else
		  grasp();
	    dir = -dir;
      }
     }
     wait(1.0);
}

int getTime(float timeBase) { return (int)(seconds() - timeBase); }



/* 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));

            }
        }
        
       /* printf("Acc Calibration?y:START n:STOP\n");
        if (wait_button()) {
            int duty;
            
            start_accel(2);
            printf("position on a   plateau, START\n");
            wait_start();
            
            while(!press_start()) {            
                duty = 0;
                for (i = 0; i < 16; i++) {
                    duty += (pw /10);
                    wait(0.01);
                }
                duty /= 16;
                
                printf("zero gees: %d    press start\n", duty);
            }
            accOffset = duty;
 		
            printf("Thresholds?     y:START n:STOP\n");
            if (wait_button()) {
                while(!press_start()) {
                    duty = 0;
		        for (i = 0; i < 16; i++) {
                        duty += (pw/10);
                        wait(0.01);
                    }
                    duty /= 16;
                    duty -= accOffset;
	 	        printf("pos gees: %d     press start\n", duty);
		    }
                posGee = duty;

                while(!press_start()) {
                    duty = 0;
		        for (i = 0; i < 16; i++) {
                        duty += (pw/10);
                        wait(0.01);
                    }
                    duty /= 16;
			  duty -= accOffset;
	 	        printf("neg gees: %d     press start\n", duty);
		    }
                negGee = duty;
            }
            stop_accel(2);
        } */                    
        printf("Recalibrate?    y:START n:STOP\n");
    }
    /*printf("dark %d %d %d %d", tBlack[0], tBlack[1], tBlack[2], bGreen);
    wait(1.5);
    printf("light %d %d %d %d", tWhite[0], tWhite[1], tWhite[2], bWhite);
    wait(1.5);
    printf("acc %d %d %d", accOffset, posGee, negGee);
    wait(1.5); */
}


/* 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, 2650 };  /*** PLACEHOLDERS ***/
    if (arm == FRONT)
       grasp();
    if (dir == RAISE)
       servo(ARM_SERVO + arm, MAX[arm]);
    else if (dir == LOWER)
       servo(ARM_SERVO + arm, MIN[arm]);
	   else if (dir == DRIVE_POS)
       servo(ARM_SERVO + arm, DRIVE_POS);
              else
                 servo(ARM_SERVO + arm, 3200);
}

/* 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);
}

void driveTail() { motor(TAIL_MOTOR, 100); }

/* 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 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;
}