#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>

#define MAX_SPEED 13000
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;
    
    printf("RIR=%d LIR=%d  \r", x, y);
    delay(1200);
    
    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 main() {

  int i,x,y,stop,k,left = 0, right = 0;

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

  CLEAR;
  HOME;

  WaitForSTART();
  printf("Starting...\n");
  delay(5000);
  
  setDirection(0x80);
  delay(3000);
  motor(1, MAX_SPEED);
  motor(2, MAX_SPEED);
  delay(5000);
  
  k = 0;
  do {
    k++;
    
    x = RIGHT_IR;
    y = LEFT_IR;
//    printf("cruising %d (RIR=%d, LIR=%d)  \r", k, x, y);
//    delay(500);
    
    if (x > 100) {
      right++;
      printf("right ir!\n");
      motor(1, 0);
      motor(2, 0);
      delay(5000);

        motor(2, 18000);
        delay(18000);
        motor(1, 0);

      do { } while(RIGHT_IR > 100);
      delay(5000);
      
      
      motor(1, MAX_SPEED);
      motor(2, MAX_SPEED);
    }
    if (y > 100) {
      left++;
      printf("left ir!\n");    
      motor(1, 0);
      motor(2, 0);
      delay(5000);

        motor(1, 18000);
        delay(18000);
        motor(2, 0);

      do { } while(LEFT_IR > 100);
      delay(5000);
      
      motor(1, MAX_SPEED);
      motor(2, MAX_SPEED);
    }

  } while (1);
  
  
}