#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 i2cBusWait = 1000;
int sensorWait = 30;

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

  int i;
  int distL;

  sensorL.setGain(9);
  usleep(i2cBusWait);
  sensorL.setRange(47);
  usleep(i2cBusWait);
  sensorC.setGain(9);
  usleep(i2cBusWait);
  sensorC.setRange(47);
  usleep(i2cBusWait);

  sensorL.ping();
  usleep(i2cBusWait * sensorWait);
  distL = sensorL.readS();
  usleep(i2cBusWait);

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

    usleep(i2cBusWait * moveDist);
/*
    for(i = 0;i < moveDist; i++)
      usleep(10000);
*/
    go.stop();
    usleep(i2cBusWait);

    turn.center();
    usleep(i2cBusWait);

    sensorL.ping();
    usleep(i2cBusWait * sensorWait);
    distL = sensorL.readS();
    usleep(i2cBusWait);

    while(distL < optDistWall)
    {
      sensorL.ping();
      usleep(i2cBusWait * sensorWait);
      distL = sensorL.readS();
      usleep(i2cBusWait);
    }

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

    usleep(i2cBusWait * moveDist);
/*
    for(i = 0;i < moveDist; i++)
      usleep(10000);
*/
    go.stop();
    usleep(i2cBusWait);
    turn.center();
    usleep(i2cBusWait);

    usleep(60000);

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

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

    usleep(60000);

    sensorL.ping();
    usleep(i2cBusWait * sensorWait);
    distL = sensorL.readS();
    usleep(i2cBusWait);
//  }

  return 0;
}