#include "msp.h"
#include "math.h"

// =========================== Convenience functions ===========================

inline void setmasked32(volatile uint32_t* ptr, uint32_t mask, uint32_t bits) {
    *ptr = (*ptr & ~mask) | bits;
}

inline void setmasked16(volatile uint16_t* ptr, uint16_t mask, uint16_t bits) {
    *ptr = (*ptr & ~mask) | bits;
}

inline void clearmasked16(volatile uint16_t* ptr, uint16_t mask) {
    *ptr &= ~(mask);
}

inline void clearbit32(volatile uint32_t* ptr, unsigned idx) {
    *ptr &= ~(1 << idx);
}

inline void clearbit16(volatile uint16_t* ptr, unsigned idx) {
    *ptr &= ~(1 << idx);
}

inline void clearbit8(volatile uint8_t* ptr, unsigned idx) {
    *ptr &= ~(1 << idx);
}

inline void setbit32(volatile uint32_t* ptr, unsigned idx) {
    *ptr |= 1 << idx;;
}

inline void setbit16(volatile uint16_t* ptr, unsigned idx) {
    *ptr |= 1 << idx;;
}

inline void setbit8(volatile uint8_t* ptr, unsigned idx) {
    *ptr |= 1 << idx;;
}

// =============================================================================


#define ADC_MAX ((1 << 14) - 1) // 14-bit ADC
#define ADC_MCTL_IDX (0)
#define TIMER_PERIOD 20 // ms
#define TIMER_DIV (8)
#define TIMER_FCLK  3000000
#define TIMER_PERIOD_TICKS (((TIMER_FCLK / TIMER_DIV) * TIMER_PERIOD) / 1000)


void startTimer(Timer_A_Type* timer) {
    // Select clock source = SMCLK
    setmasked16(&timer->CTL, TIMER_A_CTL_SSEL_MASK, TIMER_A_CTL_SSEL__SMCLK);

    // Divide by 8*1 (SMCLK defaults to 3000000Hz, dont bother to change it).
    // Although one should probably use a lower-freq clock if we are in
    // the millisecond range for interrupts.
    setmasked16(&timer->EX0, TIMER_A_EX0_IDEX_MASK, TIMER_A_EX0_IDEX__8); // divide by 8

    // CCR0 is set to the full period count.
    // When CCR0 is met, the timer should reset to 0 and repeat (in UP mode).
    timer->CCR[0] = TIMER_PERIOD_TICKS;

    // Enable IRQ from timer (caller shall enable corresponding bit in NVIC)
    setmasked16(&timer->CCTL[0], 1 << TIMER_A_CCTLN_CCIE_OFS,TIMER_A_CCTLN_CCIE);

    // Reset timer
    clearmasked16(&timer->CCTL[0], TIMER_A_CCTLN_CCIFG);
    TIMER_A0->R = 0;

    // Start the timer - Count upwards
    setmasked16(&timer->CTL, TIMER_A_CTL_MC_MASK, TIMER_A_CTL_MC__UP);
}

void enableADC() {
    // Configure pin 4.0 (A13) as an ADC input
    clearbit8(&P4->DIR, 0);
    clearbit8(&P4->SEL0, 1);
    clearbit8(&P4->SEL1, 1);

    // Enable ADC finish conversion interrupt
    NVIC_EnableIRQ(ADC14_IRQn);

    // Set voltage reference from rail to rail
    setmasked32(&ADC14->MCTL[ADC_MCTL_IDX], ADC14_MCTLN_VRSEL_MASK, ADC14_MCTLN_VRSEL_0);

    // Non-difference mode
    clearbit32(&ADC14->MCTL[ADC_MCTL_IDX], ADC14_MCTLN_DIF_OFS);

    // Input channel
    setmasked32(&ADC14->MCTL[ADC_MCTL_IDX], ADC14_MCTLN_INCH_MASK, ADC14_MCTLN_INCH_13);
    setmasked32(&ADC14->CTL1, ADC14_CTL1_CSTARTADD_MASK, ADC_MCTL_IDX << ADC14_CTL1_CSTARTADD_OFS);

    // Enable the channel 0 interrupt
    setbit32(&ADC14->IER0, ADC14_IER0_IE0_OFS);

    // This is a >software triggered< adc routine (convert when requested)
    setmasked32(&ADC14->CTL0, ADC14_CTL0_SHS_MASK, ADC14_CTL0_SHS_0);

    // Mega-speed is not ultra important so set clock source to ACLK
    setmasked32(&ADC14->CTL0, ADC14_CTL0_SSEL_MASK, ADC14_CTL0_SSEL_2);

    // Enable conversion
    setbit32(&ADC14->CTL0, ADC14_CTL0_ON_OFS);
    setbit32(&ADC14->CTL0, ADC14_CTL0_ENC_OFS);
}

void setAngle(float ratio) {
    // We work in microseconds to have the appropriate precision.
    // This allows a range of 1000 different angles (1000us to 2000 us).
    const unsigned duty_period_us = (1000 /* 1ms = 0 degrees */ + 1000 * ratio);
    const unsigned duty_ticks = ((TIMER_FCLK / TIMER_DIV) * duty_period_us) / 1000000;

    // Finally, write the new duty cycle tick count to TA0.CCR1
    TIMER_A0->CCR[1] = TIMER_PERIOD_TICKS - duty_ticks;
}


void ADC14_IRQHandler() {
    // Adjust duty cycle of PWM
    const float ratio = ((float)(ADC14->MEM[ADC_MCTL_IDX]) / ADC_MAX);
    setAngle(ratio);
}

void TA1_0_IRQHandler() {
    // Initiate software-controlled ADC conversion
    setbit32(&ADC14->CTL0, ADC14_CTL0_SC_OFS);
    clearbit32(&ADC14->CTL0, ADC14_CTL0_SC_OFS);
}

void motorController() {
    // Step 1: Enable ADC
    enableADC();
    startTimer(TIMER_A0); // PWM timer
    startTimer(TIMER_A1); // ADC sampling trigger timer (software triggered)


    // Enable PWM output of TA0.1
    // Compare mode is required to generate PWM signals.
    // Compare mode => capture mode is disables => CAP = 0
    clearbit16(&TIMER_A0->CCTL[1], TIMER_A_CCTLN_CAP_OFS);


    // Enable output for reg1 of timer 0 (P2.4)
    setbit8(&P2->DIR, 4);
    setbit8(&P2->SEL0, 4);
    clearbit8(&P2->SEL1, 4);

    // Next, we need to configure the output unit of the capture/compare
    // block to generate the proper PWM signal. The PWM is defined by
    // using the set/reset output mode (note: we are creating a
    // sawtooth wave and NOT a triangle wave).
    setmasked16(&TIMER_A0->CCTL[1], TIMER_A_CCTLN_OUTMOD_MASK, TIMER_A_CCTLN_OUTMOD_3);

    // Enable timer interrupts in NVIC
    NVIC_EnableIRQ(TA1_0_IRQn);
}


/**
 * main.c
 */
void main(void)
{
	WDT_A->CTL = WDT_A_CTL_PW | WDT_A_CTL_HOLD;		// stop watchdog timer

	// Program clock system
	CS->KEY = CS_KEY_VAL;
	// ....
    CS->KEY = 0xdeadbeef;

    motorController();
}