#include <iostream>
using namespace std;

#include <stdlib.h>

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

//enum {backwd, stopped, forwd} direction;

int tolerance = 5; // north +- tolerance is good enough
int degChange = 10; // how many degrees change for each pass - forward or backward.
                   // last pass will use tolerance
int minReverse = 36;  // speed for backup movement
int minForward = 12;  // speed for forward movement
int turnSetting = 35;  // how much wheels are turned

void turnIt(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);

  Servo turn(i2cHandle, 1100, 1900, 3);
  Motor go(i2cHandle, 1100, 1900, 4, 2);
//int i = 0;
  deg = compass.readC();
  while(!(deg >=0 && deg <= tolerance || deg >= (360 - tolerance) && deg < 360))
  {
//if(i++ > 2) break;
    turnIt(compass, turn, go);
    go.stop();
    sleep(1);
    deg = compass.readC();
    printf("Deg: %.1f\n", deg);
    deg = compass.readC();
  }

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

void turnIt(Cmps03 &compass, Servo &turn, Motor &go)
{
  float oldDeg = compass.readC();
  float deg = oldDeg;
  float newDeg;

  cout << "current heading ";
  printf("%.1f\n", oldDeg);

  if(deg < 180)
  {
    turn.right(turnSetting);
    newDeg = oldDeg - degChange;
    if(newDeg < degChange)
      newDeg = tolerance;

    cout << "turn right in reverse heading for ";
    printf("%.1f\n", newDeg);

    go.reverse(minReverse);
    while (deg > newDeg && deg < oldDeg + 1)
    {
      printf("%.1f\n", deg);
      deg = compass.readC();
    }
    cout << "right stopping" << endl;
  }
  else
  {
    turn.left(turnSetting);
    newDeg = oldDeg + degChange;
    if(newDeg > 360 - degChange)
      newDeg = 360 - tolerance;

    cout << "turn left in backward heading for ";
    printf("%.1f\n", newDeg);

    go.reverse(minReverse);
    while (deg < newDeg && deg > oldDeg - 1)
    {
      printf("%.1f\n", deg);
      deg = compass.readC();
    }
    cout << "left stopping" << endl;
  }
}