#include "outdrivers.h"
#include "notefinder.h"
#include <stdio.h>
#include "paramstring.h"
#include <stdlib.h>
#include <libusb-1.0/libusb.h>
#include "color.h"

static struct libusb_device_handle *devh = NULL;
static int did_init = 0;
static int total_leds = 0;
static float light_siding = 0;
static float * last_led_pos;
static float * last_led_amp;
static float ampoff;
static uint8_t * last_leds;

static void LEDOutDriver( const char * parameters, struct NoteFinder * nf )
{
	if( !did_init )
	{
		if( libusb_init(NULL) < 0 )
		{
			fprintf( stderr, "Error: Could not initialize libUSB\n" );
			exit( -99 );
		}

		devh = libusb_open_device_with_vid_pid( NULL, 0xabcd, 0xf003 );

		if( !devh )
		{
			fprintf( stderr,  "Error: Cannot find device.\n" );
			exit( -98 );
		}

		total_leds = atoi( GetParamStr( parameters, "leds", "300" ) );
		ampoff = atof( GetParamStr( parameters, "amp", "1.0" ) );
		light_siding = atof( GetParamStr( parameters, "light_siding", "1.4" ) );

		printf( "Found LEDs for output.  leds=%d\n", total_leds );
		did_init = 1;
		last_led_pos = malloc( sizeof( float ) * total_leds );
		last_led_amp = malloc( sizeof( float ) * total_leds );
		last_leds = malloc( total_leds * 3 );
	}

	//Step 1: Calculate the quantity of all the LEDs we'll want.
	int totbins = nf->dists;
	int i, j;
	float binvals[totbins];
	float binpos[totbins];
	float totalbinval = 0;

	for( i = 0; i < totbins; i++ )
	{
		binpos[i] = nf->dist_means[i] / nf->freqbins;
		binvals[i] = pow( nf->dist_amps[i], light_siding );
		totalbinval += binvals[i];
	}

	float rledpos[total_leds];
	float rledamp[total_leds];
	int rbinout = 0;

	for( i = 0; i < totbins; i++ )
	{
		int nrleds = (int)((binvals[i] / totalbinval) * total_leds);
		for( j = 0; j < nrleds && rbinout < total_leds; j++ )
		{
			rledpos[rbinout] = binpos[i];
			rledamp[rbinout] = binvals[i];
			rbinout++;
		}
	}

	for( ; rbinout < total_leds; rbinout++ )
	{
		rledpos[rbinout] = 0;
		rledamp[rbinout] = 0;
	}

	//Now we have to minimize "advance".
	int minadvance = 0;
	float mindiff = 1e20;

	for( i = 0; i < total_leds; i++ )
	{
		float diff = 0;
		diff = 0;
		for( j = 0; j < total_leds*4; j++ )
		{
			diff += (last_led_pos[i] - rledpos[i])*(last_led_pos[i] - rledpos[i]);
		}
		if( diff < mindiff )
		{
			mindiff = diff;
			minadvance = i;
		}
	}

	printf( "MA: %d %f\n", minadvance, mindiff );

	//Advance the LEDs to this position when outputting the values.
	for( i = 0; i < total_leds; i++ )	
	{
		int ia = ( i - minadvance ) % total_leds;
		last_led_pos[i] = rledpos[ia];
		last_led_amp[i] = rledamp[ia] * ampoff;
		int r = HSVtoHEX( last_led_pos[i], 1.0, last_led_amp[i] );
		last_leds[i*3+0] = r & 0xff;
		last_leds[i*3+1] = (r>>8) & 0xff;
		last_leds[i*3+2] = (r>>16) & 0xff;
	}

	int r = libusb_control_transfer( devh,
		0x40, //reqtype
		0xA5, //request
		0x0100, //wValue
		0x0000, //wIndex
		last_leds,
		(total_leds*3)+1,
		1000 );

}

REGISTER_OUT_DRIVER(LEDOutDriver);