#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 SPIN_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 94  /* less than 92 (89-91) */
#define NEAR_OBJECT 99 /* greater than 96 */
#define CLOSE_OBJECT 115 /* greater than 112 */

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

BOOL bIsNearObject(void) {
  /*
  bIsNearObject = (RIGHT_IR > NEAR_OBJECT) | (LEFT_IR > NEAR_OBJECT));
  
  Do it this way in case IC11 compiler has problems interpreting
    compound logic equations.
  */
  if (RIGHT_IR > NEAR_OBJECT)
    return(TRUE);
  if (LEFT_IR > NEAR_OBJECT)
    return(TRUE);
  return(FALSE);
}

BOOL bNoObjectDetected(void) {
  /* must do this way since IC11 compiler apparently has problems 
       with compound AND statements */
  /* This is equivalent to:
       bNoObjectDetected = (RIGHT_IR < NO_OBJECT) & (LEFT_IR < NO_OBJECT)
  */
  int x,y;
  x = RIGHT_IR;
  y = LEFT_IR;
//  printf("RIR=%d LIR=%d\n", x, y);
  if (x < NO_OBJECT) {
    if (y < NO_OBJECT)
      return(TRUE);
  }
  if (y < NO_OBJECT) {
    if (x < NO_OBJECT)
      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(3000);
    }
    
//    printf("Wait for IR to clear\n");
    delay(500);
    
    do {
      delay(100);
    } while (bNoObjectDetected() == FALSE);
    
    disableDistressSignal();
//    printf("End DISTRESS\n");
  }
}

void main() {

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

  CLEAR;
  HOME;

  handleStart();
  
  x = 0;
//  do {
START:
//    printf("START\n");  /* motors should be off if reach this point */
    gbDistressFlag = FALSE;  

    /* wait for IR to clear */
    do {
      handlePossibleDistressState(); 
      if (gbDistressFlag == TRUE)
        goto START;
    } while (bNoObjectDetected() == FALSE);
    
    /* should not be distressed or any object detected, 
         so go forward */
    x++;
//    printf("cruising %d", x);
    rightWheel(MAX_SPEED);  /* get the motors started */
    leftWheel(MAX_SPEED);
    delay(1500);
    rightWheel(CRUISE_SPEED);  /* throttle down to cruising speed */
    leftWheel(CRUISE_SPEED);
    
    do {
      /* motors should be cruising...*/
//      printf(".");
      
      handlePossibleDistressState();
      if (gbDistressFlag == TRUE) {
//        printf("Object detected while cruising\n");
        /* Then the motors have stopped */
        if (bNoObjectDetected() == TRUE)
          /* Restart with intention of going forward */
          goto START;
        /* Otherwise, an object is detected, so force a 
             turning decision. */
        goto forceTurn;
      }
      /* else the motors are still cruising... */
        
      gbObjectDetected = FALSE;
checkNear:
      if (bIsNearObject()) {
forceTurn:      
        /* an object was detected, stop the motors and decide
             what to do... */
        gbObjectDetected = TRUE;
//        printf("Stopping\n");
        rightWheel(NO_SPEED);
        leftWheel(NO_SPEED);
        delay(5000);
//        printf("Turn decision: ");
                
        if (RIGHT_IR > LEFT_IR) {
          /* There is an object closer to the right than to the left:
                     .
              -->
                  .
              Enable the right wheel to turn left, until an open
                direction is found.
              
          */
          
//          printf("turning RIGHT\n");
          rightWheel(SPIN_SPEED);
          leftWheel(NO_SPEED);
          do {
            /* right motor should be turning...*/
            handlePossibleDistressState();
            if (gbDistressFlag == TRUE) {
              /* We ran into something and now the motors are
                   stopped, so check IR to decided what to do. */
              if (bNoObjectDetected())
                goto START;
              /* an object was detected, so force a turn decision */
              goto forceTurn;
            }
          } while (bNoObjectDetected() == FALSE);
//          printf("end RIGHT turn: RIR=%d LIR=%d\n", RIGHT_IR, LEFT_IR);
          /* No object should be in this direction, so stop the motors
               and recheck. */
          rightWheel(NO_SPEED);
          leftWheel(NO_SPEED);
          delay(5000);
          if (bNoObjectDetected())
            goto START;
          goto forceTurn;
        }
        else {
          /* There is an object closer to the left than to the right:
                  .
              -->
                     .
              Enable the left wheel to turn right, until an open
                direction is found.
              
          */

//          printf("turning LEFT\n");
          leftWheel(SPIN_SPEED);
          rightWheel(NO_SPEED);
          do {
            /* left motor should be turning...*/          
            handlePossibleDistressState();
            if (gbDistressFlag == TRUE) {
              /* We ran into something and now the motors are
                   stopped, so check IR to decided what to do. */
              if (bNoObjectDetected())
                goto START;
              /* an object was detected, so force a turn decision */
              goto forceTurn;
            }
          } while (bNoObjectDetected() == FALSE);
          /* No object should be in this direction, so stop the motors
               and recheck. */
//          printf("end LEFT turn\n");
          leftWheel(0);
          rightWheel(0);
          delay(5000);
          if (bNoObjectDetected())
            goto START;
          goto forceTurn;
        }
      }
      if (gbObjectDetected == TRUE) {
        /* An object was detected, and we should have turned to a
             direction that is not obstructed.  But the motors
             have stopped, so restart with the intention of
             going forward again. */
        goto START;
      }
      /* otherwise, continue cruising...*/
    } while (1); 
    
//  } while(1);
  
}