#include "leds.h"

void leds_setup(void)
{
	pinMode(LEDS_GRN, OUTPUT);
	pinMode(LEDS_RED, OUTPUT);
	pinMode(LED1, OUTPUT);
	pinMode(LED2, OUTPUT);
	pinMode(LED3, OUTPUT);
	pinMode(LED4, OUTPUT);
}

/**
 * Tasks for the LEDs. Needs to be called every few ms(?) for multiplexing.
 * When an LED is blinking and in the off period, it is necessary to call
 * digitalWrite() twice and set the cathode. This ensures that the function
 * takes equally long to return on every iteration, s.t. the blinking duty
 * cycle is exactly 50% and LEDs blink equally bright, regardless of the status
 * of other LEDs.
 */
void leds_tasks(void)
{
	leds.counter++;
	switch (leds.counter & 0x03) {
		case 0:
			digitalWrite(LED4, 1);
			if ((leds.a & LED_BLINK) && leds.counter & 0x4000) {
				digitalWrite(LEDS_GRN, 0);
				digitalWrite(LEDS_RED, 0);
			} else {
				digitalWrite(LEDS_GRN, leds.a & 1);
				digitalWrite(LEDS_RED, leds.a & 2);
			}
			digitalWrite(LED1, 0);
			break;
		case 1:
			digitalWrite(LED1, 1);
			if ((leds.b & LED_BLINK) && leds.counter & 0x4000) {
				digitalWrite(LEDS_GRN, 0);
				digitalWrite(LEDS_RED, 0);
			} else {
				digitalWrite(LEDS_GRN, leds.b & 1);
				digitalWrite(LEDS_RED, leds.b & 2);
			}
			digitalWrite(LED2, 0);
			break;
		case 2:
			digitalWrite(LED2, 1);
			if ((leds.c & LED_BLINK) && leds.counter & 0x4000) {
				digitalWrite(LEDS_GRN, 0);
				digitalWrite(LEDS_RED, 0);
			} else {
				digitalWrite(LEDS_GRN, leds.c & 1);
				digitalWrite(LEDS_RED, leds.c & 2);
			}
			digitalWrite(LED3, 0);
			break;
		case 3:
			digitalWrite(LED3, 1);
			if ((leds.d & LED_BLINK) && leds.counter & 0x4000) {
				digitalWrite(LEDS_GRN, 0);
				digitalWrite(LEDS_RED, 0);
			} else {
				digitalWrite(LEDS_GRN, leds.d & 1);
				digitalWrite(LEDS_RED, leds.d & 2);
			}
			digitalWrite(LED4, 0);
			break;
	}
}