#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 <analog.h>
#include <motor5.h>
#include <misc.h>

int currDirection;

void setDirection(int x) {
  currDirection = x;
  DIRECTION = x;
}

void delay(int x) {
/* x == 5000 is ABOUT one second on an 8mhz 68HC11 */
  unsigned int i,z;
  for (i = 0; i < x; i++) {
    z = 65536;
    z += z/2;
    z /= z;
    z /= i;
    z -= z/2;
    z *= x;
  }
}

#define RIGHT_FRONT_BUMPER analog(6)
#define LEFT_FRONT_BUMPER analog(4)
#define LEFT_IR analog(2)
#define RIGHT_IR analog(3)

void WaitForSTART() {
  int x,y;
  printf("Waiting to START\n(press right front bumper to force start)\n");
  delay(5000);
  do {
    x = RIGHT_IR;
    y = LEFT_IR;
    if ((x < 91) & (y < 91)) {
      printf("No IR: RIR=%d LIR=%d\n", x, y);
      delay(1000);      
      break;
    }
    if (RIGHT_FRONT_BUMPER < 10) {
      printf("Right front bumper pressed\n");
      delay(1000);
      break;
    }
  } while(1);

}

void goForward() {
  printf("going foward\n");
  delay(1000);
  
  motor(1, 0);
  motor(2, 0);
  delay(5000);
  
  setDirection(0x80);
  delay(5000);
  
  motor(1, 15000);
  motor(2, 15000);
//  delay(5000);
}

void doStop() {
  int x,y,stop;
      printf("RIR=%d LIR=%d\n", x, y);
      delay(500);
      motor(1, 0);
      motor(2, 0);
      printf("stopped\n");
      delay(5000);
      stop = TRUE;
      do {
        if (RIGHT_IR <= 100) {
          if (LEFT_IR <= 100) {
            printf("object moved\n");
            delay(500);
            stop = FALSE;
          }
        }
      } while (stop == TRUE);
      
      printf("backing up\n");
      delay(500);
      setDirection(0x40);
      delay(5000);
      motor(1, 15000);
      motor(2, 15000);
      delay(5000);
      
      goForward();

}

void main() {

  int i,x,y,stop,k;

  INIT_SERIAL;
  INIT_ANALOG;
  IRE_ON;
  x = 0;
  
  init_motor();

  CLEAR;
  HOME;

  WaitForSTART();
  printf("Starting...\n");
  delay(3000);
  
  goForward();
  k = 0;
  
  do {
    delay(50);
    x = RIGHT_IR;
    y = LEFT_IR;
    if (x > 100)
      doStop();
    else if (y > 100)
      doStop();
      
    k++;
    printf("cruising %d\r", k);
    delay(500);
  } while(1);  
  
}