#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

#include "abdrive.h"
#include "simpletext.h"
#include "simpletools.h"
#include "ping.h"
#include "functions.h"

int main(int argc, const char* argv[])
{

  int path[500];
  for(int j = 0; j < 10; j++)
  {
    path[ j ] = 0;
  }

  int index = 0;
  int temp = 0;


  drive_speed(30,30);

  while(ping_cm(8) > 5)
    {
      int front = ping_cm(8);

      int irLeft = 0;
      for(int dacVal = 0; dacVal < 160; dacVal += 2)
      {
        dac_ctr(26, 0, dacVal);
        freqout(11, 1, 38000);
        irLeft += input(10);
      }


      int irRight = 0;
      for(int dacVal = 0; dacVal < 160; dacVal += 2)
      {
        dac_ctr(27, 1, dacVal);
        freqout(1, 1, 38000);
        irRight += input(2);
      }

      int indicator = 0;
      // get parameters --------------------
      indicator = temp - irLeft;


      if(indicator >= 0 && indicator <= 3){
        drive_speed(40,30);
        path[index] = 1;
      }
      if(indicator > 3 && indicator <= 5){
        drive_speed(50,30);
        path[index] = 2;
      }
      if(indicator < 0 && indicator >= -3){
        drive_speed(30,40);
        path[index] = 3;
      }
      if(indicator < -3 && indicator >= -5){
        drive_speed(30,50);
        path[index] = 4;
      }


      temp = irLeft;
      printf("%d  %d  \n", irRight, irLeft);

      index++;
    }


  drive_speed(0, 0);
  drive_goto(-10,-10);
  degreesTurn(180);

  path[index] = 0;

  for(int i = index; i >= 0; i--){
    //stored[i] = path[index - i];
    //printf("%d\n", stored[i]);
    reversePath(path[i]);
    printf("%d\n", path[i]);
  }

  drive_speed(0, 0);



  return 0;
}