#include <stdint.h>
#include <msp430.h>
#include "musicdata.h"

#define interrupt(x) void __attribute__((interrupt (x)))

void delay_ms(uint16_t ms) {
    while (ms--)
        __delay_cycles(1000);
}

uint16_t wavelen[] = { 0, 0, 0 };
uint16_t *chanreg[3];
uint16_t count[] = { 0, 0, 0 };
uint8_t duty[] = { 1, 1, 1 };
uint16_t toggle[] = { 0, 0, 0 };

int main() {
    // Disable watchdog
    WDTCTL = WDTPW | WDTHOLD;

    // 1MHz internal timer
    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;

    // P2 (XOUT/XIN) as GPIO
    P2SEL = 0x00;
    
    P1OUT = 0x00;
    P1DIR = 0x01; // LED

    P1REN |= 0x08;

    //while (!(P1IN & 0x08)) ;

    // TA0 on P1.1, TA1 on P1.2
    P1DIR |= 0x02 | 0x04 | 0x10;

    P1SEL |= 0x02 | 0x04 | 0x10;
    P1SEL2 |= 0x10;

    TACTL = TASSEL1 | TACLR; // SMCLK source for timer

    chanreg[0] = (uint16_t *)0x162;
    chanreg[1] = (uint16_t *)0x164;
    chanreg[2] = (uint16_t *)0x166;
    
    //TACCTL0 = OUTMOD_4 | CCIE; // Channel 0 (output mode toggle, enable interrupt)
    //TACCTL1 = OUTMOD_4 | CCIE; // Channel 1
    //TACCTL2 = OUTMOD_4 | CCIE; // Channel 2

    __enable_interrupt();

    TACTL |= MC1; // Enable timer, continuous mode

    for (uint16_t i = 0; i < sizeof(music) / sizeof(uint8_t); i++) {
        uint8_t cmd = music[i];

        if (cmd & 0xC0) {
            uint8_t chan = ((cmd & 0xC0) >> 6) - 1, freq = cmd & 0x3F;

            if (freq) {
                wavelen[chan] = note_wl[freq];
                toggle[chan] = 0;
                duty[chan] = 1;
                *(chanreg[chan]) = OUTMOD_4 | CCIE;
            } else {
                *(chanreg[chan]) = OUTMOD_0 | CCIE;
            }
        } else {
            int32_t sleep = (int32_t)cmd * ticks_per_beat;
            while (sleep > 0) {
                int32_t start = TAR;
                __delay_cycles(100);
                int32_t end = TAR;

                if (start > end)
                    end += 0xFFFF;

                sleep -= end - start;
            }

            P1OUT ^= 1;
        }
    }

    TACTL = TACLR;
    P1OUT = 1;

    return 0;
}

#define UPDATE_CHANNEL(n) \
uint16_t wl = wavelen[n]; \
if (toggle[n] & 1) \
    count[n] += wl >> duty[n]; \
else \
    count[n] += wl - (wl >> duty[n]); \
toggle[n]++; \
if (toggle[n] > 16) \
    duty[n] = 2; \
TACCR##n = count[n]; \
TACCTL##n &= ~CCIFG;

interrupt(TIMER0_A0_VECTOR) ta0_isr() {
    UPDATE_CHANNEL(0);
}

interrupt(TIMER0_A1_VECTOR) ta1_isr() {
    if (TACCTL1 & CCIFG) {
        UPDATE_CHANNEL(1);
    }
    
    if (TACCTL2 & CCIFG) {
        UPDATE_CHANNEL(2);
    }
}

#undef UPDATE_CHANNEL