#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 CRUISE_SPEED 10000
#define MAX_SPEED 18000

#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 93
#define NEAR_OBJECT 100
#define CLOSE_OBJECT 115

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

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() {
  int x,y;
  printf("Waiting to START\n(press upper rear switch)\n");
  do {
    x = START_BUMPER;
    if (x > 100)
      break;
    delay(100);
  } while(1);
  printf("Starting...\n");
  delay(1000);
}

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

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

BOOL bIsDistressed() {
  int x,y;

  x = LEFT_FRONT_BUMPER;
  if (x < 10) {
    printf("DISTRESS: LFB\n");
    delay(500);
    return(TRUE);
  }

  y = RIGHT_FRONT_BUMPER;
  if (y < 10) {
    printf("DISTRESS: RFB\n");
    delay(500);
    return(TRUE);
  }
    
/*
  if (EDGE_DETECT > 127) {
    printf("DISTRESS: ED\n");
    delay(500);
    return(TRUE);
  }
*/
  
  return(FALSE);
}

void handlePossibleDistressState(void) {
  int x,y;
  if (bIsDistressed() == TRUE) {
    gbDistressFlag = TRUE;
    rightWheel(NO_SPEED);
    leftWheel(NO_SPEED);

    enableDistressSignal();
    
    while (bIsDistressed() == TRUE) {
      delay(5000);
    }
    
    printf("Wait for IR to clear\n");
    delay(500);
    
    do {
      delay(100);
    } while ((RIGHT_IR > NO_OBJECT) & (LEFT_IR > NO_OBJECT));
    
    disableDistressSignal();
    printf("End DISTRESS\n");
  }
}

void main() {

  int x;
  
  INIT_SERIAL;
  init_motor();
  INIT_ANALOG;
  IRE_ON;

  CLEAR;
  HOME;

  handleStart();

  do {
    printf("PWM: ");
    x = read_int();
    leftWheel(x);
    delay(10000);
    leftWheel(0);

  } while(1);
  
 
}