#include <avr/io.h>
#include <util/delay.h>
#include <string.h>

#include "lib/light_ws2812.h"
#include "lib/pm.h"
#include "lib/mma8653.h"

#define STRIPLEN 10
struct cRGB led[STRIPLEN];
int brightness = 9;

void update() {
	ws2812_setleds(led, STRIPLEN);
	if (!(PIND & (1 << 3)))
		pm_reset();
}

int axl_test()
{
	int ret;
	int c = 0;
	struct axl_result measurements;
	
	int dir_x;
	int dir_y;
	int ledno;
	int intens;
	
	pm_axl_on();
	_delay_ms(50);
	
	ret = mma8653_init();
	if (ret) {
		led[8].b = 0x10;
		update();
		while(1);
	}

	while(1) {
		c++;
		
		#define THRES 10
		ret = mma8653_get_measurements(&measurements);
		dir_x = 0;
		if (measurements.x > THRES)
			dir_x = -1;
		else if (measurements.x < -THRES)
			dir_x = 1;
		dir_y = 0;
		if (measurements.y > THRES)
			dir_y = 1;
		else if (measurements.y < -THRES)
			dir_y = -1;

		if (dir_y == -1)
			ledno = dir_x + 1;
		else if (dir_y == 0)
			ledno = 3 + 1 -dir_x;
		else if (dir_y == 1)
			ledno = dir_x + 6 + 1;
		
		intens = abs(measurements.x + measurements.y);
		intens = intens >> 2;
		if (intens < 1)
			intens = 1;
		if (measurements.z > 0)
			led[ledno].b = intens;
		else
			led[ledno].r = intens;

		if (ret)
			led[ret].r = 0x10;

		/*if (ret) {
			led[8].g = 0;
			led[8].r = c & 1;
		} else {
			led[8].r = 0;
			led[8].g = c & 1;
		}*/
		update();
		//_delay_ms(50);
		memset(led, 0, 9 * 3);
		//update();
		_delay_ms(50);
	}
}

int main()
{	
	pm_init();
	_delay_ms(1);
	
	memset(led, 0, STRIPLEN * 3);
	for(int i = 0; i<STRIPLEN; ++i) {
		led[i].g = 0x10;
		update();
		_delay_ms(30);
		led[i].g = 0x0;
	}
	update();

	axl_test();

	while (1) {
		led[0].r = brightness;
		led[2].b = 0;
		update();
		_delay_ms(250);
		
		led[1].g = brightness;
		led[0].r = 0;
		update();
		_delay_ms(250);

		led[1].g = 0;
		led[2].b = brightness;
		update();
		_delay_ms(250);
		
		for (int i=0; i<STRIPLEN; ++i) {
			led[i].r=brightness;
			led[i].g=brightness;
			led[i].b=brightness;
		}
		update();
		_delay_ms(50);
		for (int i=0; i<STRIPLEN; ++i) {
			led[i].r=0;
			led[i].g=0;
			led[i].b=0;
		}
		update();
		_delay_ms(200);
	}
}

// User button interrupt
ISR(INT1_vect)
{
	pm_reset();
	return;
}