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

#define LEFT; rightWheel(NO_SPEED); \
              leftWheel(TURN_SPEED);

#define RIGHT; leftWheel(NO_SPEED); \
               rightWheel(TURN_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 < 4; i++) {
    if (RIGHT_IR > (NEAR_OBJECT+offset))
      x++;
    delay(100);
    if (LEFT_IR > (NEAR_OBJECT+offset))
      y++;
    delay(100);
  }
  if (x > 2)
    return(TRUE);
  if (y > 2) 
    return(TRUE);
    
  return(FALSE);
}

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) {
  if (bIsDistressed() == TRUE) {
    gbDistressFlag = TRUE;
    STOP;

    enableDistressSignal();
    
    while (bIsDistressed() == TRUE) {
      delay(3000);
    }
    
    printf("Wait for IR to clear\n");
    delay(500);
    
    do {
      delay(100);
    } while (bObjectDetected(0) == TRUE);
    
    disableDistressSignal();
    printf("End DISTRESS\n");
  }
}

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

  CLEAR;
  HOME;

  handleStart();

  x = 0;
  do {
START:  
    gbDistressFlag = FALSE;
    
    if (bObjectDetected(0) == TRUE) {
      printf("Object detected->");
      STOP;
      delay(5000);
      if (RIGHT_IR > LEFT_IR) {
        printf("turning right->");
        RIGHT;
      } else {
        printf("turning left->");
        LEFT;
      }
      
      do {
        handlePossibleDistressState();
        if (gbDistressFlag == TRUE)
          goto START;
        if (bObjectDetected(0) == FALSE)
          break;
      } while (1);
      STOP;
      delay(5000);
      
      printf("cleared\n");
      
      goto START;
    }
    
    leftWheel(MAX_SPEED);
    rightWheel(MAX_SPEED);
  
    do {
      x++;
      printf("cruising %d\n", x);
      delay(500);
      
      handlePossibleDistressState();
      if (gbDistressFlag == TRUE)
        goto START;

    } while (bObjectDetected(0) == FALSE);
    
  } while(1);
}