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

/*struct pad pads[] = {
    { .port = 0, .bit = BIT4 },
    { .port = 0, .bit = BIT5 },
    { .port = 1, .bit = BIT0 },
    { .port = 1, .bit = BIT1 },
    { .port = 1, .bit = BIT2 },
    { .port = 0, .bit = BIT7 },
    { .port = 0, .bit = BIT6 },
    { .port = 1, .bit = BIT5 },
    { .port = 1, .bit = BIT4 },
    { .port = 1, .bit = BIT3 },
};*/

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


// 700
uint16_t threshold = 600;

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

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

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

    uint16_t delta;

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

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

    return delta > threshold;
}

void pad_init_all() {
    int i;

    delay_ms(1);

    for (i = 0; i < sizeof(pads)/sizeof(pads[0]); i++) {
        struct pad *pad = &pads[i];

        pad->base = pad_read(pad);
    }

    delay_ms(1);
}

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

volatile bool enable = false;

uint16_t cur_pressed = 0;

int main() {
    int i;
    uint16_t pressed = 0;

    WDTCTL = WDTPW | WDTHOLD;

    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;
    BCSCTL1 |= DIVA_0;
    BCSCTL3 |= LFXT1S_2;
        
    P1REN |= BIT3;
    P1OUT |= 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;

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

    pad_init_all();

    __enable_interrupt();

    uint16_t ledon = 0;

    while (1) {
        while (!enable) {
            P1OUT &= ~BIT0;
            ledon = 0;
            cur_pressed = 0xFFFF;
            __bis_status_register(LPM0_bits);
        }

        if (P1IN & BIT3)
            pad_init_all();

        pressed = 0;

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

        if (pressed != cur_pressed) {
            if (pressed & ~cur_pressed) {
                ledon = 2;
                P1OUT |= BIT0;
            }

            cur_pressed = pressed;

            send(0x80 | cur_pressed);
        }

        if (ledon > 0) {
            ledon--;
            if (ledon == 0)
                P1OUT &= ~BIT0;
        }
    }

    return 0;
}

interrupt(USCIAB0RX_VECTOR) uart_rx_isr() { 
    uint8_t 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);
}