#include <iostream>
using namespace std;

#include <stdlib.h>

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

int minDistWall = 40; // value is in cm = 5 inches
int maxDistWall = 80; // value is in cm = 10 inches
int optDistWall = (maxDistWall + minDistWall)/2; // value between min and max distance from wall
int stopDist = 10; // value is in cm = 10 inches
int perChange = 2;
int extreme;
int extremeFlag = 0;
int extremeCtValue;
int extremeCt;
int stopSw;

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

void process(Srf10 &sensorC, Srf10 &sensorL, Servo &turn, Motor &go);
void move(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, 1900, 4, 3);
  Servo turn(i2cHandle, 1100, 1900, 3); // steering servo
  Srf10 sensorC(i2cHandle, 0x72, 'c'); // center sensor
  Srf10 sensorL(i2cHandle, 0x70, 'c'); // left sensor

  int n;

//  cout << "extreme turn: ";
//  cin >> extreme;
//  cout << "extreme count value: ";
//  cin >> extremeCtValue;

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

  stopSw = 0;
  go.forward(minForward);
  usleep(1000);
  for(int i = 0; i < n; i++)
    process(sensorC, sensorL, turn, go);

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

  return 0;
}

void process(Srf10 &sensorC, Srf10 &sensorL, Servo &turn, Motor &go)
{
  int distC;
  int distL;

  sensorC.ping();
  usleep(10000);
  sensorL.ping();
  usleep(20000);
  distC = sensorC.readS();
  distL = sensorL.readS();
cout << "Distance in cm: " << distL << endl;
  if(distC > stopDist)
  {
    if(stopSw)
    {
      go.forward(minForward);
      stopSw = 0;
    }
/*    if(distL < minDistWall)
    {
      turn.right(extreme);
      extremeFlag = 1;
cout << "extreme right" << endl;
      extremeCt = extremeCtValue;
    }
    else
      if(distL > maxDistWall)
      {
        turn.left(extreme);
cout << "extreme left" << endl;
        extremeFlag = 2;
        extremeCt = extremeCtValue;
      }
    else
      if(extremeFlag)
      {
cout << "extremeFlag set" << endl;
        if(--extremeCt)
        {
          if(extremeFlag == 1)
            turn.left(extreme);
          else
            if(extremeFlag == 2)
              turn.right(extreme);
        }
        else
        {
          extremeFlag = 0;
cout << "extreme reset" << endl;
        }
      }
    else
*/    if(distL < optDistWall)
      {
        turn.right((optDistWall - distL) * 2);
        usleep(1000);
cout << "opt right: " << (optDistWall - distL) * 2<< endl;
      }
    else
      if(distL > optDistWall)
      {
        turn.left((distL - optDistWall) * 2);
        usleep(1000);
cout << "opt left: " << (distL - optDistWall) * 2 << endl;
      }
      else
      {
        turn.center();
        usleep(1000);
cout << "center" << endl;
      }
  }
  else
  {
    go.stop();
    usleep(1000);
    turn.center();
    stopSw = 1;
  }

}