/**********************
 *  Demo code for 6.270 2001
 *  This shows the reflex control style of generating code.
 **********************/

#define TRUE 1
#define FALSE 0

#define ENCODER_PORT 7

#define SERVO_PORT 5
#define SERVO_LEFT 0
#define SERVO_RIGHT 4000
#define SERVO_STRAIGHT 2000

#define DISTANCE_90 193

#define LEFT_BUMP_PORT 8
#define RIGHT_BUMP_PORT 9

#define MOTOR_1 2
#define MOTOR_2 3

#define LINE_PORT 16
#define LINE_TRUE 3
#define LINE_FALSE 30

/*  This is already defined in our libraries 
int abs(int x) {
  return (x>0)?x:-x;
}
*/
int threshold(int port, int falseValue, int trueValue) {

  int sample = analog(port);
  int distFromTrue = abs(sample-trueValue);
  int distFromFalse = abs(sample-falseValue);

  if (distFromTrue < distFromFalse) return TRUE;
  else return FALSE;
}

int line() {
  return (threshold(LINE_PORT, LINE_FALSE, LINE_TRUE));
}

int right_bump() {
  return digital(RIGHT_BUMP_PORT);
}

int left_bump() {
  return digital(LEFT_BUMP_PORT);
}


void setup() {

  ir_transmit_off();
  
  enable_servos();
  enable_encoder(ENCODER_PORT);

  printf("Press Start!\n");
  start_press();

}

void drive(int speed) {
  motor(MOTOR_1, speed);
  motor(MOTOR_2, speed);
}

void veer_right() {
  servo(SERVO_PORT, (SERVO_RIGHT*1 + SERVO_STRAIGHT*9)/10);
  drive(100);
}

void veer_left() {
  servo(SERVO_PORT, (SERVO_LEFT*1 + SERVO_STRAIGHT*9)/10);
  drive(100);
}

void turn_left() {
  servo(SERVO_PORT, SERVO_LEFT);
  drive(100);
}

void right_wall_follow() {
  while(!line()) {
    if (right_bump()) veer_left();
    else veer_right();
  }
}

void left_90() {
  reset_encoder(ENCODER_PORT);
  while(read_encoder(ENCODER_PORT) < DISTANCE_90) {
    turn_left();
  }
}

void main() {
  printf("Hello, World!\n");

  setup();

  /* Orient(); */

  right_wall_follow();

  left_90();

  /* follow_line() */

}