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

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

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

void pwm(int duty, int ms) {
    TACCR1 = duty;

    delay_ms(ms);

    TACCR1 = 0;
}

void wait_button() {
    while (P1IN & 0x08)
        delay_ms(10);

    while (!(P1IN & 0x08))
        delay_ms(10);
}

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

    // TA0 on P1.1, TA1 on P1.2
    P1SEL |= 0x04;
    //P1SEL2 |= 0x04;
    P1DIR |= 0x04;

    // Switch & pullup
    P1REN |= 0x08;
    P1OUT |= 0x08;

    TACTL = TASSEL1 | TACLR; // SMCLK source for timer
    
    TACCR0 = 20000; // Refresh every 20ms
    TACCR1 = 0; // 0 output power
    TACCTL1 = OUTMOD_7; // Reset/Set
    TACTL |= MC0; // Enable timer, up mode

    //ADC10CTL0 &= ~ENC; // Disable ADC
    //ADC10CTL0 = MSC | ADC10SHT_3 | ADC10ON | ADC10IE; // 64 clock ticks, ADC on, interrupt enabled
    //ADC10CTL1 = INCH_5 | ADC10SSEL_2 | CONSEQ_2; // channel P1.5, MCLK clock, repeat-single-channel
    //ADC10DTC0 |= ADC10CT;

    //__enable_interrupt();
    
    //while(1) {
    //    ADC10CTL0 |= ENC | ADC10SC;
    //    delay_ms(20);
    //    P1OUT ^= 0x01;
    //}

    const int DISPLACE = 200;
    const int CENTER = 1350;

    pwm(CENTER, 200);

    while (1) {
        wait_button();

        P1OUT |= 0x01;

        pwm(CENTER - DISPLACE, 200);
        pwm(CENTER, 200);

        P1OUT &= ~0x01;

        wait_button();

        P1OUT |= 0x01;

        pwm(CENTER + DISPLACE, 200);
        pwm(CENTER, 200);

        P1OUT &= ~0x01;
    }

    TACTL = TACLR;
    P1OUT = 1;

    return 0;
}

uint16_t adc_result;
uint16_t count = 0;

interrupt (ADC10_VECTOR) adc_isr() {
    uint16_t adc = ADC10MEM;
    
    if ((adc < adc_result && adc < adc_result - 10) || (adc > adc_result && adc > adc_result + 10)) {
        TACCR2 = 500 + adc * 2;
        adc_result = adc;
    }
}