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

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

void init_rgb();

void set_rgb(uint8_t r, uint8_t g, uint8_t b);

void wheel(uint8_t pos) {
	pos = 255 - pos;
	if (pos < 85) {
		set_rgb(255 - pos * 3, 0, pos * 3);
	} else if (pos < 170) {
		pos = pos - 85;
		set_rgb(0, pos * 3, 255 - pos * 3);
	} else {
		pos = pos - 170;
		set_rgb(pos * 3, 255 - pos * 3, 0);
	}
}

// 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)
}

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

    uint8_t pos = 0;
    while (1) {
        wheel(pos);										// Set color based on wheel position
        _delay_ms(10);									// Small delay for smooth transition
        pos++;											// Increment color position
    }
}