#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#include "utils.h"
#include "bitmanip.h"

// prototypes
void init_rgb();
void set_rgb(uint8_t r, uint8_t g, uint8_t b);
uint8_t get_red(char *input);
uint8_t get_green(char *input);
uint8_t get_blue(char *input);
uint8_t hex_to_int(char c);

// Table 14-9 : Port D Pins Alternate Functions
// OC2B (PD3) : Red
// OC0B (PD5) : Green
// OC0A (PD6) : Blue
void init_rgb() {
	MODE_OUTPUT(RGB5_RED);
	MODE_OUTPUT(RGB5_GEEN);
	MODE_OUTPUT(RGB5_BLUE);

    TCCR0A = 0;											// 15.9.1 : Clear Timer0 control register A
	TCCR0B = 0;											// 15.9.2 : Clear Timer0 control register B
	TCCR2A = 0;											// Clear Timer2 control register A

	TCCR0A |= (1 << WGM00) | (1 << WGM01);				// Table 15-8 : Timer0 in Fast PWM mode
	TCCR2A |= (1 << WGM20) | (1 << WGM21);				// Table 18-8 : Timer2 in Fast PWM mode

    TCCR0A |= (1 << COM0A1);							// Table 15-3 : BLUE - Timer0 Non-inverting mode for Blue (OC0A -> PD6)
	TCCR0A |= (1 << COM0B1);							// Table 15-6 : GREEN - Timer0 Non-inverting mode for Green (OC0B -> PD5)
	TCCR2A |= (1 << COM2B1);							// Table 18-6 : RED - Timer2 Non-inverting mode for Red (OC2B -> PD3)

    TCCR0B |= (1 << CS01);								// Table 15-9 : Prescaler 8 → ~8kHz PWM frequency
    TCCR2B |= (1 << CS21);								// Table 18-9 : Prescaler 8 → ~8kHz PWM frequency

    OCR0A = 0;											// Blue OFF
    OCR0B = 0;											// Green OFF
    OCR2B = 0;											// Red OFF
}

void set_rgb(uint8_t r, uint8_t g, uint8_t b) {
    OCR2B = r;											// Red (PD3, Timer2 OCR2B)
    OCR0B = g;											// Green (PD5, Timer0 OCR0B)
    OCR0A = b;											// Blue (PD6, Timer0 OCR0A)
}

// convert hex to int : https://stackoverflow.com/a/39052987
uint8_t hex_to_int(char c) {
	if (c >= '0' && c <= '9') {
		return c - '0';
	}
	if (c >= 'A' && c <= 'F') {
		return c - 'A' + 10;
	}
	if (c >= 'a' && c <= 'f') {
		return c - 'a' + 10;
	}
	return 0;
}

// "<< 4" is equivalent to "* 16"
uint8_t get_red(char *input) {
	if (input[0] == '#') {
		return (hex_to_int(input[1]) << 4) + hex_to_int(input[2]);
	}
	return 0;
}
uint8_t get_green(char *input) {
	if (input[0] == '#') {
		return (hex_to_int(input[3]) << 4) + hex_to_int(input[4]);
	}
	return 0;
}
uint8_t get_blue(char *input) {
	if (input[0] == '#') {
		return (hex_to_int(input[5]) << 4) + hex_to_int(input[6]);
	}
	return 0;
}

// led RGB D5 must turns on in a loop of colors using PWM
int main() {
	init_rgb();

	char *input = "#FFFF00";
	uint8_t r = get_red(input);
	uint8_t g = get_green(input);
	uint8_t b = get_blue(input);
	set_rgb(r, g, b);
    while (1);
}