#include <iostream>

using namespace std;

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

int optDistWall = 60; // value is in cm, optimal distance from wall
int minDistWall = optDistWall * 3 / 4; // value is in cm, optimal - 1/4
int maxDistWall = optDistWall * 5 / 4; // value is in cm, optimal + 1/4
int stopDist = 25; // value is in cm = 10 inches
int extreme = 75;
int moveDist = 40;
int i2cBusWait = 1000;
int sensorWait = 30;

int minReverse = 18;  // speed for backup movement
int minForward = 16;  // 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, 3);
  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(9);
//  usleep(i2cBusWait);
//  sensorL.setRange(47);
//  usleep(i2cBusWait);
  sensorC.setGain(9);
  usleep(i2cBusWait);
  sensorC.setRange(47);
  usleep(i2cBusWait);

  int i;
  int n;

  cout << "How many times to loop: ";
  cin >> n;

  go.forward(minForward);
  usleep(i2cBusWait);
  for(i = 0;i < n; i++)
  {
    process(sensorC, sensorL, turn, go);
  }

  turn.center();
  usleep(i2cBusWait);
  go.stop();
  usleep(i2cBusWait);

  return 0;
}

void process(RSnsr &sensorC, RSnsr &sensorL, Servo &turn, Motor &go)
{
  static int preDist = 0;
  static int extremeFlag = 0;

  int distC;
  int distL;

  sensorC.ping();
  usleep(10000);
  sensorL.ping();
  usleep(20000);
  distC = sensorC.mReadS();
  usleep(i2cBusWait);
  distL = sensorL.mReadS();
  usleep(i2cBusWait);

  if(distC > stopDist)
  {
    cout << "distL: " << distL << endl;
    if(distL < minDistWall)
    {
      cout << "< minDistWall ";
      if(distL > preDist)
      {
        cout << "center" << endl;
        turn.center();
        usleep(i2cBusWait);
      }
      else
      {
        cout << "extreme right" << endl;
        turn.right(extreme);
        usleep(i2cBusWait);
        extremeFlag = 1;
      }
    }
    else
    {
      if(distL > maxDistWall)
      {
        cout << "> maxDistWall ";
        if(distL < preDist)
        {
          cout << "center" << endl;
          turn.center();
          usleep(i2cBusWait);
        }
        else
        {
          cout << "extreme left" << endl;
          turn.left(extreme);
          usleep(i2cBusWait);
          extremeFlag = 1;
        }
      }
      else
      {
        if(distL > optDistWall)
        {
          cout << "> optDistWall ";
          if(distL < preDist)
          {
            if(distL > optDistWall + 10 || !extremeFlag)
            {
              cout << "center" << endl;
              turn.center();
              usleep(i2cBusWait);
            }
            else
            {
              cout << "almost right" << endl;
              turn.right(extreme);
              usleep(i2cBusWait);
              extremeFlag = 0;
            }
          }
          else
          {
            cout << "turn left" << endl;
            turn.left((distL - optDistWall) * 2);
            usleep(i2cBusWait);
          }
        }
        else
        {
          if(distL < optDistWall)
          {
            cout << "< optDistWall ";
            if(distL > preDist)
            {
              if(distL < optDistWall - 10 || !extremeFlag)
              {
                cout << "center" << endl;
                turn.center();
                usleep(i2cBusWait);
              }
              else
              {
                cout << "almost left" << endl;
                turn.left(extreme);
                usleep(i2cBusWait);
                extremeFlag = 0;
              }
            }
            else
            {
              cout << "turn right" << endl;
              turn.right((optDistWall - distL) * 2);
              usleep(i2cBusWait);
            }
          }
          else
          {
            cout << "opt center" << endl;
            turn.center();
            usleep(i2cBusWait);
          }
        }
      }
      preDist = distL;
    }
  }
  else
  {
    go.stop();
    usleep(i2cBusWait);
    cout << "stopping ..." << endl;
  }
}