#include <iostream>

using namespace std;

#include "Motor.h"
#include "Servo.h"
#include "Srf10.h"

int optDistWall = 40; // value is in cm, optimal distance from wall
int minDistWall = optDistWall * 3 / 4; // value is in cm, optimal - 1/4
int maxDistWall = optDistWall * 5 / 4; // value is in cm, optimal + 1/4
int stopDist = 25; // value is in cm = 10 inches
int extreme = 75;
int moveDist = 40;

int minReverse = 18;  // speed for backup movement
int minForward = 18;  // speed for forward movement

void process(Srf10 &sensorC, Srf10 &sensorL, 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);
  Servo turn(i2cHandle, 1100, 1894, 3); // steering servo
  Srf10 sensorC(i2cHandle, 0x72, 'c'); // center sensor
  Srf10 sensorL(i2cHandle, 0x70, 'c'); // left sensor

  int i;
  int distL;

  sensorL.ping();
  usleep(1000);
  distL = sensorL.readS();
  usleep(1000);

  while(distL < optDistWall)
  {
    turn.right(extreme);
    usleep(1000);
    go.forward(minForward);
    usleep(1000);

    for(i = 0;i < moveDist; i++)
      usleep(10000);

    go.stop();
    usleep(1000);

    usleep(60000);

    turn.left(extreme);
    usleep(1000);
    go.forward(minForward);
    usleep(1000);

    for(i = 0; i < moveDist; i++)
      usleep(10000);

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

    usleep(60000);

    go.reverse(minReverse);
    usleep(1000);
    for(i = 0; i < moveDist * 3 / 2; i++)
      usleep(10000);

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

    usleep(60000);

    sensorL.ping();
    usleep(1000);
    distL = sensorL.readS();
    usleep(1000);
  }

  return 0;
}