#include "lib/system.h"
#include "lib/hal.h"
#include <stdlib.h>
#include <util/delay.h>

#define TIMER_INMENU TIMER_APP0
#define TIMER_AUX TIMER_APP1

static int clr = 1;

void axl_test()
{
	int ret;
	struct axl_result measurements;
	
	int dir_x;
	int dir_y;
	int ledno;
	int intens;

	if (block_for(TIMER_AUX, timer_ms(50))) {
		display_clear(0);
		
		#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;

		display_update();
	}
}


void led_test(int first_launch)
{
	static int i;
	
	if (first_launch) {
		i = 0;
		led[0].r = 1;
	}

	if (block_for(TIMER_AUX, timer_ms(30))) {
		led[i].g = 0x0;
		i = (i + 1) % STRIPLEN;
		led[i].g = 0x10;
		if (led[i].r && led[i].b) {
			led[i].r = 0;
			led[i].b = 1;
		} else if (led[i].r) {
			led[i].b = 1;
		} else {
			led[i].r = 1;
			led[i].b = 0;
		}
		display_update();
	}
}

void system_test()
{
	// Display power states on first face
	if (HAL_ASSERTED_N(VLED_EN_N))
		led[0].b = 1;
	else
		led[0].b = 0;

	if (HAL_ASSERTED_N(CHG_ACTIVE_N))
		led[1].r = 1;
	else
		led[1].r = 0;
	
	if (block_for(TIMER_AUX, timer_ms(50)))
		display_update();
}

// Note: This is only for watchdog testing.
// Never use _delay_ms(...) in your programs. Use timer_* functions instead.
void wdt_test()
{
	int i = 50;
	display_clear(0);
	while (i--) {
		led[0].b = !led[0].b;
		display_update();
		_delay_ms(100);
	}
	led[0].b = 0;
	led[0].r = 1;
	display_update();
	_delay_ms(2000);
}

void app_init()
{
	clr = 1;
}

void app_suspend()
{
	return;
}

#define APP_COUNT 4
enum app_return app_mainloop()
{
	static int app_sel = 0;

	if (button_event(EV_PRESS)) {
		app_sel = (app_sel + 1) % APP_COUNT;
		display_clear();
		for (int i = 0; i < STRIPLEN; i += 9) {
			led[app_sel + i].g = 4;
		}
		display_update();
		timer_set(TIMER_INMENU, timer_ms(750));
		clr = 1;
	} else if (button_event(EV_LONGPRESS)) {
		return SUSPEND;
	}
	
	if (timer_expired(TIMER_INMENU)) {
		if (clr) {
			display_clear();
		}
		switch (app_sel) {
			case 0:
				led_test(clr);
				break;
			case 1:
				axl_test();
				break;
			case 2:
				system_test();
				break;
			case 3:
				wdt_test();
				break;
			default:
				app_sel = 0;
				break;
		}
		clr = 0;
	}

	return RUN;
}