/*!

  ADAPTED FROM: 
  \example calculate_xy.c Calculates X-Y coordinates

  Having the X axis aligned to the front step of the sensor, calculates the coordinates for measurement data
  \author Satofumi KAMIMURA

  $Id: calculate_xy.c,v e5d1719877a2 2015/05/07 04:12:14 jun $
  
  By Fernando Osorio - June 2015 (FOsorio)
*/
    
#include "urg_sensor.h"
#include "urg_utils.h"
#include "open_urg_sensor.h"
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <sys/ioctl.h>

int fd, n, i;
char buf[64] = "temp text";
struct termios toptions;

int main(int argc, char *argv[])
{
    urg_t urg;
    char texto[100];
    long *data;
    long max_distance;
    long min_distance;
    long time_stamp;
    int i,cnt;
    int n;
    int dmenor, dmx,dmy, angulo;
    long admenor;
    double amenor;

    if (open_urg_sensor(&urg, argc, argv) < 0) {
        return 1;
    }

    data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
    if (!data) {
        perror("urg_max_index()");
        return 1;
    }

    urg_start_measurement(&urg, URG_DISTANCE, 1000, 0);

cnt=0;

//LOOP
while (1)
{
    // Gets measurement data
    n = urg_get_distance(&urg, data, &time_stamp);
    if (n < 0) {
        printf("urg_get_distance: %s\n", urg_error(&urg));
        urg_close(&urg);
        return 1;
    }

    // Outputs X-Y coordinates
    urg_distance_min_max(&urg, &min_distance, &max_distance);
    dmenor=10;
    dmx=10000;
    admenor=10000;
    for (i = 0; i < n; ++i) {
        long distance = data[i];
        double radian;
        long x;
        long y;

        if ((distance < min_distance) || (distance > max_distance)) {
            continue;
        }

        radian = urg_index2rad(&urg, i);
        x = (long)(distance * cos(radian));
        y = (long)(distance * sin(radian));
     
        if ((radian < -M_PI) || (radian > M_PI))
           continue;
 
        if (admenor > distance)
        {
            admenor=distance;
            amenor=radian;
        }
        // printf("%ld %ld %lf %ld\n", x, y, radian*(180.0/M_PI),distance);
        // fprintf(fout,"%ld %ld\n", x, y);
    }
    angulo=(int)(amenor*(180.0/M_PI));
    printf("MIN(ang,dist): %d %ld ",angulo,admenor);

    cnt++;
    if (cnt >= 3)
    {    
       sprintf(texto,"python pwm3.py %d &",angulo);
       printf(" [%d] ",angulo); 
       system(texto);
       cnt=0;
    }
    printf("\n");
    //system("sleep 3");

}
//END LOOP

     printf("\n");

    // Disconnects
    free(data);
    urg_close(&urg);

#if defined(URG_MSC)
    getchar();
#endif
    return 0;
}