#include <iostream>
using namespace std;

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

int main()
{
  int i2cHandle;
  int dist1, dist2, dist3;
  float value, deg;

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

  Cmps03 compass(i2cHandle);
//  Srf10 sensor1(i2cHandle, 0x71, 'i');
//  Srf10 sensor2(i2cHandle, 0x72, 'i');
//  Srf10 sensor3(i2cHandle, 0x73, 'i');

//  Servo turn(i2cHandle, 1100, 1900, 4);

  while(1)
  {
//    ioctl(i2cHandle, I2C_SLAVE, 0x60);

//    write(i2cHandle, readBuf, 2);

//    usleep(65000);

//    write(i2cHandle, &readReg, 1);
//    read(i2cHandle, readBuf, 2);

//    printf("%d inches\n", readBuf[0]*256 + readBuf[1]);

//    ioctl(i2cHandle, I2C_SLAVE, 0x60);

//    write(i2cHandle, &readReg, 1);
//    read(i2cHandle, readBuf, 2);

//    value = (readBuf[0]*256 + readBuf[1])/10;

//    printf("%.1f deg\n", value);
//  float value;

//    sensor2.ping();
//    usleep(65000);
//    dist2 = sensor2.readS();

//    deg = compass.readC();

//    cout << "Distance: " << dist2 << endl;
//    cout << "Degree of Cars position: " << deg << endl;
//    usleep(65000);
/*
    sensor1.ping();
//    sensor3.ping();
    usleep(65000);
    sensor2.ping();
    usleep(65000);

    dist1 = sensor1.readS();
    dist2 = sensor2.readS();
//    dist3 = sensor3.readS();

    if(dist1 < 10)
      turn.right(50);
//    else if(dist3 < 10)
//      turn.left(50);
    else if(dist2 < 10)
      turn.center();
*/

//    cout << "Distance: " << distance << endl;
  }

  return 0;
}