#include <string.h>
#include <ogcsys.h>
#include "light.h"

#define LIGHT_SCALE 128

// Values pulled from wii light demo
static struct timespec _wiilight_timeOn;
static struct timespec _wiilight_timeOff;

static int WIILIGHT_IsOn;
static lwp_t _wiilight_thread;

static vu32 *_wiilight_reg = (u32*)0xCD0000C0;

void _wiilight_turn(int enable)
{
	u32 val = (*_wiilight_reg&~0x20);
	if(enable) val |= 0x20;
	*_wiilight_reg=val;
}

static void * _wiilight_loop(void *arg)
{	
	struct timespec timeOn;
	struct timespec timeOff;
	while(WIILIGHT_IsOn){
		timeOn=_wiilight_timeOn;
		timeOff=_wiilight_timeOff;
		_wiilight_turn(1);
		nanosleep(&timeOn);
		if(timeOff.tv_nsec>0) _wiilight_turn(0);
		nanosleep(&timeOff);
	}
	
	return NULL;	
}

int WIILIGHT_SetLevel(int level)
{
	long level_on;
	long level_off;
	if(level>255) level=255;
	if(level<0) level=0;
	level_on=level*40000;
	level_off=10200000-level_on;
	_wiilight_timeOn.tv_nsec=level_on;
	_wiilight_timeOff.tv_nsec=level_off;
	
	return level;
}

void WIILIGHT_Init()
{
	_wiilight_timeOn.tv_sec=(time_t)0;
	_wiilight_timeOff.tv_sec=(time_t)0;
	WIILIGHT_IsOn=0;
	WIILIGHT_SetLevel(0);
}

void WIILIGHT_TurnOff()
{	
	WIILIGHT_IsOn=false;
}

void WIILIGHT_TurnOn()
{	
	WIILIGHT_IsOn=true;
	LWP_CreateThread(&_wiilight_thread, _wiilight_loop,NULL,NULL,0,80);	
}

Light::Light()
{
	// Ensure no wii light
	WIILIGHT_Init();
	WIILIGHT_SetLevel(0);
	WIILIGHT_TurnOff();
	
	// Ensure average is zero
	memset(lastAverages, 0, sizeof(int) * NUM_AVERAGES);
}

Light::~Light()
{
	WIILIGHT_SetLevel(0);
	WIILIGHT_TurnOff();
}

void Light::resetAverage()
{
	// Ensure average is zero
	memset(lastAverages, 0, sizeof(int) * NUM_AVERAGES);
	
	// Ensure no light on
	WIILIGHT_SetLevel(0);
}

void Light::setFlashEnabled(bool enabled)
{
	if(!enabled)
	{
		// Turn off
		WIILIGHT_SetLevel(0);
		WIILIGHT_TurnOff();
	}
	else
	{
		// Turn on
		WIILIGHT_SetLevel(0);
		WIILIGHT_TurnOn();
	}
}

void Light::setLightData(int *out)
{
	// Get average of all fft
	int average = 0;
	
	for(int k = 0; k < 16; k++)
	{
		// Add to average
		average += out[k];
	}
	
	// Make sure true average
	average /= 16;
	
	// Compute overall last average
	int soundAverage = 0;
	for(int i = 0; i < NUM_AVERAGES; i++)
	{
		soundAverage += lastAverages[i];
	}
	
	// Now we know the average fft strength over last NUM_AVERAGES samples
	soundAverage /= NUM_AVERAGES;
	
	// Lower cap
	int level = average - soundAverage;
	if(level < 0) { level = 0; }
	
	// Set level
	WIILIGHT_SetLevel(level / LIGHT_SCALE);
	
	// Move into average buffer
	memmove(&lastAverages[1], &lastAverages[0], sizeof(int) * (NUM_AVERAGES - 1));
	lastAverages[NUM_AVERAGES - 1] = average;
}
