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

#define S1 0x08
#define LED1 0x01
#define LED2 0x40
#define CENTER 1450
#define DISPLACE 300

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

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

int positions[] = { CENTER, CENTER - DISPLACE, CENTER, CENTER + DISPLACE };

void move_to(int displace) {
    TACCR1 = CENTER + displace;
    TACCTL1 = OUTMOD_7; // Reset/Set
    TACTL |= MC0; // Enable timer, up mode

    delay_ms(200);

    TACCTL1 = OUTMOD_1; // Off mode
    TACTL |= TACLR; // Disable timer
}

volatile char soft_button = 0;

void wait_button() {
    while (P1IN & 0x08) {
        if (soft_button) {
            soft_button = 0;
            return;
        }
    }
    delay_ms(10);
    while (!(P1IN & 0x08)) ;
    delay_ms(100);
}

int main() {
    // Disable watchdog
    WDTCTL = WDTPW | WDTHOLD;

    // 1MHz internal timer
    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;

    // P2 (XOUT/XIN) as GPIO
    P2SEL = 0x00;
    
    P1OUT = S1; // pull-up for S2
    P1REN = S1; // resistor for S2
    P1DIR = LED1 | LED2; // LEDs

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

    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

    move_to(0);

    while (1) {
        while (soft_button == 0);

        if (soft_button == 1) {
            move_to(DISPLACE);
            move_to(0);
        } else {
            move_to(-DISPLACE);
            move_to(0);
        }

        soft_button = 0;
    }

    /*move_to(-DISPLACE);
    move_to(0);

    int i = 0;

    while (1) {
        wait_button();

        i++;
        if (i & 1) {
            move_to(DISPLACE);
            move_to(0);
        } else {
            move_to(-DISPLACE);
            move_to(0);
        }
    }
    
    TACCR1 = 1500;

    int i = 0;

    while (1) {
        int pos = positions[i];

        TACCR1 = pos;
        
        P1OUT &= ~(LED1 | LED2);

        if (pos == CENTER)
            P1OUT |= LED1 | LED2;
        else if (pos < CENTER)
            P1OUT |= LED2;
        else
            P1OUT |= LED1;

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

        i = (i + 1) % 4;
    }
    

    TACTL = TACLR;
    P1OUT = 1;
    */

    return 0;
}