#include <stdio.h>    /* printf and other standard I/O routines */
#include <hc11.h>     /* define HC11 registers */
#include <vectors.h>  /* define interrupt vector array */
#include <isrdecl.h>  /* initialize unused interrupt service routines */
#include <serial.h>   /* initialize serial I/O to PC */
#include <motor5.h>
#include <misc.h>

#define NO_SPEED 0
#define LOW_SPEED 5000
#define MAX_SPEED 15000
#define TURN_SPEED 28000

#define RIGHT_FRONT_BUMPER analog(6)
#define LEFT_FRONT_BUMPER analog(4)
#define LEFT_IR analog(2)
#define RIGHT_IR analog(3)
#define START_BUMPER analog(0)
#define EDGE_DETECT analog(1)

#define NO_OBJECT 92  /* less than 92 (89-91) */
#define NEAR_OBJECT 93 /* greater than 96 */
#define CLOSE_OBJECT 115 /* greater than 112 */

#define rightWheel(x) motor(1,x)
#define leftWheel(x) motor(2,x)

#define STOP; rightWheel(NO_SPEED);\
             leftWheel(NO_SPEED);

BOOL gbDistressFlag;
BOOL gbObjectDetected;

void delay(int x) {
/* x == 5000 is ABOUT one second on an 8mhz 68HC11.  This
     function is NOT for precision timing, only for simple 
     program delays. */
  unsigned int i,z;
  for (i = 0; i < x; i++) {
    z = 65536;
    z += z/2;
    z /= z;
    z /= i;
    z -= z/2;
    z *= x;
  }
}

void handleStart() {
  printf("READY: ");
  do {
    if (START_BUMPER > 100)
      break;
    delay(100);
  } while(1);
  printf("Starting\n");
  delay(1000);
}

void enableDistressSignal() {
  motor(5, 40000);
}

void disableDistressSignal() {
  motor(5, 0);
}

BOOL bObjectDetected(int offset) {
  int i,x,y;
  x = 0;
  y = 0;
  for (i = 0; i < 5; i++) {
    if (RIGHT_IR > (NEAR_OBJECT+offset))
      x++;
    if (LEFT_IR > (NEAR_OBJECT+offset))
      y++;
    delay(10);
  }
  if (x > 0)
    return(TRUE);
  if (y > 0) 
    return(TRUE);
  return(FALSE);
}

void turnLeft() {
  rightWheel(NO_SPEED);
  leftWheel(TURN_SPEED);
  do {
    delay(200);
  } while (bObjectDetected(1) == TRUE);
  STOP;
}

void turnRight() {
  leftWheel(NO_SPEED);
  rightWheel(TURN_SPEED);
  do {
    delay(200);
  } while (bObjectDetected(1) == TRUE);
  STOP;
}

void main() {
  int x;
  
  INIT_SERIAL;
  INIT_ANALOG;
  IRE_ON;
  
  init_motor();

  CLEAR;
  HOME;

  handleStart();
  
  do {
    x = 0;
START:  
    if (bObjectDetected(0) == TRUE) {
      printf("Distress!\n");
      enableDistressSignal();
      do {
        if (bObjectDetected(0) == FALSE)
          break;
      } while (1);
      printf("End distress!\n");
      disableDistressSignal();
    }
  } while(1);
}