#include CMSIS_device_header
#include "Driver_GPIO.h"

#include "cmsis_os2.h"

#include "sram_vtor.h"
#include "debounce.h"

extern ARM_DRIVER_GPIO Driver_GPIOA, Driver_GPIOC;
ARM_DRIVER_GPIO *ioa = &Driver_GPIOA, *ioc = &Driver_GPIOC;

// CMSIS RTOS2 error handler
void osRtxErrorNotify(uint32_t code)
{
	(void)code;
	// Handle RTOS errors
	while(1);
}

void control_led_task(void *args)
{
	(void)args;

	sample_state button_samples;
	debounce_init(&button_samples, true, 5);
	bool led = true;
	ioa->SetOutput(5, led);

	while (1)
	{
		bool *stable;
		stable = debounce(
			&button_samples, ioc->GetInput(13));
		if (stable && !*stable)
		{
			led = !led;
			ioa->SetOutput(5, led);
		}
		vTaskDelay(pdMS_TO_TICKS(1000));
	}
}

int main(void)
{
	NVIC_Relocate_VTOR();

	SystemCoreClockUpdate();

	RCC->AHB1ENR |=
		RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOCEN;
	(void)RCC->AHB1ENR;
	(void)RCC->AHB1ENR;

	ioa->Setup(5, NULL);
	ioa->SetDirection(5, ARM_GPIO_OUTPUT);
	ioc->Setup(13, NULL);

	osKernelInitialize();
	
	const osThreadAttr_t thread_attr = {
		.name = "LED Control",
		.stack_size = 512  // 512 words = 2048 bytes
	};
	osThreadNew(control_led_task, NULL, &thread_attr);
	
	osKernelStart();
	
	// Should never reach here
	while(1);
}