#include "input_capture.h"

#include "inc/msp432p401r.h"

void (*capture_task)(uint16_t time);

// Initialize Timer A0 in edge time mode to request interrupts on
// the rising edge of P7.3 (TA0CCP0). The interrupt service routine
// acknowledges the interrupt and calls a user function.
void TimerA0CaptureInit(void (*task)(uint16_t time)) {
    capture_task = task;
    P7->SEL0 |= 0x08;
    P7->SEL1 &= ~0x08; // configure P7.3 as TA0CCP0
    P7->DIR &= ~0x08;

    TIMER_A0->CTL &= ~0x30; // halt Timer A0
    TIMER_A0->CTL = 0x200; // SMCLK
    TIMER_A0->CCTL[0] = 0x4910; // capture on rising edge, synchronous capture, capture mode, interrupt enabled
    TIMER_A0->EX0 &= ~0x7; // input clock divider /1

    NVIC->IP[2] = (NVIC->IP[2] & 0xFFFFFF00) | 0x00000002; // priority 2

    // interrupts enabled in the main program after all devices initialized
    NVIC->ISER[0] = 0x100; // enable interrupt 8 in NVIC

    TIMER_A0->CTL |= 0x24;        // reset and start Timer A0 in continuous up mode
}

void TA0_0_IRQHandler(void) {
    TIMER_A0->CCTL[0] &= ~0x1;      // acknowledge capture/compare interrupt
    capture_task(TIMER_A0->CCR[0]); // execute user task
}


// Left Encoder A connected to P10.5 (J5)
// Right Encoder A connected to P10.4 (J5)

// user function, time is up-counting timer value when edge occurred in units of 0.083 usec
// called when P10.4/.5 (TA3CCP0/1) edge occurs
void (*capture_task0)(uint16_t time); 
void (*capture_task1)(uint16_t time);

// Initialize Timer A3 in edge time mode to request interrupts on
// the rising edges of P10.4 (TA3CCP0) and P10.5 (TA3CCP1).
// Interrupt service routines acknowledge the interrupt and call a user function.
// Assumes: low-speed subsystem master clock is 12 MHz
void TimerA3CaptureInit(void (*task0)(uint16_t time), 
                        void (*task1)(uint16_t time)) {
    capture_task0 = task0;
    capture_task1 = task1;
    P10->SEL0 |= 0x30;   // Set SEL0 for bits 4 and 5
    P10->SEL1 &= ~0x30;  // Clear SEL1 for bits 4 and 5
    P10->DIR &= ~0x30;   // Set as inputs

    TIMER_A3->CTL &= ~0x30;        // halt Timer A3
    TIMER_A3->CTL = 0x200;          // SMCLK
    TIMER_A3->CCTL[0] = 0x4910;      // rising, sync, capture, interrupt
    TIMER_A3->CCTL[1] = 0x4910;
    TIMER_A3->EX0 &= ~0x7;        // input clock divider /1

    NVIC->IP[3] = (NVIC->IP[3] & 0xFF0000FF) | 0x00020300; // priority 2 and 3
    
    //? enabling something different??
    NVIC->ISER[0] = (1 << 13) | (1 << 14); // enable 14 and 15


    // Start Timer A3 in continuous mode with clear
    TIMER_A3->CTL |= 0x0024;         // TACLR | MC__CONTINUOUS
}

// The rising edge of P10.4 will cause an interrupt
void TA3_0_IRQHandler(void) {
    TIMER_A3->CCTL[0] &= ~0x0001;    // ack
    capture_task0(TIMER_A3->CCR[0]);
}

// The rising edge of P10.5 will cause an interrupt
void TA3_N_IRQHandler(void) {
    TIMER_A3->CCTL[1] &= ~0x0001;  // ack
    capture_task1(TIMER_A3->CCR[1]);
}

// #define PRESCALER_FACTOR 3
// static uint16_t prev_capture0 = 0;
// static uint16_t prev_capture1 = 0;
// // ISR for TA3 CCR0 (P10.4 rising edge)
// void TA3_0_IRQHandler(void) {
//     uint16_t current_capture = TIMER_A3->CCR[0];
//     uint16_t period = current_capture - prev_capture0;
//     prev_capture0 = current_capture;
    
//     TIMER_A3->CCTL[0] &= ~0x0001;    // Acknowledge interrupt
//     capture_task0(period * PRESCALER_FACTOR); // Apply prescaler
// }

// // ISR for TA3 CCR1 (P10.5 rising edge)
// void TA3_N_IRQHandler(void) {
//     if (TIMER_A3->CCTL[1] & 0x0001) { // Check CCIFG1
//         uint16_t current_capture = TIMER_A3->CCR[1];
//         uint16_t period = current_capture - prev_capture1;
//         prev_capture1 = current_capture;
        
//         TIMER_A3->CCTL[1] &= ~0x0001; // Acknowledge interrupt
//         capture_task1(period * PRESCALER_FACTOR); // Apply prescaler
//     }
// }