#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 ramp(int from, int to) {
    from += CENTER;
    to += CENTER;

    TACCR1 = from;
    TACCTL1 = OUTMOD_7; // Reset/Set
    TACTL |= MC0; // Enable timer, up mode

    int delta = (from > to) ? -1 : 1;

    for (int i = from; i != to; i += delta) {
        if (i < CENTER - DISPLACE - 1 || i > CENTER + DISPLACE + 1) {
            TACCTL1 = OUTMOD_1; // Off mode
            TACTL |= TACLR; // Disable timer

            // safety
            while (1) ;
        }

        TACCR1 = i;
        __delay_cycles(600);
    }

    delay_ms(50);

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

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

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

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

    // UART

    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;

    TACTL = TASSEL1 | TACLR; // SMCLK source for timer
    
    TACCR0 = 20000; // Refresh every 20ms
    TACCR1 = 0; // 0 output power

    move_to(0);

    UCA0CTL1 &= ~UCSWRST;
    IE2 |= UCA0RXIE;

    while (1) {
        __bis_status_register(LPM3_bits | GIE);

        if (rx == '0') {
            move_to(DISPLACE);
            move_to(0);
            //ramp(0,  DISPLACE);
            //delay_ms(50);
            //ramp(DISPLACE, 0);
        } else if (rx == '1') {
            move_to(-DISPLACE);
            move_to(0);
            //ramp(0, -DISPLACE);
            //delay_ms(50);
            //ramp(-DISPLACE, 0);
        }
    }

    return 0;
}

interrupt(USCIAB0RX_VECTOR) uart_rx_isr() { 
    rx = UCA0RXBUF;

    __bic_status_register_on_exit(LPM3_bits);
}