/*
  testRightCorner.cpp

  move forward to wall and turns to the right
*/

#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 turnDist = 100; // distance in centimeters to start turn
int extreme = 75;
int i2cBusWait = 1000;
int pingWait = 30000;

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;
  int distC;

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

  sensorC.ping();
  usleep(pingWait);
  distC = sensorC.readS();
  usleep(i2cBusWait);

  turn.center();
  usleep(i2cBusWait);
  go.forward(minForward);
  usleep(i2cBusWait);

  if(distC <= turnDist)
  {
    turn.right(extreme);
    usleep(i2cBusWait);
    go.forward(minForward);
    usleep(i2cBusWait);

    sensorC.ping();
    usleep(pingWait);
    distC = sensorC.readS();
    usleep(i2cBusWait);

    while(distC < optDistWall)
    {
      sensorC.ping();
      usleep(pingWait);
      distC = sensorC.readS();
      usleep(i2cBusWait);
cout << "distance center: " << distC << endl;
    }
  }
  go.stop();
//  }

  return 0;
}