/* gcc -Wall -std=gnu11 -O2 -fno-aggressive-loop-optimizations -pedantic -lportaudio -lm *.c -o sonar */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "sonar.h"


int main ( int argc, char **argv )
{
    int opt;
    int sensor_id[3] = {-1, -1, -1};
    int verbose=0;
    
    sonar_config conf;
    conf.ds = 0;
    
    while ( (opt = getopt(argc, argv, "t:l:r:d:vs")) != -1 )
    {
        switch (opt)
        {
            /* specify sensor */
            case 't':
            case 'l':
            case 'r':
            	/* not clever, just lazy */
                if ( 1 != sscanf(optarg, "%d", &sensor_id[(opt == 't') ? 0 : (opt == 'l') ? 1 : 2]) ) 
                {
                    perror("Failed to parse -<t|l|r> argument");
                    exit(-1);
                }
                break;
            case 'd': /* specify ds (distance between sensors) */
                conf.ds = strtod(optarg, NULL);
                if ( conf.ds > 343 )
                {
                    fprintf ( stderr, "Specified sensor distance is too large. Must be < 343\n" );
                    exit(-1);
                }
                if (conf.ds <  0.1)
                {
                    fprintf ( stderr, "Specified sensor distance is too small. Must be > 0.1\n" );
                    exit(-1);
                }
                break;
            case 's': /* list audio input devices */
                sonar_init();
                sonar_listdev();
                exit(0);
                break;
            case 'v':
                verbose = 1;
                break;
            default:
                //fprintf ( stderr, "Usage: %s -l | -s <sensor1> -s <sensor2> -s <sensor3> -d <distance in meters> \n", argv[0] );
                exit(-1);
        }
    }
    
    /* done processing arguments, make sure arguments make sense */
    if ( -1 == sensor_id[0] || -1 == sensor_id[1] || -1 == sensor_id[2] )
    {
        fprintf ( stderr, "Error: 3 sensors, -t (top) -l (left) and -r (right) must be specified.\n" );
        exit(-1);
    }
    
    if ( conf.ds == 0 )
    {
        fprintf ( stderr, "Error: Sensor distance (-d) must be set.\n" );
        exit(-1);
    }
    
    if ( sensor_id[0] == sensor_id[1] || sensor_id[0] == sensor_id[2] || sensor_id[1] == sensor_id[2] )
    {
        fprintf ( stderr, "Error: Each sensor (-t,-l,-r) must be unique.\n" );
        exit(-1);
    }
    
    if ( verbose )
    {
        printf ( "Sensors: %d, %d, and %d\n"
                 "Distance between sensors: %fm",
                 sensor_id[0], sensor_id[1], sensor_id[2],
                 conf.ds
               );
    }
    
    /* init audio hardware */
    sonar_init();
/*
    sonar_openstreams(sensor_id);
    
    sonar_
*/    
    coord loc;
    for (;;)
    {
        sonar_getloc(&loc, conf);
        sleep(1);
    }
    return 0;
}