#include <iostream>
using namespace std;

#include <stdlib.h>

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

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, 2);
//  Servo turn(i2cHandle, 1100, 1900, 3);
//  Srf10 sensor2(i2cHandle, 0x72, 'i');

  int inc, slp, n;
  char direction;

  cout << "Which direction do you want to test(f, b)?" << endl;
  cin >> direction;

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

//turn.right(50);

  while(1)
  {
    cout << "Enter increase amount, zero to stop: " << endl;
    cin >> inc;
    cout << "Enter amount to sleep in seconds: (1 or 2)" << endl;
    cin >> slp;
    cout << "How many times to loop: " << endl;
    cin >> n;

  if(direction == 'f')
    go.forward(inc);
  else
    go.reverse(inc);
//  go.reverse(inc);
  for(int i=0; i < n; i++){
//    go.reverse(inc);
    sleep(slp);
//    go.stop();
//    sleep(slp);
  }
  go.stop();
    
  }
  return 0;
}