#include <iostream>
using namespace std;

#include <stdlib.h>

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

float deg;
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 = 20;  // speed for backup movement
int minForward = 20;  // speed for forward movement
int turnSetting = 75;  // how much wheels are turned
int i2cBusWait = 1000;

void firstLoop(Cmps03 &compass, Servo &turn, Motor &go);
void secondLoop(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);
  }

  Motor go(i2cHandle, 1100, 1876, 4, 3);
  usleep(i2cBusWait);
  Servo turn(i2cHandle, 1100, 1894, 3);
  usleep(i2cBusWait);
  Cmps03 compass(i2cHandle);
  usleep(i2cBusWait);

  deg = compass.readC();
  usleep(i2cBusWait);

  firstLoop(compass, turn, go);
  secondLoop(compass, turn, go);

  go.stop();
  usleep(i2cBusWait);
  turn.center();
  usleep(i2cBusWait);

  return 0;
}

void firstLoop(Cmps03 &compass, Servo &turn, Motor &go)
{
  int passed360 = 0;
  float curDeg;
  int done = 0;

  turn.right(turnSetting);
  usleep(i2cBusWait);
  go.forward(minForward);
  usleep(i2cBusWait);

  while(!done)
  {
    curDeg = compass.readC();
    usleep(i2cBusWait);
    if(deg > 340)
    {
      if(curDeg < deg)
      {
        done = 1;
        passed360 = 1;
      }
    }
    else
      if(curDeg > deg + 10)
        done = 1;
  }

  done = 0;

  while(!done)
  {
    curDeg = compass.readC();
    usleep(i2cBusWait);
    if(!passed360)
    {
      if(deg > curDeg)
        passed360 = 1;
    }
    else
    {
      if(curDeg >= deg - 1)
        done = 1;
    }
  }
}

void secondLoop(Cmps03 &compass, Servo &turn, Motor &go)
{
  int passed0 = 0;
  float curDeg;
  int done = 0;

  turn.left(turnSetting);
  usleep(i2cBusWait);
  go.forward(minForward);
  usleep(i2cBusWait);

  while(!done)
  {
    curDeg = compass.readC();
    usleep(i2cBusWait);
    if(deg < 20)
    {
      if(curDeg > deg)
      {
        done = 1;
        passed0 = 1;
      }
    }
    else
      if(curDeg < deg - 10)
        done = 1;
  }

  done = 0;

  while(!done)
  {
    curDeg = compass.readC();
    usleep(i2cBusWait);
    if(!passed0)
    {
      if(deg < curDeg)
        passed0 = 1;
    }
    else
    {
      if(curDeg <= deg + 1)
        done = 1;
    }
  }
}