#ifndef __pwm_h__
#define __pwm_h__

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

/*
TCCRnA:
7-6 : Set comparison with OCnA
5-4 : Set comparison with OCnB
1-0:

PB7 - OC3B
PB6 - OCR3A TCCR3B TCNT3 TCCR3A COM3A0 WGM32 CS31 CS30

PB4 - OC0B
PB3 - OC0A

PD7 - OC2A
PD6 - OC2B

PD5 - OC1A
PD4 - OC1B
*/


// 0.954 hz is lowest frequency possible with this function,
// based on settings in PWM_on()
// Passing in 0 as the frequency will stop the speaker from generating sound
void set_PWM(unsigned char channel, double frequency) {
    static double current_frequency_0; // Keeps track of the currently set frequency
    static double current_frequency_1; // Keeps track of the currently set frequency
    static double current_frequency_2; // Keeps track of the currently set frequency
    static double current_frequency_3; // Keeps track of the currently set frequency

    switch(channel)
    {
        case 0:
            if (frequency != current_frequency_0) // Only update registers when frequency changes
            {
                if (!frequency) { TCCR0B &= 0x08; } //stops timer/counter
                else { TCCR0B |= 0x03; } // resumes/continues timer/counter

                // prevents OCR0A from overflowing, using prescaler 64
                // 0.954 is smallest frequency that will not result in overflow
                if (frequency < 0.954) { OCR0A = 0xFFFF; }

                // prevents OCR0A from underflowing, using prescaler 64
                // 31250 is largest frequency that will not result in underflow
                else if (frequency > 31250) { OCR0A = 0x0000; }

                // set OCR0A based on desired frequency
                else { OCR0A = (short)(8000000 / (128 * frequency)) - 1; }

                TCNT0 = 0; // resets counter
                current_frequency_0 = frequency; // Updates the current frequency
            }
            break;
        case 1:
            if (frequency != current_frequency_1) // Only update registers when frequency changes
            {
                if (!frequency) { TCCR1B &= 0x08; } //stops timer/counter
                else { TCCR1B |= 0x03; } // resumes/continues timer/counter

                // prevents OCR1A from overflowing, using prescaler 64
                // 0.954 is smallest frequency that will not result in overflow
                if (frequency < 0.954) { OCR1A = 0xFFFF; }

                // prevents OCR1A from underflowing, using prescaler 64
                // 31250 is largest frequency that will not result in underflow
                else if (frequency > 31250) { OCR1A = 0x0000; }

                // set OCR1A based on desired frequency
                else { OCR1A = (short)(8000000 / (128 * frequency)) - 1; }

                TCNT1 = 0; // resets counter
                current_frequency_1 = frequency; // Updates the current frequency
            }
            break;
        case 2:
            if (frequency != current_frequency_2) // Only update registers when frequency changes
            {
                if (!frequency) { TCCR2B &= 0x08; } //stops timer/counter
                else { TCCR2B |= 0x03; } // resumes/continues timer/counter

                // prevents OCR2A from overflowing, using prescaler 64
                // 0.954 is smallest frequency that will not result in overflow
                if (frequency < 0.954) { OCR2A = 0xFFFF; }

                // prevents OCR2A from underflowing, using prescaler 64
                // 31250 is largest frequency that will not result in underflow
                else if (frequency > 31250) { OCR2A = 0x0000; }

                // set OCR2A based on desired frequency
                else { OCR2A = (short)(8000000 / (128 * frequency)) - 1; }

                TCNT2 = 0; // resets counter
                current_frequency_2 = frequency; // Updates the current frequency
            }
            break;
        case 3:
            if (frequency != current_frequency_3) // Only update registers when frequency changes
            {
                if (!frequency) { TCCR3B &= 0x08; } //stops timer/counter
                else { TCCR3B |= 0x03; } // resumes/continues timer/counter

                // prevents OCR3A from overflowing, using prescaler 64
                // 0.954 is smallest frequency that will not result in overflow
                if (frequency < 0.954) { OCR3A = 0xFFFF; }

                // prevents OCR3A from underflowing, using prescaler 64
                // 31250 is largest frequency that will not result in underflow
                else if (frequency > 31250) { OCR3A = 0x0000; }

                // set OCR3A based on desired frequency
                else { OCR3A = (short)(8000000 / (128 * frequency)) - 1; }

                TCNT3 = 0; // resets counter
                current_frequency_3 = frequency; // Updates the current frequency
            }
            break;
        default:
            break;
    }
}

void PWM_on(unsigned char channel) {
    // COMnA0: Toggle channel to compare match between counter and OCRnA
    // WGMn2: When counter (TCNTn) matches OCRnA, reset counter
    // CSn1 & CSn0: Set a prescaler of 64

    switch(channel)
    {
        case 0:
            TCCR0A = (1 << COM0A0);
            TCCR0B = (1 << WGM02) | (1 << CS01) | (1 << CS00);
            set_PWM(0,0);
            break;
        case 1:
            TCCR1A = (1 << COM1A0);
            TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10);
            set_PWM(1,0);
            break;
        case 2:
            TCCR2A = (1 << COM2A0);
            TCCR2B = (1 << WGM22) | (1 << CS21) | (1 << CS20);
            set_PWM(2,0);
            break;
        case 3:
            TCCR3A = (1 << COM3A0);
            TCCR3B = (1 << WGM32) | (1 << CS31) | (1 << CS30);
            set_PWM(3,0);
            break;
        default:
            break;
    }
}

void PWM_off(unsigned char channel) {
    switch(channel)
    {
        case 0:
            TCCR0A = 0x00;
            TCCR0B = 0x00;
            break;
        case 1:
            TCCR1A = 0x00;
            TCCR1B = 0x00;
            break;
        case 2:
            TCCR2A = 0x00;
            TCCR2B = 0x00;
            break;
        case 3:
            TCCR3A = 0x00;
            TCCR3B = 0x00;
            break;
        default:
            break;
    }
}

#endif