#include <avr/io.h>
#include "mystd.h"
#include "timer0.h"
#include "timer2.h"

#define D5_R	 PD5
#define D5_G	 PD6
#define D5_B	 PD3

#define RGB_MASK (_BV(D5_R) | _BV(D5_G) | _BV(D5_B))

void init_rgb(void) {
	t0_init_fpwm_3(PRESCALER_64);
	t2_init_fpwm_3(PRESCALER_64);

	t0_set_out_mode(TO_A | TO_B, TOM_00);
	t2_set_out_mode(TO_B, TOM_00);
	DDRD |= RGB_MASK;
}

void set_rgb(uint8_t r, uint8_t g, uint8_t b) {
	if (r == 0x00) {
		t0_set_out_mode(TO_B, TOM_00);
		PORTD = (PORTD & ~_BV(D5_R));
	} else {
		t0_set_out_mode(TO_B, TOM_10);
		t0_set_ocr(TO_B, r);
	}

	if (g == 0x00) {
		t0_set_out_mode(TO_A, TOM_00);
		PORTD = (PORTD & ~_BV(D5_G));
	} else {
		t0_set_out_mode(TO_A, TOM_10);
		t0_set_ocr(TO_A, g);
	}

	if (b == 0x00) {
		t2_set_out_mode(TO_B, TOM_00);
		PORTD = (PORTD & ~_BV(D5_B));
	} else {
		t2_set_out_mode(TO_B, TOM_10);
		t2_set_ocr(TO_B, b);
	}
}