#include "actuator.h"

void actuator_init(void) {
    // Initialize PWM pins for actuators
    rcu_periph_clock_enable(ACTUATOR_CH1_PWM_RCU);
    rcu_periph_clock_enable(ACTUATOR_CH2_PWM_RCU);
    rcu_periph_clock_enable(ACTUATOR_CH1_GPIO_RCU);
    rcu_periph_clock_enable(ACTUATOR_CH2_GPIO_RCU);
    // rcu_periph_clock_enable(PWM3_RCU);
    // rcu_periph_clock_enable(PWM4_RCU);

    rcu_periph_clock_enable(ACTUATOR_PWM_TIMER_RCU); // Enable clock for TIMER0

    gpio_mode_set(ACTUATOR_CH1_PWM_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, ACTUATOR_CH1_PWM_PIN);
    gpio_output_options_set(ACTUATOR_CH1_PWM_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, ACTUATOR_CH1_PWM_PIN);
    gpio_af_set(ACTUATOR_CH1_PWM_PORT, GPIO_AF_2, ACTUATOR_CH1_PWM_PIN);

    gpio_mode_set(ACTUATOR_CH2_PWM_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, ACTUATOR_CH2_PWM_PIN);
    gpio_output_options_set(ACTUATOR_CH2_PWM_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, ACTUATOR_CH2_PWM_PIN);
    gpio_af_set(ACTUATOR_CH2_PWM_PORT, GPIO_AF_2, ACTUATOR_CH2_PWM_PIN);

    gpio_mode_set(ACTUATOR_CH1_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, ACTUATOR_CH1_GPIO_PIN);
    gpio_output_options_set(ACTUATOR_CH1_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, ACTUATOR_CH1_GPIO_PIN);
    gpio_bit_reset(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN); // Set initial state to LOW

    gpio_mode_set(ACTUATOR_CH2_GPIO_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, ACTUATOR_CH2_GPIO_PIN);
    gpio_output_options_set(ACTUATOR_CH2_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, ACTUATOR_CH2_GPIO_PIN);
    gpio_bit_reset(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN); // Set initial state to LOW

    // Additional actuator initialization code can be added here

    // HSI = 72MHz , Freq Timer = 72MHz / (Prescaler + 1) / (Period + 1)
    timer_deinit(ACTUATOR_PWM_TIMER);
    timer_parameter_struct timer_initpara;
    timer_initpara.prescaler            = 2; // 72MHz  / (2+1) = 24MHz
    timer_initpara.alignedmode          = TIMER_COUNTER_EDGE;
    timer_initpara.counterdirection     = TIMER_COUNTER_UP;
    timer_initpara.period               = 999; // 24MHz / (999 + 1) = 24kHz
    timer_initpara.clockdivision        = TIMER_CKDIV_DIV1;
    timer_initpara.repetitioncounter    = 0;
    timer_init(ACTUATOR_PWM_TIMER, &timer_initpara);

    timer_oc_parameter_struct timer_ocinitpara;
    timer_ocinitpara.ocpolarity          = TIMER_OC_POLARITY_HIGH;
    timer_ocinitpara.outputstate        = TIMER_CCX_ENABLE;
    timer_ocinitpara.outputnstate       = TIMER_CCXN_DISABLE;
    timer_ocinitpara.ocpolarity         = TIMER_OC_POLARITY_HIGH;
    timer_ocinitpara.ocnpolarity        = TIMER_OCN_POLARITY_HIGH;
    timer_ocinitpara.ocidlestate        = TIMER_OC_IDLE_STATE_LOW;
    timer_ocinitpara.ocnidlestate       = TIMER_OCN_IDLE_STATE_LOW;

    // CH0
    timer_channel_output_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, &timer_ocinitpara);
    timer_channel_output_mode_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, TIMER_OC_SHADOW_ENABLE);
    timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, 0);

    // CH1
    timer_channel_output_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, &timer_ocinitpara);
    timer_channel_output_mode_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, TIMER_OC_SHADOW_ENABLE);
    timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, 0);

    timer_auto_reload_shadow_enable(ACTUATOR_PWM_TIMER);
    timer_primary_output_config(ACTUATOR_PWM_TIMER, ENABLE);
    timer_enable(ACTUATOR_PWM_TIMER);
}

void actuator_output(actuator_channel_t channel, uint16_t duty, actuator_direction_t direction) {

    if (duty > ACTUARTOR_PERIOD_MAX) duty = ACTUARTOR_PERIOD_MAX; // Limit duty cycle to max value

    if (direction != DIRECTION_PUSH && direction != DIRECTION_PULL) {
        // Invalid direction, handle error if necessary
        return;
    }

    // Set PWM output for the specified actuator channel
    // This is a placeholder; actual implementation will depend on the PWM configuration
    // For example, setting the duty cycle to a specific value to activate the actuator
    if (channel == ACTUATOR_CH1) {
        if (direction == DIRECTION_PUSH) {
            SEGGER_RTT_printf(0, "[ACTUATOR] Actuator CH1 PUSH with duty %d\n", duty);
            gpio_bit_reset(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN); // Set direction LOW
            timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, duty);
        } else if (direction == DIRECTION_PULL) {
            SEGGER_RTT_printf(0, "[ACTUATOR] Actuator CH1 PULL with duty %d\n", duty);
            gpio_bit_set(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN);   // Set direction HIGH
            timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, (ACTUARTOR_PERIOD_MAX - duty));
        }
    } else if (channel == ACTUATOR_CH2) {
        if (direction == DIRECTION_PUSH) {
            SEGGER_RTT_printf(0, "[ACTUATOR] Actuator CH2 PUSH with duty %d\n", duty);
            gpio_bit_reset(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN); // Set direction LOW
            timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, duty);
        } else if (direction == DIRECTION_PULL) {
            SEGGER_RTT_printf(0, "[ACTUATOR] Actuator CH2 PULL with duty %d\n", duty);
            gpio_bit_set(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN);   // Set direction HIGH
            timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, (ACTUARTOR_PERIOD_MAX - duty));
        }
    } else {
        // Invalid channel, handle error if necessary
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, 0);    // Ensure CH1 is off
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, 0);    // Ensure CH2 is off
        gpio_bit_reset(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN); // Set CH1 direction LOW
        gpio_bit_reset(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN); // Set CH2 direction LOW
    }
}

void actuator_stop(actuator_channel_t channel) {
    if(channel == ACTUATOR_CH1) {
        SEGGER_RTT_printf(0, "[ACTUATOR] Stopping Actuator CH1\n");
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, ACTUARTOR_PERIOD_MAX);    // Ensure CH1 is off
        gpio_bit_set(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN); // Set CH1 direction High
    } else if(channel == ACTUATOR_CH2) {
        SEGGER_RTT_printf(0, "[ACTUATOR] Stopping Actuator CH2\n");
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, ACTUARTOR_PERIOD_MAX);    // Ensure CH2 is off
        gpio_bit_set(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN); // Set CH2 direction High
    } else {
        // Invalid channel, handle error if necessary
        SEGGER_RTT_printf(0, "[ACTUATOR] Invalid channel specified for stop: %d\n", channel);
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH1_PWM_TIMER_CH, ACTUARTOR_PERIOD_MAX);    // Ensure CH1 is off
        timer_channel_output_pulse_value_config(ACTUATOR_PWM_TIMER, ACTUATOR_CH2_PWM_TIMER_CH, ACTUARTOR_PERIOD_MAX);    // Ensure CH2 is off
        gpio_bit_set(ACTUATOR_CH1_GPIO_PORT, ACTUATOR_CH1_GPIO_PIN); // Set CH1 direction High
        gpio_bit_set(ACTUATOR_CH2_GPIO_PORT, ACTUATOR_CH2_GPIO_PIN); // Set CH2 direction High
    }
}