/*
  testMotorFwRv

  moves vehicle forward or backward for a specified count
  will stop if it approaches an object.
*/

#include <iostream>
using namespace std;

#include <stdlib.h>

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

const int minForward = 12;
const int minReverse = 20;

int main()
{
  int i2cHandle;

  if((i2cHandle = open("/dev/i2c-0", O_RDWR)) < 0)
  {
    cout << "Open Failed" << endl;
    exit(1);
  }

  Motor go(i2cHandle, 1100, 1900, 4, 3);
  Servo turn(i2cHandle, 1100, 1894, 3); // center wheels
  Srf10 sensorL(i2cHandle, 0x70, 'c'); // left sensor
  Srf10 sensorC(i2cHandle, 0x72, 'c'); // center sensor
  Srf10 sensorR(i2cHandle, 0x73, 'c'); // right sensor

  int preDist = 0, curDist;
  int loopCt, i;
  char direction;

  cout << "Enter number times to loop: ";
  cin >> loopCt;
  cout << "Which direction (f, r): ";
  cin >> direction;

  if(direction == 'f')
    go.forward(minForward);
  else
    go.reverse(minReverse);

  for(i = 0; i < loopCt; i++)
  {
    sensorC.ping();
    usleep(65000);
    sensorL.ping();
    usleep(65000);
    sensorR.ping();
    usleep(65000);
    curDist = sensorC.readS();
    usleep(1000);
    
cout << "previous: " << preDist << " current: " << curDist << endl;

cout << "left: " << sensorL.readS() << endl;
    usleep(1000);
cout << "right: " << sensorR.readS() << endl;
    usleep(1000);    
    if(curDist < preDist)
    {
      cout << "Stopping with current distance: " << curDist << endl;
      cout << "Loop count is at " << i << endl;
      break;
    }
    preDist = curDist;
  }

  go.stop();

  return 0;
}