#include "Clock.h"
#include "msp.h"
#include <stdint.h>

//=============================================================================
// 1. 상수 정의 (Constants)
//=============================================================================
// (!!주의!!) SMCLK = 12MHz 기준입니다.
#define RPM_MAGIC_NUMBER 2000000
#define TIMER_A0_PWM_PERIOD_TICKS 15000
#define STALL_TIMER_PERIOD 50000

#define LEFT_PWM_PIN BIT7
#define LEFT_DIR_PIN BIT5
#define LEFT_SLEEP_PIN BIT7

#define RIGHT_PWM_PIN BIT6
#define RIGHT_DIR_PIN BIT4
#define RIGHT_SLEEP_PIN BIT6

#define TURN_30_DEG_PULSES 60
#define TURN_SPEED 2000

static volatile uint32_t g_left_pulse_period = 0;
static volatile uint32_t g_right_pulse_period = 0;
static volatile uint16_t g_last_left_capture = 0;
static volatile uint16_t g_last_right_capture = 0;
static volatile uint32_t g_left_pulse_count = 0;
static volatile uint32_t g_right_pulse_count = 0;

static volatile uint8_t g_left_new_data = 0;
static volatile uint8_t g_right_new_data = 0;

/*
    FUNCTION DECLARATIONS
*/
void Motor_Init(void);
void Motor_Left_Forward(uint16_t speed);
void Motor_Left_Backward(uint16_t speed);
void Motor_Right_Forward(uint16_t speed);
void Motor_Right_Backward(uint16_t speed);
void Motor_Left_Stop(void);
void Motor_Right_Stop(void);
void Motor_Stop_All(void);

void rpm_tachometer_init(void);
uint32_t get_left_rpm(void);
uint32_t get_right_rpm(void);
void reset_pulse_counts(void);
uint32_t get_left_pulse_count(void);
uint32_t get_right_pulse_count(void);

static void capture_init_timer_A3(void);
static void Timer_A2_Stall_Init(void);
void Rotate_30_Degrees(void);

/*
    MAIN FUNCTION
*/
int main(void) {
    WDT_A->CTL = WDT_A_CTL_PW | WDT_A_CTL_HOLD;

    Clock_Init48MHz();

    Motor_Init();
    rpm_tachometer_init();

    __enable_irq();

    __delay_cycles(12000000);

    Rotate_30_Degrees();

    while (1) {
        Clock_Delay1ms(1000);
    }
}

/*
    IMPLEMENTATIONS 30DEG ROT. FUNCTION
*/
void Rotate_30_Degrees(void) {
    reset_pulse_counts();
    Motor_Left_Forward(TURN_SPEED);
    Motor_Right_Backward(TURN_SPEED);

    uint8_t left_reached = 0;
    uint8_t right_reached = 0;

    while (left_reached == 0 || right_reached == 0) {
        if (left_reached == 0) {
            if (get_left_pulse_count() >= TURN_30_DEG_PULSES) {
                Motor_Left_Stop();
                left_reached = 1;
            }
        }

        if (right_reached == 0) {
            if (get_right_pulse_count() >= TURN_30_DEG_PULSES) {
                Motor_Right_Stop();
                right_reached = 1;
            }
        }
        __delay_cycles(1000);
    }

    Motor_Stop_All();
}

/*
    IMPLEMENTATION MOTOR RELATIVE FUNCTIONS
*/
void Motor_Init(void) {
    P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
    P3->DIR |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
    P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
    P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);

    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
    P5->DIR |= (LEFT_DIR_PIN | RIGHT_DIR_PIN);
    P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
    P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);

    P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
    P2->DIR |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);
    P2->SEL0 |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);
    P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);

    TIMER_A0->CTL = TIMER_A_CTL_SSEL__SMCLK |
                    TIMER_A_CTL_ID__1 |
                    TIMER_A_CTL_CLR;
    TIMER_A0->CCR[0] = TIMER_A0_PWM_PERIOD_TICKS;

    TIMER_A0->CCTL[3] = TIMER_A_CCTLN_OUTMOD_7;
    TIMER_A0->CCR[3] = 0;

    TIMER_A0->CCTL[4] = TIMER_A_CCTLN_OUTMOD_7;
    TIMER_A0->CCR[4] = 0;

    TIMER_A0->CTL |= TIMER_A_CTL_MC__UP;
}

void Motor_Left_Forward(uint16_t speed) {
    if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
    P5->OUT &= ~LEFT_DIR_PIN;
    TIMER_A0->CCR[4] = speed;
}

void Motor_Left_Backward(uint16_t speed) {
    if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
    P5->OUT |= LEFT_DIR_PIN;
    TIMER_A0->CCR[4] = speed;
}

void Motor_Right_Forward(uint16_t speed) {
    if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
    P5->OUT &= ~RIGHT_DIR_PIN;
    TIMER_A0->CCR[3] = speed;
}

void Motor_Right_Backward(uint16_t speed) {
    if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
    P5->OUT |= RIGHT_DIR_PIN;
    TIMER_A0->CCR[3] = speed;
}

void Motor_Left_Stop(void) {
    TIMER_A0->CCR[4] = 0;
}

void Motor_Right_Stop(void) {
    TIMER_A0->CCR[3] = 0;
}

void Motor_Stop_All(void) {
    Motor_Left_Stop();
    Motor_Right_Stop();
}

/*
    IMPLEMENTATION RPM FUNC.
*/
void rpm_tachometer_init(void) {
    capture_init_timer_A3();
    Timer_A2_Stall_Init();
}

uint32_t get_left_rpm(void) {
    uint32_t period;
    __disable_irq();
    period = g_left_pulse_period;
    __enable_irq();

    if (period == 0) return 0;
    return RPM_MAGIC_NUMBER / period;
}

uint32_t get_right_rpm(void) {
    uint32_t period;
    __disable_irq();
    period = g_right_pulse_period;
    __enable_irq();

    if (period == 0) return 0;
    return RPM_MAGIC_NUMBER / period;
}

void reset_pulse_counts(void) {
    __disable_irq();
    g_left_pulse_count = 0;
    g_right_pulse_count = 0;
    __enable_irq();
}
uint32_t get_left_pulse_count(void) {
    uint32_t count;
    __disable_irq();
    count = g_left_pulse_count;
    __enable_irq();
    return count;
}
uint32_t get_right_pulse_count(void) {
    uint32_t count;
    __disable_irq();
    count = g_right_pulse_count;
    __enable_irq();
    return count;
}

static void Timer_A2_Stall_Init(void) {
    TIMER_A2->CTL = TIMER_A_CTL_SSEL__SMCLK |
                    TIMER_A_CTL_ID__2 |
                    TIMER_A_CTL_MC__STOP;
    TIMER_A2->CCTL[0] = TIMER_A_CCTLN_CCIE;
    TIMER_A2->CCR[0] = (STALL_TIMER_PERIOD - 1);
    TIMER_A2->EX0 = TIMER_A_EX0_TAIDEX_5;

    NVIC->IP[3] = (NVIC->IP[3] & 0xFFFFFF00) | 0x40;
    NVIC->ISER[0] = 1 << TA2_0_IRQn;

    TIMER_A2->CTL |= TIMER_A_CTL_MC__UP | TIMER_A_CTL_CLR;
}

void TA2_0_IRQHandler(void) {
    TIMER_A2->CCTL[0] &= ~TIMER_A_CCTLN_CCIFG;

    if (g_left_new_data == 0) g_left_pulse_period = 0;
    else
        g_left_new_data = 0;

    if (g_right_new_data == 0) g_right_pulse_period = 0;
    else
        g_right_new_data = 0;
}

static void capture_init_timer_A3(void) {
    TIMER_A3->CTL = TIMER_A_CTL_SSEL__SMCLK |
                    TIMER_A_CTL_MC__CONTINUOUS |
                    TIMER_A_CTL_ID__1 |
                    TIMER_A_CTL_CLR;
    P10->SEL0 |= BIT4;
    P10->SEL1 &= ~BIT4;
    P10->DIR &= ~BIT4;
    TIMER_A3->CCTL[0] = TIMER_A_CCTLN_CM_1 | TIMER_A_CCTLN_CCIS_0 |
                        TIMER_A_CCTLN_SCS | TIMER_A_CCTLN_CAP | TIMER_A_CCTLN_CCIE;
    P10->SEL0 |= BIT5;
    P10->SEL1 &= ~BIT5;
    P10->DIR &= ~BIT5;
    TIMER_A3->CCTL[1] = TIMER_A_CCTLN_CM_1 | TIMER_A_CCTLN_CCIS_0 |
                        TIMER_A_CCTLN_SCS | TIMER_A_CCTLN_CAP | TIMER_A_CCTLN_CCIE;
    NVIC->IP[3] = (NVIC->IP[3] & 0x00FFFFFF) | 0x40000000;
    NVIC->ISER[0] = 1 << TA3_0_IRQn;
    NVIC->IP[4] = (NVIC->IP[4] & 0xFFFFFF00) | 0x40;
    NVIC->ISER[0] = 1 << TA3_N_IRQn;
}

void TA3_0_IRQHandler(void) {
    TIMER_A3->CCTL[0] &= ~TIMER_A_CCTLN_CCIFG;
    uint16_t current_capture = TIMER_A3->CCR[0];
    uint32_t period = (current_capture > g_last_right_capture) ? (current_capture - g_last_right_capture) : ((0xFFFF - g_last_right_capture) + current_capture + 1);
    g_right_pulse_period = period;
    g_right_new_data = 1;
    g_last_right_capture = current_capture;
    g_right_pulse_count++;
}

void TA3_N_IRQHandler(void) {
    if (TIMER_A3->CCTL[1] & TIMER_A_CCTLN_CCIFG) {
        TIMER_A3->CCTL[1] &= ~TIMER_A_CCTLN_CCIFG;
        uint16_t current_capture = TIMER_A3->CCR[1];
        uint32_t period = (current_capture > g_last_left_capture) ? (current_capture - g_last_left_capture) : ((0xFFFF - g_last_left_capture) + current_capture + 1);
        g_left_pulse_period = period;
        g_left_new_data = 1;
        g_last_left_capture = current_capture;
        g_left_pulse_count++;
    }
}