#include <iostream>

using namespace std;

#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 = 25; // value is in cm = 10 inches
int extreme = 35;

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

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

  int i;
  int n;
  
  cout << "How many times to loop: ";
  cin >> n;
  
  go.forward(minForward);
  usleep(1000);
  for(i = 0;i < n; i++)
  {
    process(sensorC, sensorL, turn, go);
  }
  
  turn.center();
  usleep(1000);
  go.stop();
  usleep(1000);

  return 0;
}

void process(Srf10 &sensorC, Srf10 &sensorL, Servo &turn, Motor &go)
{
  static int readCt = 0;
  static int distTbl[3];
  static int preDist = 0;
  static int extremeFlag = 0;

  int distC;
  int distL;
  int curDist;
  
  sensorC.ping();
  usleep(10000);
  sensorL.ping();
  usleep(20000);
  distC =sensorC.readS();
  usleep(1000);
  distL = sensorL.readS();
  usleep(1000);
  
//  cout << "readC: ";
//  cin >> distL;

  if(distC < stopDist)
  {    
    switch(++readCt)
    {
      case 1:
        distTbl[1] = distL;
        break;
      case 2:
        if(distL >= distTbl[1])
        {
          distTbl[2] = distL;
          distTbl[0] = 0;
        }
        else
        {
          distTbl[0] = distL;
          distTbl[2] = 0;
        }
        break;
      case 3:
        readCt = 0;
        if(distL >= distTbl[1])
        {
          if(distTbl[2])
          {
            if(distL >= distTbl[2])
              curDist = distTbl[2];
            else
              curDist = distL;
          }
          else
            curDist = distTbl[1];
        }
        else
        {
          if(distTbl[0])
          {
            if(distL >= distTbl[0])
              curDist = distL;
            else
              curDist = distTbl[0];
          }
          else
            curDist = distTbl[1];
        }
        break;
    }
    if(!readCt)
    {
      cout << "curDist: " << curDist << endl;
      if(curDist < minDistWall)
      {
        cout << "< minDistWall ";
        if(curDist > preDist)
        {
          cout << "center" << endl;
          turn.center();
          usleep(1000);
        }
        else
        {
          cout << "extreme right" << endl;
          turn.right(extreme);
          extremeFlag = 1;
          usleep(1000);
        }
      }
      else
      {
        if(curDist > maxDistWall)
        {
          cout << "> maxDistWall ";
          if(curDist < preDist)
          {
            cout << "center" << endl;
            turn.center();
            usleep(1000);
          }
          else
          {
            cout << "extreme left" << endl;
            turn.left(extreme);
            extremeFlag = 1;
            usleep(1000);
          }
        }
        else
        {
          if(curDist > optDistWall)
          {
            cout << "> optDistWall ";
            if(curDist < preDist)
            {
              if(curDist > optDistWall + 10 || !extremeFlag)
              {
                cout << "center" << endl;
                turn.center();
                usleep(1000);
              }
              else
              {
                cout << "almost right" << endl;
                turn.right(extreme);
                extremeFlag = 0;
                usleep(1000);
              }
            }
            else
            {
              cout << "turn left" << endl;
              turn.left((curDist - optDistWall) * 2);
              usleep(1000);
            }
          }
          else
          {
            if(curDist < optDistWall)
            {
              cout << "< optDistWall ";
              if(curDist > preDist)
              {
                if(curDist < optDistWall - 10 || !extremeFlag)
                {
                  cout << "center" << endl;
                  turn.center();
                  usleep(1000);
                }
                else
                {
                  cout << "almost left" << endl;
                  turn.left(extreme);
                  extremeFlag = 0;
                  usleep(1000);
                }
              }
              else
              {
                cout << "turn right" << endl;
                turn.right((optDistWall - curDist) * 2);
                usleep(1000);
              }
            }
            else
            {
              cout << "opt center" << endl;
              turn.center();
              usleep(1000);
            }
          }
        }
      }
      preDist = curDist;
    }
  }
  else
    go.stop();
}