#include <poti.h>
#include <pca9555.h>
#include <stdint.h>
#include <stm32f0xx.h>
#include <stm-periph/stm32-gpio-macros.h>
#include <systick.h>
#include <dmx.h>
#include <gamma.h>
#include <animation.h>
#include <string.h>
#include <stdbool.h>

static uint8_t dmx_universe[129];
static struct animation led_animation;

enum color_mode {
	MODE_RED,
	MODE_GREEN,
	MODE_BLUE,
	MODE_RGB,
	MODE_WHITE_DISCRETE,
	MODE_ANIMATION,
	MODE_GREEN_BLUE_ANGLE,
	MODE_WHITE_ANGLE,
	MODE_DMX_SHUTDOWN,
};

static void setup_pins(void)
{
	uint32_t tmp;
	
	RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN;
	
	tmp = GPIOA->MODER;
	
	/* Reset all used port pins */
	tmp &= MODER_DELETE(0) & MODER_DELETE(1) & MODER_DELETE(2) & MODER_DELETE(3) &
		MODER_DELETE(4) & MODER_DELETE(5) & MODER_DELETE(6) & MODER_DELETE(7) &
		MODER_DELETE(8) & MODER_DELETE(9) & MODER_DELETE(10); 
	
	/* Analog ports for POTIs */
	tmp |= ANALOG(0) | ANALOG(1) | ANALOG(4);
	
	/* Setup USART1 RX/TX (AF1) */
	tmp |= ALTFUNC(2) | ALTFUNC(3);
	SETAF(GPIOA, 2, 1);
	SETAF(GPIOA, 3, 1);
	
	/* LED enables for Potentiometers */
	tmp |= OUTPUT(5) | OUTPUT(6) | OUTPUT(7);
	GPIOA->ODR &= ~((1 << 5) | (1 << 6) | (1 << 7));
	
	/* Setup I2C pins (AF4) */
	tmp |= ALTFUNC(9) | ALTFUNC(10);
	SETAF(GPIOA, 9, 4);
	SETAF(GPIOA, 10, 4);
	
	/* Set I2C Pins to open drain. Others are push pull */
	GPIOA->OTYPER = OTYP_OPENDRAIN(9) | OTYP_OPENDRAIN(10);
	
	/* Write changes to GPIOA mode register */
	GPIOA->MODER = tmp;
	
	/* PB1 Input with pullup */
	GPIOB->MODER &= MODER_DELETE(1);
	GPIOB->PUPDR = PULLUP(1);
}

static uint32_t map_index_to_range(int32_t index, uint32_t len)
{
	int32_t mapped;

	if (index < 0) {
		mapped = len - (-index) % len;
	} else {
		mapped = index % len;
	}

	return (uint32_t)mapped;
}

static void dmx_universe_set_rgbw(uint32_t led_idx, const struct rgbw_color *col)
{
	dmx_universe[led_idx * 4 + 0] = col->red;
	dmx_universe[led_idx * 4 + 1] = col->green;
	dmx_universe[led_idx * 4 + 2] = col->blue;
	dmx_universe[led_idx * 4 + 3] = col->white;
}

static void set_angle(uint32_t led_count, uint32_t pos, uint32_t width, const struct rgbw_color *col, const struct rgbw_color *rest)
{
	int32_t lower_end;
	int32_t upper_end;
	int32_t idx;
	int32_t mapped_idx;

	if (width > led_count)
		return;
	
	upper_end = pos + width / 2u;
	lower_end = pos - width / 2u;

	/* Set leds to color */
	for (idx = lower_end; idx <= upper_end; idx++) {
		mapped_idx = map_index_to_range(idx, led_count);
		dmx_universe_set_rgbw(mapped_idx, col);
	}

	/* Set the rest ring to rest color */
	for (idx = upper_end + 1; idx < (upper_end + 1 + (led_count - width)); idx++) {
		mapped_idx = map_index_to_range(idx, led_count);
		dmx_universe_set_rgbw(mapped_idx, rest);
	}
}

static void process_mode(enum color_mode mode, const struct poti_values *poti_vals)
{
	uint32_t odr;
	static bool last_blink_state = false;
	bool blink_state;
	int i;
	struct rgbw_color col1 = {0, 0, 0, 0};
	struct rgbw_color col2 = {0, 0, 0, 0};
	
	odr = GPIOA->ODR;
	odr &= ~((1<<5) | (1<<6) | (1<<7));

	switch (mode) {
	case MODE_RED:
	case MODE_GREEN:
	case MODE_BLUE:
		for (i = 0; i < 32; i++) {
			dmx_universe[i * 4 + 0] = mode == MODE_RED ? poti_vals->pot_vals_filtered[0] : 0;
			dmx_universe[i * 4 + 1] = mode == MODE_GREEN ? poti_vals->pot_vals_filtered[0] : 0;
			dmx_universe[i * 4 + 2] = mode == MODE_BLUE ? poti_vals->pot_vals_filtered[0] : 0;
			dmx_universe[i * 4 + 3] = 0x0;
		}
		dmx_universe[128] = 0;
		odr |= (1<<5);
		break;
	case MODE_RGB:
		odr |= (1<<5) | (1<<6) | (1<<7);
		for (i = 0; i < 32; i++) {
			dmx_universe[i * 4 + 0] = poti_vals->pot_vals_filtered[0];
			dmx_universe[i * 4 + 1] = poti_vals->pot_vals_filtered[1];
			dmx_universe[i * 4 + 2] = poti_vals->pot_vals_filtered[2];
			dmx_universe[i * 4 + 3] = 0x0;
		}
		dmx_universe[128] = 0;
		break;
	case MODE_WHITE_DISCRETE:
		odr |= (1<<5) | (1<<6);
		for (i = 0; i < 32; i++) {
			dmx_universe[i * 4 + 0] = 0;
			dmx_universe[i * 4 + 1] = 0;
			dmx_universe[i * 4 + 2] = 0;
			dmx_universe[i * 4 + 3] = gamma_corr(poti_vals->pot_vals_filtered[1]);
		}
		dmx_universe[128] = gamma_corr(poti_vals->pot_vals_filtered[0]);
		break;
	case MODE_ANIMATION:
		animation_process(&led_animation, dmx_universe);
		break;
	case MODE_DMX_SHUTDOWN:
		blink_state = systick_get_blink_state();
		if (blink_state != last_blink_state) {
			port_expander_set_leds(blink_state ? (1<<0) : (1<<4));
		}
		last_blink_state = blink_state;
		break;
	case MODE_GREEN_BLUE_ANGLE:
		dmx_universe[128] = 0u;
		odr |= (1<<5) | (1<<6) | (1<<7);
		col1.blue = poti_vals->pot_vals_filtered[0];
		col2.green = poti_vals->pot_vals_filtered[0];
		set_angle(32u, (int32_t)poti_vals->pot_vals_filtered[1] * 32u / 255u,
			(int32_t)poti_vals->pot_vals_filtered[2] * 32u / 255u, &col1, &col2);
		break;
	case MODE_WHITE_ANGLE:
		dmx_universe[128] = 0u;
		odr |= (1<<5) | (1<<6) | (1<<7);
		col1.white = poti_vals->pot_vals_filtered[0];
		col1.red = poti_vals->pot_vals_filtered[0];
		col1.green = poti_vals->pot_vals_filtered[0];
		col1.blue = poti_vals->pot_vals_filtered[0];
		set_angle(32u, (int32_t)poti_vals->pot_vals_filtered[1] * 32u / 255u,
			(int32_t)poti_vals->pot_vals_filtered[2] * 32u / 255u, &col1, &col2);
		break;
	default:
		break;
	}
	
	GPIOA->ODR = odr;
}

static bool filtered_poti_vals_equal(const struct poti_values *p1, const struct poti_values *p2)
{
	uint32_t i;
	bool ret = true;

	for (i = 0; i < POTI_COUNT; i++) {
		if (p1->pot_vals_filtered[i] != p2->pot_vals_filtered[i]) {
			ret = false;
			break;
		}
	}

	return ret;
}

/**
 * @brief Check if a color mode requires continous updates
 * 
 * @param mode Color mode input
 * @return true Color mode requires continous updates
 * @return false Color mode does not reuqire continuous update
 */
static bool mode_is_continuous(enum color_mode mode)
{
	bool ret = false;

	switch (mode) {
	case MODE_ANIMATION:
	case MODE_DMX_SHUTDOWN:
		ret = true;
		break;
	default:
		ret = false;
		break;
	}

	return ret;
}

int main(void)
{
	int i;
	uint8_t port;
	uint8_t button_pressed;
	enum color_mode mode = MODE_DMX_SHUTDOWN;
	enum color_mode old_mode = MODE_BLUE;
	struct poti_values poti_vals = {0};
	struct poti_values old_poti_vals =  {0};
	bool buttons_released_since_dual_press = true;

	setup_pins();
	dmx_init(dmx_universe, sizeof(dmx_universe), GPIOA, 2u, 2000u, 10u, 5u);
	poti_init_adc();
	port_expander_init();

	port_expander_set_leds(1 << 4);

	/* Setup Systick for 1ms ticks */
	SysTick_Config(48000000UL/1000);

	for (i = 0; i < sizeof(dmx_universe); i++) {
		dmx_universe[i] = 0x00;
	}

	while (1) {
		systick_wait_ms(20);
		poti_get_values(&poti_vals);
		port = port_expander_get_buttons();

		/* Isolate lowest set bit. This prevents edge cases where multipe switches are pressed */
		button_pressed = 0u;
		for (i = 0; i < 8; i++) {
			if (port & 0x1) {
				button_pressed = i + 1u;
				break;
			}
			port >>= 1;
		}

		if (button_pressed) {
			if (!buttons_released_since_dual_press)
				button_pressed = 0u;
		} else {
			/* Buttons are released. Remember this */
			buttons_released_since_dual_press = true;
		}

		switch (button_pressed) {
		case 1:
			mode = MODE_RED;
			break;
		case 2:
			mode = MODE_GREEN;
			break;
		case 3:
			mode = MODE_BLUE;
			break;
		case 4:
			mode = MODE_RGB;
			break;
		case 5:
			mode = MODE_WHITE_DISCRETE;
			break;
		case 6:
			mode = MODE_WHITE_ANGLE;
			break;
		case 7:
			mode = MODE_GREEN_BLUE_ANGLE;
			break;
		case 8:
			animation_reset(&led_animation);
			mode = MODE_ANIMATION;
			break;
		default:
			break;
		}

		/* Check for special case: 1 + 5 pressed simultaneously*/
		if ((port & ((1<<0 | (1<<4)))) == ((1<<4) | (1<<0))) {
			/* Prevent nomal logic from triggering on this */
			button_pressed = 0;
			
			/* set mode to DMX off and disable DMX */
			mode = MODE_DMX_SHUTDOWN;
			buttons_released_since_dual_press = false;
			dmx_stream_stop();
		} else if (mode != old_mode && old_mode == MODE_DMX_SHUTDOWN && buttons_released_since_dual_press) {
			/* Wakeup DMX */
			dmx_stream_start();
		}

		if (button_pressed)
			port_expander_set_leds((1 << (button_pressed - 1)));

		if (mode != old_mode || !filtered_poti_vals_equal(&poti_vals, &old_poti_vals) ||
		mode_is_continuous(mode)) {
			/* The values changed. Update the DMX frame */
			process_mode(mode, &poti_vals);
		}

		/* Move to old values. Used to detect changes */
		old_mode = mode;
		old_poti_vals = poti_vals;
	}
}