#include <iostream>

using namespace std;

#include "Motor.h"
#include "Servo.h"
#include "RSnsr.h"

int centerWheels = 50;
int stopDist = 50;
int i2cBusWait = 1000;
int sensorWait = 65000;

int minReverse = 20;  // speed for backup movement
int minForward = 20;  // speed for forward movement

void process(RSnsr &sensorC, RSnsr &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, 4);
  usleep(i2cBusWait);
  Servo turn(i2cHandle, 1100, 1894, 3); // steering servo
  usleep(i2cBusWait);
  RSnsr sensorC(i2cHandle, 0x72, 'c'); // center sensor
  usleep(i2cBusWait);
  RSnsr sensorL(i2cHandle, 0x70, 'c'); // left sensor
  usleep(i2cBusWait);

//  sensorL.setGain(8);
//  usleep(i2cBusWait);
//  sensorL.setRange(47);
//  usleep(i2cBusWait);
//  sensorC.setGain(8);
//  usleep(i2cBusWait);
//  sensorC.setRange(47);
//  usleep(i2cBusWait);

  int distC;
  int distL;
  enum {backwd, stopped, forwd} direction;


  while(1)
  {
    sensorC.ping();
    usleep(sensorWait);
    sensorL.ping();
    usleep(sensorWait);
    distC = sensorC.mReadS();
    usleep(i2cBusWait);
    distL = sensorL.mReadS();
    usleep(i2cBusWait);  

cout << "DistL: " << distL << "  DistC: " << distC << endl;

    if(distC > stopDist * 2 + 10 || distC < 10)
    {
      if(direction != stopped)
      {
        go.stop();
        usleep(i2cBusWait);
        direction = stopped;
      }
    }
    else
    {
      if(distC > stopDist + 15)
      {
        if(direction != forwd)
        {
          go.forward(minForward);
          usleep(i2cBusWait);
          direction = forwd;
        }
      }
      else
      {
        if(distC < stopDist +5)
        {
          if(direction != backwd)
          {
            go.reverse(minReverse);
            usleep(i2cBusWait);
            direction = backwd;
          }
        }
        else
        {
          if(direction != stopped)
          {
            go.stop();
            usleep(i2cBusWait);
            direction = stopped;
          }
        }
      }
    }


    if(distL > centerWheels * 2 + 10 || distL < 10)
    {
      turn.center();
      usleep(i2cBusWait);
    }
    else
    {
      if(distL > centerWheels)
      {
        turn.left((distL - centerWheels + 10) * 100 / centerWheels);
        usleep(i2cBusWait);
      }
      else
      {
        if(distL < centerWheels)
        {
          turn.right((centerWheels + 10 - distL) * 100 / centerWheels);
          usleep(i2cBusWait);        
        }
        else
        {
          turn.center();
          usleep(i2cBusWait);
        }
      }
    }
  }

  return 0;
}