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

#include "sonar.h"

void sonar_init (void)
{
	/* init portaudio */
	PaError err;
	if ( (err = Pa_Initialize()) )
	{
		printf ( "Failed to init: %s\n", Pa_GetErrorText ( err ) );
		exit ( -1 );
	}

	/* uh.. sure */
	atexit((void (*)(void))Pa_Terminate);

}

void sonar_set_midpoints (coord *mps, double l)
{
    /* determine the midpoints of a triangle of length l.
     * This assumes the triangle is equilateral and centered
     * on the origin point.
     */

    double h_bigt; /* height of big triangle */
    double r_bigt; /* radius of big triangle */

    /* small triangle = triangle formed by midpoints of big triangle */

    double r_smallt; /* radius of small triangle */
    double l_smallt; /* length of side of small t */
    double m; /*  gives midpoint y coord. */

    /* use pythagoras to determine height of equilateral triangle */
    h_bigt = sqrt( pow(l, 2) - pow( l/2, 2) );

    /* radius of an equilateral triangle = length of side / sqrt(3) */
    r_bigt = l / sqrt(3);

    /* the difference in the height of bigt and its radius gives radius of smallt */
    r_smallt = h_bigt - r_bigt;

    l_smallt = sqrt(3) * r_smallt;

    /* use pythagoras again */
    m = sqrt( pow(r_smallt, 2) - pow( l_smallt / 2, 2) );

    mps[TOPLEFT].x = -l_smallt / 2;
    mps[TOPLEFT].y = m;

    mps[RIGHTTOP].x = l_smallt / 2;
    mps[RIGHTTOP].y = m;

    mps[LEFTRIGHT].x = 0;
    mps[LEFTRIGHT].y = -r_smallt;
}

void sonar_listdev (void)
{
	/* Figure out input devices we have */
	PaDeviceIndex dev;
	const PaDeviceInfo *dev_info;
	for ( dev = 0; dev < Pa_GetDeviceCount(); dev++ )
	{
		dev_info = Pa_GetDeviceInfo ( dev );
		printf (
			"Index: %d\n"
			"Name: %s\n"
			"Input Channels: %d\n"
			"Sample Rate: %f\n"
			"Default Low Input Latency: %f\n"
			"\n",
			dev, dev_info->name,
			dev_info->maxInputChannels,
			dev_info->defaultSampleRate,
			dev_info->defaultLowInputLatency );
	}
}

int sonar_coord_cmp (coord a, coord b)
{
	return (a.x == b.x && a.y == b.y);
}

void sonar_getloc ( coord *loc, sonar_config conf )
{
	unsigned int i;
	PaError err;
	coord midpoint[3];
	coord mp_order[3];
	coord mp_map[3][3];

	sonar_set_midpoints (midpoint, conf.ds);

	mp_map[LEFT][RIGHT] = midpoint[LEFTRIGHT];
	mp_map[RIGHT][LEFT] = midpoint[LEFTRIGHT];

	mp_map[TOP][LEFT] = midpoint[TOPLEFT];
	mp_map[LEFT][TOP] = midpoint[TOPLEFT];

	mp_map[RIGHT][TOP] = midpoint[RIGHTTOP];
	mp_map[TOP][RIGHT] = midpoint[RIGHTTOP];

	PaStreamParameters sparams[2];

	for ( i = 0; i < 3; i++ )
	{
		sparams[i].device = conf.sensor_id[i];
		sparams[i].channelCount = 1;
		sparams[i].sampleFormat = paInt16;
		sparams[i].suggestedLatency = 0;
		sparams[i].hostApiSpecificStreamInfo = NULL;
	}

	for ( i = 0; i < 3; i++ )
		if ( (err = Pa_OpenStream (
				&conf.sensor_stream[i],
				&sparams[i],
				NULL,
				44100,
				paFramesPerBufferUnspecified,
				paNoFlag,
				NULL,
				NULL )) )
		{
			printf ( "Failed to open stream for device %d: %s\n", conf.sensor_id[i], Pa_GetErrorText ( err ) );
			exit ( -2 );
		};
	
	struct timespec t[3];
	double theta[3];
	double b[3];
	int t_to_m[3];
	int order = 0;
	
	for ( i = 0; i < 3; i++ )
		t[i].tv_sec = 0;

	for ( i = 0; i < 3; i++ )
		Pa_StartStream ( conf.sensor_stream[i] );
	
	for (;;)
	{
		for ( i = 0; i < 3; i++ )
		{
			err = Pa_ReadStream ( conf.sensor_stream[i], &conf.sensor_data[i], 1 );
			if (! ( paInputOverflowed == err || err == paNoError ) )
			{
				fprintf ( stderr, "Failed to read stream: %s\n", Pa_GetErrorText ( err ) );
				exit ( -3 );
			}
		}
		
		for ( i = 0; i <= 2; i++ )
		{
			if ( abs( conf.sensor_data[i] ) > conf.threshold && t[i].tv_sec != 0 )
			{
				clock_gettime ( CLOCK_REALTIME, &t[i] );
				t_to_m[order++] = i;
			}
		}
		
		if ( order > 2 )
		{
			order = 0;
			for ( i = 0; i < 3; i++ )
				t[i].tv_sec = 0;
			
			/*
			 *  Find Thetas
			 */
			
			/* between t0 and t1 */
			theta[0] = acos ( (ns_to_s(t[1].tv_nsec - t[0].tv_nsec) * MACH) / conf.ds );

			/* between t1 and t2 */
			theta[1] = acos ( (ns_to_s(t[2].tv_nsec - t[1].tv_nsec) * MACH) / conf.ds );

			/* between t0 and t2 */
			theta[2] = acos ( (ns_to_s(t[2].tv_nsec - t[0].tv_nsec) * MACH) / conf.ds );
			
			/*
			 *  Map midpoints to sensors
			 */
			mp_order[0] = mp_map[t_to_m[0]][t_to_m[1]];
			mp_order[1] = mp_map[t_to_m[1]][t_to_m[2]];
			mp_order[2] = mp_map[t_to_m[0]][t_to_m[2]];
			
			/*
			 *  Adjust thetas depending on midpoint
			 */
			for ( i = 0; i < 3; i++ )
			{
				if ( sonar_coord_cmp ( mp_order[i], midpoint[TOPLEFT] ) )
				{
					theta[i] += pi / 3;
				}
				else if ( sonar_coord_cmp ( mp_order[i], midpoint[RIGHTTOP] ) )
				{
					theta[i] -= pi / 3;
				}
				
			}
			
			/*
			 *  Determine b[]s (y-intercepts)
			 */
			for ( i = 0; i < 3; i++ )
				b[i] = mp_order[i].y - tan(theta[i]) * mp_order[i].x;
				
			/*
			 *  Determine X of loc (TODO: Average together all loc.x)
			 */
			loc->x = ( b[1] - b[0] ) / ( tan(theta[0]) - tan(theta[1]) );
			
			
			/*
			 *  Determine Y of loc (TODO: Average together all loc.y)
			 */
			loc->y = tan(theta[0]) * loc->x + b[0];
		}
	}
}