#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 93  /* less than 92 (89-91) */
#define NEAR_OBJECT 95 /* greater than  */
#define CLOSE_OBJECT 115 /* greater than */

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

/* these are returned by iDecidedTurnDirection() */
#define TURN_LEFT 0
#define TURN_RIGHT 1

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 motorStop(void) {
  rightWheel(NO_SPEED);
  leftWheel(NO_SPEED);
}

void handlePossibleDistressState(void) {
  if (bIsDistressed() == TRUE) {
    gbDistressFlag = TRUE;
    motorStop();

    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");
  }
}

int iDecideTurnDirection(void) {
  int x, y;
  
  x = RIGHT_IR;
  delay(100);
  y = LEFT_IR;
  
/*
    y  x
   89  100
*/
  
  if (x > y) {
    /*
                   .
      ROBOT==>
               .
    */
    return(TURN_LEFT);
  }
  /*
  else:
               .
      ROBOT==>
                   .

  */
  return(TURN_RIGHT);

}

void motorLeftTurn(void) {
  int i;
  leftWheel(NO_SPEED);
  rightWheel(NO_SPEED);
  delay(100);
  for (i = 5000; i < TURN_SPEED; i += 1000) {
    rightWheel(i);
    delay(100);
  }
}

void motorRightTurn(void) {
  int i;
  leftWheel(NO_SPEED);
  rightWheel(NO_SPEED);
  delay(100);
  for (i = 5000; i < TURN_SPEED; i += 1000) {
    leftWheel(i);
    delay(100);
  }
}

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

  CLEAR;
  HOME;

  handleStart();

  x = 0;
  do {
START:  
    gbDistressFlag = FALSE;
    
    if (bObjectDetected(0) == TRUE) {
      printf("Object detected->");
      motorStop();
      delay(5000);
      if (iDecideTurnDirection() == TURN_RIGHT) {
        printf("right turn->");
        motorRightTurn();
      } else {
        printf("left turn->");
        motorLeftTurn();
      }
      delay(5000);
      i = 0;
      do {
        printf(".");
        handlePossibleDistressState();
        if (gbDistressFlag == TRUE)
          goto START;
        if (bObjectDetected(0) == FALSE)
          break;
        i++;
        if (i > 
      } while (1);
      printf("stopping->");
      motorStop();
      delay(5000);
      
      printf("cleared\n");
      
      goto START;
    }
    
    for (i = 5000; i <= MAX_SPEED; i += 1000) {
      leftWheel(i);
      rightWheel(i);
      delay(200);
    }
  
    do {
      x++;
      printf("cruising %d\n", x);
      delay(500);
      
      gbDistressFlag = FALSE;
      handlePossibleDistressState();
      if (gbDistressFlag == TRUE)
        goto START;
        
      if (bObjectDetected(0) == TRUE)
        break;

    } while (1);
    
  } while(1);
}