/*
  testSensorRead.cpp

  reads sensor and displays value in inches, centimeters, or microseconds
*/

#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Srf10.h"

int main()
{
  int i2cHandle;

  if((i2cHandle = open("/dev/i2c-0", O_RDWR)) < 0)
  {
    cout << "Open Failed" << endl;
    exit(1);
  }
  
/*
  There are 16 addresses available to the srf10 range sensor (0-15).  The
  changeAddress method will take a number from 0 - 15 and perform the
  necessary commands to change the address of that sensor.
*/
  int i2cAddr;
  char resultType;  // i = inches, c = centimeters, s = microseconds
  
  cout << "Enter sensor address (0 - 15): ";
  cin >> i2cAddr;
  i2cAddr += 0x70;

  cout << "Enter result type (i, c, s): ";
  cin >> resultType;

  if(i2cAddr >= 0x70 && i2cAddr <= 0x7F)
    switch(resultType)
    {
      case 'i':
      case 'I':
      case 'c':
      case 'C':
      case 's':
      case 'S':
        break;
      default:
        cout << "Invalid result type" << endl;
        exit(1);
    }
  else
  {
    cout << "Invalid i2c address (0 - 15)" << endl;
    exit(1);
  }

  Srf10 sensor(i2cHandle, i2cAddr, resultType);

  sensor.setGain(9);
  usleep(1000);
  sensor.setRange(47);
  usleep(1000);

  while(1)
  {
    sensor.ping();
    usleep(65000);

    cout << "Distance: " << sensor.readS();
    switch(resultType)
    {
      case 'i':
      case 'I':
        cout << " inches" << endl;
        break;
      case 'c':
      case 'C':
        cout << " centimeters" << endl;
        break;
      case 's':
      case 'S':
        cout << " microseconds" << endl;
        break;
    }
    usleep(1000);
  }

  return 0;
}