#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;
    
    uint16_t base, delta;
};

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

uint16_t threshold = 700;

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

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;
}

void pad_init(struct pad *pad) {
    pad->base = pad_read(pad);
    pad->delta = 0;
}

bool pad_pressed(struct pad *pad) {
    uint16_t result = pad_read(pad);

    if (pad->base < result) {
        pad->base = (pad->base + result) >> 1;
        pad->delta = 0;
    } else {
        pad->delta = pad->base - result;
    }

    if (pad->delta < threshold)
        pad->base--;

    return pad->delta > threshold;
}

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

int main() {
    int i;
    uint8_t cur_pressed = 0, pressed = 0;

    WDTCTL = WDTPW | WDTHOLD;

    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;
    BCSCTL1 |= DIVA_0;
    BCSCTL3 |= LFXT1S_2;
        
    P1REN |= BIT3;
    P1OUT |= BIT3;
    P1DIR |= BIT0;

    do {
        P1OUT |= BIT0;
        delay_ms(100);
        P1OUT &= ~BIT0;
        delay_ms(100);
    } while (P1IN & BIT3);

    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;

    // 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;

    for (i = 0; i < sizeof(pads)/sizeof(pads[0]); i++)
        pad_init(&pads[i]);

    __delay_cycles(5000);

    __enable_interrupt();

    //send('G'); send('o'); send('\r'); send('\n');

    while (1) {
        pressed = 0;

        for (i = 0; i < sizeof(pads)/sizeof(pads[0]); i++) {
            if (pad_pressed(&pads[i]))
                pressed |= 1 << i;
        }

        if (pressed != cur_pressed) {
            cur_pressed = pressed;

            send(0x80 | cur_pressed);

            if (cur_pressed)
                P1OUT |= BIT0;
            else
                P1OUT &= ~BIT0;
        }

        __delay_cycles(1000);
    }

    return 0;
}

/*interrupt(USCIAB0RX_VECTOR) uart_rx_isr() { 
    debug[0]++;
    while (!(IFG2 & UCA0TXIFG)) ;

    uint8_t rec = UCA0RXBUF;
}*/

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