#include <iostream>
using namespace std;

#include <stdlib.h>

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

int minDistWall = 50; // value is in cm = 5 inches
int maxDistWall = 70; // value is in cm = 10 inches
int optDistWall = (maxDistWall + minDistWall)/2; // value between min and max distance from wall
int stopDist = 25; // value is in cm = 10 inches
int perChange = 2;
int extreme;
int Flag = 0;

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

int runTime = 5000;  // used for sustained movement
int coastTime = 2000;  // used for coasting. decreased speed

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 << "Optimal distance: " << optDistWall << endl;
  cout << "How many time to loop: ";
  cin >> n;
  cout << "extreme turn: ";
  cin >> extreme;

  go.forward(minForward);
  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();
//  move(go);
  usleep(10000);
//  move(go);
  sensorL.ping();
  usleep(20000);
//  move(go);
//  move(go);
  distC = sensorC.readS();
  distL = sensorL.readS();
cout << "Distance in cm: " << distL << endl;
  if(distC > stopDist)
  {
    if(distL < minDistWall)
    {
      turn.right(extreme);
      Flag = 1;
    }
    else
      if(distL > maxDistWall)
      {
        turn.left(extreme);
        Flag =1;
      }
    else
      if(distL > minDistWall && distL < optDistWall && Flag != 1)
      {
        turn.right((optDistWall-distL)*perChange);
      }
    else
      if(distL < maxDistWall && distL > optDistWall && Flag != 1)
      {
        turn.left((distL-optDistWall)*perChange);
      }
    else
      if(distL > minDistWall && distL < ((optDistWall + minDistWall)/2))
      {
        turn.right((((optDistWall + minDistWall)/2)-distL)*perChange);
      }
    else
      if(distL < maxDistWall && distL > ((maxDistWall + optDistWall)/2))
      {
        turn.left((distL-((maxDistWall + optDistWall)/2))*perChange);
      }
    else
      if(distL >= ((optDistWall + minDistWall)/2))
      {
        if(distL < optDistWall)
          turn.left((optDistWall-distL)*perChange);
        else
        {
          turn.center();
          Flag = 0;
        }
          
      }
    else
      if(distL <= ((maxDistWall + optDistWall)/2))
      {
        if(distL > optDistWall)
          turn.right((distL-optDistWall)*perChange);
        else
        {
          turn.center();
          Flag = 0;
        }
      }
    else
      turn.center();
  }

}

void move(Motor &go)
{
  go.forward(minForward);
//  usleep(runTime);
//  go.forward(minForward - 2);
//  usleep(coastTime);
}