#include <stdint.h>
#include <stdbool.h>
#include <msp430.h>

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

struct port {
    volatile uint8_t *sel, *sel2;
};

struct port ports[] = {
    { .sel = &P1SEL, .sel2 = &P1SEL2 },
    { .sel = &P2SEL, .sel2 = &P2SEL2 }
};

struct pad {
    uint8_t port;
    uint8_t bit;
};

struct pad pads[] = {
    { .port = 0, .bit = BIT6 },
    { .port = 1, .bit = BIT5 },
    { .port = 1, .bit = BIT4 },
    { .port = 1, .bit = BIT3 },
};

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

volatile uint16_t debug[20];

uint16_t pad_read(struct pad *pad) {
    volatile uint16_t result;

    TA0CTL = TASSEL_3 | MC_2;
    TA0CCTL1 = CM_3 | CCIS_2 | CAP;
    IE1 |= WDTIE;

    *(ports[pad->port].sel) &= ~pad->bit;
    *(ports[pad->port].sel2) |= pad->bit;

    WDTCTL = WDTPW | WDTTMSEL | WDTSSEL | WDTIS0 | WDTIS1;
    TA0CTL |= TACLR;

    __bis_status_register(LPM3_bits | GIE);

    TA0CCTL1 ^= CCIS0;
    result = TA0CCR1;
    WDTCTL = WDTPW | WDTHOLD;

    *(ports[pad->port].sel) &= ~pad->bit;
    *(ports[pad->port].sel2) &= ~pad->bit;

    TA0CTL |= TACLR;

    return result;
}

uint16_t pad_read2(struct pad *pad) {
    uint16_t result = 0;

    TACCR0 = 1000; // accumulationCycles

    *(ports[pad->port].sel) &= ~pad->bit;
    *(ports[pad->port].sel2) |= pad->bit;

    TA0CTL = TACLR | TASSEL_3 | MC_1;
    TA0CTL &= ~TAIFG;
    while (!(TA0CTL & TAIFG))
        result++;

    TA0CTL &= ~MC_1;

    *(ports[pad->port].sel) &= ~pad->bit;
    *(ports[pad->port].sel2) &= ~pad->bit;

    return result;
}

void send(char data) {
    UCA0TXBUF = data;
    while (!(IFG2 & UCA0TXIFG)) ;
}

volatile uint8_t recvbyte, enable;

int main() {
    WDTCTL = WDTPW | WDTHOLD;

    BCSCTL1 = CALBC1_8MHZ;
    DCOCTL = CALDCO_8MHZ;
    BCSCTL1 |= DIVA_0;
    BCSCTL3 |= LFXT1S_2;
        
    P1REN |= BIT3;
    P1DIR |= BIT0;

    P1OUT &= ~BIT3;
    P1REN &= ~BIT3;

    UCA0CTL1 |= UCSSEL_2; // SMCLK clock

    // http://mspgcc.sourceforge.net/cgi-bin/msp-uart.pl?clock=1000000&baud=9600&submit=calculate
    //UCA0BR0 = 0x68;
    //UCA0BR1 = 0x00;
    //UCA0MCTL = 0x04;
    
    // http://mspgcc.sourceforge.net/cgi-bin/msp-uart.pl?clock=8000000&baud=115200&submit=calculate
    UCA0BR0 = 0x45;
    UCA0BR1 = 0x00;
    UCA0MCTL = 0xAA;

    // UART
    P1SEL = 0x02 | 0x04;
    P1SEL2 = 0x02 | 0x04; // RXD on P1.1, TXD on P1.2

    P1OUT = 0x00;
    P1DIR |= BIT0 | BIT6;

    // Everything as GPIO
    P2SEL &= ~(BIT6 | BIT7);
    P2OUT = 0x00;
    P2DIR = 0xFF;

    UCA0CTL1 &= ~UCSWRST; // Start UART

    IE2 |= UCA0RXIE;

    __enable_interrupt();

    P1OUT |= BIT0;
    delay_ms(500);
    P1OUT &= ~BIT0;
    
    uint16_t n = 0;
    while (1) {
        while (!enable)
            __bis_status_register(LPM0_bits);
        
        P1OUT |= BIT0;
        while (enable) {
            uint16_t val = pad_read(&pads[n]);
            send(n);
            send(val >> 8);
            send(val & 0xFF);

            n += 1;
            if (n >= sizeof(pads)/sizeof(struct pad))
                n = 0;
        }
        P1OUT &= ~BIT0;
    }

    return 0;
}

interrupt(USCIAB0RX_VECTOR) uart_rx_isr() { 
    recvbyte = UCA0RXBUF;

    if (!enable) {
        if (recvbyte == 0x81) {
            enable = true;
            __bic_status_register_on_exit(LPM3_bits);
        }

        return;
    }

    if (recvbyte == 0x82)
        enable = false;
}

interrupt(WDT_VECTOR) watchdog_isr() {
    __bic_status_register_on_exit(LPM3_bits);
}