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

    bool pressed;
};

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

uint16_t accu_cycles = 64;
uint16_t threshold = 550;

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

uint16_t debug[20];

uint16_t pad_read(struct pad *pad) {
    uint16_t times = accu_cycles;
    uint16_t result;

    debug[10]++;

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

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

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

    return result;
}

/*uint16_t pad_read(struct pad *pad) {
    uint16_t times = accu_cycles;
    uint16_t result;

    debug[10]++;

    debug[3] = ports[pad->port].sel;
    debug[4] = &P1SEL;

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

    TA0CCTL0 = CM_3 | CCIS_1 | CAP; // +/- edge, ACLK, capture mode

    debug[12]++;

    while (!(TA0CCTL0 & CCIFG)) debug[2]++; // Wait for capture
    debug[11]++;

    TA0CTL |= TACLR; // Clear timer
    
    while (times--) {
        TA0CCTL0 = CM_3 | CCIS_1 | CAP;
        while (!(TA0CCTL0 & CCIFG)) debug[3]++;
    }
    
    result = TA0CCR0;

    TA0CTL = TASSEL_3 | MC_2;

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

    return result;
}*/

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

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) {
    while (UCA0STAT & UCBUSY) ;
    UCA0TXBUF = data;
}

int main() {
    int i;
    bool pressed;

    WDTCTL = WDTPW | WDTHOLD;

    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;
    BCSCTL1 |= DIVA_0;
    BCSCTL3 |= LFXT1S_2;

    __delay_cycles(16000);

    // Led
    P1DIR |= 0x01;

    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 = BIT6;

    // Everything as GPIO
    P2SEL = 0x00;
    P2SEL2 = 0x00;
    P2OUT = 0x00;
    P2DIR = 0xFF;

    UCA0CTL1 &= ~UCSWRST; // Start UART

    IE2 |= UCA0RXIE;

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

    __delay_cycles(5000);

    //__enable_interrupt();

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

    while (1) {
        // Capacitive sense timer
        TA0CTL = TASSEL_3 | MC_2;
        TA0CCTL1 = CM_3 | CCIS_2 | CAP;
        IE1 |= WDTIE;

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

            debug[1]++;

            if (pressed != pads[i].pressed) {
                //send(('A' + i) | (pressed ? 0 : 0x20));

                pads[i].pressed = pressed;

                if (i == 0) {
                    if (pressed)
                        P1OUT |= BIT0;
                    else
                        P1OUT &= ~BIT0;
                }
            }
        }

        __delay_cycles(5000);
    }

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