/*
  Michael Simmons
  Master Project: 
   gRAPI: an indoor four-wheeled robot API for a gumstix connex
  
  Last Modified: 17 Feb 2009
  
  Point North test program
*/

#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Servo.h"
#include "Motor.h"
#include "Cmps03.h"

enum {backwd, stopped, forwd} direction;

int tolerance = 3; // north +- tolerance is good enough
int degChange = 5; // how many degrees change for each pass - forward or backward.
                   // last pass will use tolerance
int minReverse = 21;  // speed for backup movement
int minForward = 18;  // speed for forward movement
int turnSetting = 50;  // how much wheels are turned
int i2cBusWait = 1000;  // how long to wait after an i2c command
int moveDist = 500;

void turnLeft(Cmps03 &compass, Servo &turn, Motor &go);
void turnRight(Cmps03 &compass, Servo &turn, Motor &go);

int main()
{
  int i2cHandle;

  if((i2cHandle = open("/dev/i2c-0", O_RDWR)) < 0)
  {
    cout << "Open Failed" << endl;
    exit(1);
  }

  float deg;

  Cmps03 compass(i2cHandle);

  Motor go(i2cHandle, 1100, 1876, 4, 3);
  usleep(i2cBusWait);
  Servo turn(i2cHandle, 1100, 1894, 3); // steering servo
  usleep(i2cBusWait);

  direction = backwd;

  deg = compass.readC();
  usleep(i2cBusWait);
  while(!(deg >=0 && deg <= tolerance || deg >= (360 - tolerance) && deg < 360))
  {
    if(deg <= 180)
    {
      if(deg <= 15)
        if(deg <= 5)
          turnSetting = 25;
        else
          turnSetting = 35;
      if(direction == backwd)
        turnRight(compass, turn, go);
      else
        turnLeft(compass, turn, go);
    }
    else
    {
      if(deg >= 345)
        if(deg >= 355)
          turnSetting = 25;
        else
          turnSetting = 35;
      if(direction == backwd)
        turnLeft(compass, turn, go);
      else
        turnRight(compass, turn, go);
    }
    sleep(2);
    deg = compass.readC();
    usleep(i2cBusWait);
  }

  go.stop();
  usleep(i2cBusWait);
  turn.center();
  usleep(i2cBusWait);
  deg = compass.readC();
  printf("Final Heading Deg: %.1f\n", deg);
  return 0;
} // main

void turnRight(Cmps03 &compass, Servo &turn, Motor &go)
{
  float deg;
  int i;

  turn.right(turnSetting);
  usleep(i2cBusWait);
  switch(direction)
  {
    case backwd:
      deg = compass.readC();
      usleep(i2cBusWait);
      cout << "current heading ";
      printf("%.1f\n", deg);

      go.reverse(minReverse);
      usleep(i2cBusWait * moveDist * tolerance);
      break;
    case forwd:
      deg = compass.readC();
      usleep(i2cBusWait);
      cout << "current heading ";
      printf("%.1f\n", deg);

      go.forward(minForward);
      usleep(i2cBusWait * moveDist * tolerance);
  }
  go.stop();
  usleep(i2cBusWait);
  cout << "right stopping" << endl;
  if(direction == backwd)
    direction = forwd;
  else
    direction = backwd;
} // turnRight

void turnLeft(Cmps03 &compass, Servo &turn, Motor &go)
{
  float deg;
  int i;

  turn.left(turnSetting);
  usleep(i2cBusWait);
  switch(direction)
  {
    case backwd:
      deg = compass.readC();
      usleep(i2cBusWait);
      cout << "current heading ";
      printf("%.1f\n", deg);

      go.reverse(minReverse);
      usleep(i2cBusWait * moveDist * tolerance);
      break;
    case forwd:
      deg = compass.readC();
      usleep(i2cBusWait);
      cout << "current heading ";
      printf("%.1f\n", deg);

      go.forward(minForward);
      usleep(i2cBusWait * moveDist * tolerance);
  }
  go.stop();
  usleep(i2cBusWait);
  cout << "left stopping" << endl;
  if(direction == backwd)
    direction = forwd;
  else
    direction = backwd;
} // turnLeft