#include <stdio.h>
#include "poslib.h"
#include <time.h>

#define TIME_DELAY_LASER 2.0
#define TIME_DELAY_ATGOAL 60.0

float goalx[20];
float goaly[20];


void check_goals(void) {
  if (call_newgoal() == 1) {
    fprintf(stdout, "call_newgoal response is:1.\n");
    numgoals = call_howmanygoals();
    fprintf(stdout, "There are %d goals waiting.\n", numgoals);
    for (i=0; numgoals>=i; i++) {
      goalPos = call_getgoal(i);
      fprintf(stdout, "call_getgoal response is x=%.0f, y=%.0f.\n", goalPos.x, goalPos.y);
    }
  }
}


void remove_nearest_goal(float currentx, float currenty) {
   int i, j;
   int nearestx, nearesty;
   nearestx = 0;
   float newgoalx[20];
   float newgoaly[20];
   for (i=0; 20>i; i++) {
     if ((abs(goalx[i] - currentx) < nearestx) && (goalx[i] != 0)) {
       nearestx = i;
     }
   }
   for (j=0; 20>j; j++) {
     if ((abs(goaly[j] - currenty) < nearesty) && (goaly[i] != 0)) {
       nearesty = j;
     }
   }
   if (i == j) {
     /* Ok, this one is easy */
     /* Create an array with all of the elements except this one in it */
     if (i == 0 ) {
       for (int a=0; 19>=a; a++) {
         newgoalx[a] = goalx[a+1];
         newgoaly[a] = goaly[a+1];
       }
     } 
     else {
       if (i == 19) {
         for (int a=0; 18>=a; a++) {
           newgoalx[a] = goalx[a];
           newgoaly[a] = goaly[a];
         }
       }
       else {
         for (b=0; i>b; b++) {
           newgoalx[b] = goalx[b];
           newgoaly[b] = goaly[b];
         }
         for (b=(i+1); 19>b; b++) {
           newgoalx[b-1] = goalx[b];
           newgoaly[b-1] = goaly[b];
         }
       }
     }
   }
   else {
     fprintf(stderr, "Hey dude, this one ain't easy!\n");
     fprintf(stderr, "Least x=%d, y=%d\n", i, j);
   }
}



int main(void) {
   goalPosition goalPos;
   int numgoals;
   int i;
   float ltime_difference;
   float gtime_difference;
   struct timeval this_time;
   static struct timeval last_time_laser  = {0, 0};
   static struct timeval last_time_atgoal  = {0, 0};

   fprintf(stdout, "Starting...\n");
   initperl();
   poslibinit();
   
   while (1) {
     gettimeofday(&this_time, NULL);
     ltime_difference = 
       ((float) (this_time.tv_sec - last_time_laser.tv_sec))
        + (((float) (this_time.tv_usec - last_time_laser.tv_usec))
         /  1000000.0);
     gtime_difference =
       ((float) (this_time.tv_sec - last_time_atgoal.tv_sec))
        + (((float) (this_time.tv_usec - last_time_atgoal.tv_usec))
         /  1000000.0);

     if (ltime_difference >= TIME_DELAY_LASER) {
       check_goals();
       last_time_laser.tv_sec  = this_time.tv_sec;
       last_time_laser.tv_usec = this_time.tv_usec;
     }
     if (gtime_difference >= TIME_DELAY_ATGOAL) {
       last_time_atgoal.tv_sec  = this_time.tv_sec;
       last_time_atgoal.tv_usec = this_time.tv_usec;
     }
   
   }
     perlclose();
}