#include "step.hpp"
#include "pico/stdio.h"
#include "hardware/pio.h"
#include "hardware/irq.h"
#include <pico/time.h>

#include <math.h>

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

TMC2209_step_dual * g_step_driver_instance;

void step_irq0_handler() {
    g_step_driver_instance->irq_handler0();
};

void step_irq1_handler() {
    g_step_driver_instance->irq_handler1();
};

TMC2209_step_dual::TMC2209_step_dual(
    uint step0_pin, 
    uint step1_pin, 
    uint dir0_pin, 
    uint dir1_pin, 
    uint enable0_pin, 
    uint enable1_pin, 
    PIO pio_core
) {
    this->step0_pin = step0_pin;
    this->step1_pin = step1_pin;
    this->dir0_pin = dir0_pin;
    this->dir1_pin = dir1_pin;
    this->enable0_pin = enable0_pin;
    this->enable1_pin = enable1_pin;
    this->pio_core = pio_core;

    g_step_driver_instance = this;

    this->xDoneSemaphore0 = xSemaphoreCreateBinary();
    this->xDoneSemaphore1 = xSemaphoreCreateBinary();

    init_gpio();
    init_pio();
};

void TMC2209_step_dual::init_gpio() {
    gpio_init(this->enable0_pin);
    gpio_init(this->enable1_pin);
    gpio_set_dir(this->enable0_pin, GPIO_OUT);
    gpio_set_dir(this->enable1_pin, GPIO_OUT);
    gpio_put(this->enable0_pin, 0);
    gpio_put(this->enable1_pin, 0);
    vTaskDelay(1 / portTICK_PERIOD_MS);

    gpio_init(this->dir0_pin);
    gpio_init(this->dir1_pin);
    gpio_set_dir(this->dir0_pin, GPIO_OUT);
    gpio_set_dir(this->dir1_pin, GPIO_OUT);    
    gpio_put(this->dir0_pin, 1);
    gpio_put(this->dir1_pin, 1);
    vTaskDelay(1 / portTICK_PERIOD_MS);
};

void TMC2209_step_dual::init_pio() {
    pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, false);
    pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, false);
    pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, false);
    pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, false);

    this->pulser_program_offset = pio_add_program(this->pio_core, &pulser_program);

    this->sm_config_divider0 = pulser_program_get_default_config(this->pulser_program_offset);
    this->sm_config_divider1 = pulser_program_get_default_config(this->pulser_program_offset);
    this->sm_config_counter0 = pulser_program_get_default_config(this->pulser_program_offset);
    this->sm_config_counter1 = pulser_program_get_default_config(this->pulser_program_offset);

    pio_gpio_init(this->pio_core, this->step0_pin);
    pio_gpio_init(this->pio_core, this->step1_pin);

    pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter0, this->step0_pin, 1, true);
    pio_sm_set_consecutive_pindirs(this->pio_core, this->sm_num_counter1, this->step1_pin, 1, true);

    sm_config_set_set_pins(&this->sm_config_counter0, this->step0_pin, 1);
    sm_config_set_set_pins(&this->sm_config_counter1, this->step1_pin, 1);

    pio_sm_init(
        this->pio_core, 
        this->sm_num_divider0, 
        this->pulser_program_offset + this->subprogram_offset_divider, 
        &this->sm_config_divider0
    );
    pio_sm_init(
        this->pio_core, 
        this->sm_num_divider1, 
        this->pulser_program_offset + this->subprogram_offset_divider, 
        &this->sm_config_divider1
    );
    pio_sm_init(
        this->pio_core, 
        this->sm_num_counter0, 
        this->pulser_program_offset + this->subprogram_offset_counter, 
        &this->sm_config_counter0
    );
    pio_sm_init(
        this->pio_core, 
        this->sm_num_counter1, 
        this->pulser_program_offset + this->subprogram_offset_counter, 
        &this->sm_config_counter1
    );

    float clock_div = ((float)(SYS_CLK_KHZ))/(base_clock_freq_khz);

    pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider0, clock_div);
    pio_sm_set_clkdiv(this->pio_core, this->sm_num_divider1, clock_div);
    pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter0, 1.0);
    pio_sm_set_clkdiv(this->pio_core, this->sm_num_counter1, 1.0);

    pio_sm_set_enabled(this->pio_core, this->sm_num_divider0, true);
    pio_sm_set_enabled(this->pio_core, this->sm_num_divider1, true);
    pio_sm_set_enabled(this->pio_core, this->sm_num_counter0, true);
    pio_sm_set_enabled(this->pio_core, this->sm_num_counter1, true);

    pio_set_irq0_source_enabled(this->pio_core, pis_interrupt2, true);
    pio_set_irq1_source_enabled(this->pio_core, pis_interrupt3, true);

    if(this->pio_core == pio0) {
        irq_set_exclusive_handler(PIO0_IRQ_0, step_irq0_handler);
        irq_set_exclusive_handler(PIO0_IRQ_1, step_irq1_handler);

        irq_set_enabled(PIO0_IRQ_0, true);
        irq_set_enabled(PIO0_IRQ_1, true);
    } else if(this->pio_core == pio1) {
        irq_set_exclusive_handler(PIO1_IRQ_0, step_irq0_handler);
        irq_set_exclusive_handler(PIO1_IRQ_1, step_irq1_handler);

        irq_set_enabled(PIO1_IRQ_0, true);
        irq_set_enabled(PIO1_IRQ_1, true);
    }

    set_conf0(1000, 0);
    set_conf1(1000, 0);
};

void TMC2209_step_dual::set_conf0(uint frequency, bool dir) {
    uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6;
    
    gpio_put(this->dir0_pin, dir);
    pio_sm_put_blocking(
        this->pio_core, 
        this->sm_num_divider0, 
        cycles
    );
};

void TMC2209_step_dual::set_conf1(uint frequency, bool dir) {
    uint32_t cycles = (uint32_t)(base_clock_freq_khz / (((float)(frequency))/1000.0)) - 6;

    gpio_put(this->dir1_pin, dir);
    pio_sm_put_blocking(
        this->pio_core, 
        this->sm_num_divider1, 
        cycles
    );
};

void TMC2209_step_dual::pulse0(uint n) {
    if(n > 0)
        this->remaining_step_count0 = n;

    pio_sm_put_blocking(
        this->pio_core, this->sm_num_counter0, n
    );
};

void TMC2209_step_dual::pulse1(uint n) {
    if(n > 0)
        this->remaining_step_count1 = n;

    pio_sm_put_blocking(
        this->pio_core, this->sm_num_counter1, n
    );
};

void TMC2209_step_dual::pulse0_int(int n) {
    if(n < 0) {
        n = abs(n);
        gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
    }

    if(n != 0)
        this->remaining_step_count0 = n;

    pio_sm_put_blocking(
        this->pio_core, this->sm_num_counter0, n
    );
};

void TMC2209_step_dual::pulse1_int(int n) {
    if(n < 0) {
        n = abs(n);
        gpio_put(this->dir1_pin, gpio_get(this->dir1_pin) ^ 1);
    }

    if(n != 0)
        this->remaining_step_count0 = n;

    pio_sm_put_blocking(
        this->pio_core, this->sm_num_counter1, n
    );
};

void TMC2209_step_dual::irq_handler0() {
    uint n = pio_sm_get(this->pio_core, this->sm_num_counter0);
    if(n > 0)
        this->remaining_step_count0 = n;

    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(this->xDoneSemaphore0, &xHigherPriorityTaskWoken);
    
    pio_interrupt_clear(this->pio_core, 2);

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void TMC2209_step_dual::irq_handler1() {
    uint n = pio_sm_get(this->pio_core, this->sm_num_counter1);
    if(n > 0)
        this->remaining_step_count1 = n;

    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(this->xDoneSemaphore1, &xHigherPriorityTaskWoken);
    
    pio_interrupt_clear(this->pio_core, 3);

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void TMC2209_step_dual::wait0(int timeout_ms) {
    int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS;

    xSemaphoreTake(this->xDoneSemaphore0, t);
};

void TMC2209_step_dual::wait1(int timeout_ms) {
    int t = timeout_ms <= 0 ? portMAX_DELAY : timeout_ms / portTICK_PERIOD_MS;

    xSemaphoreTake(this->xDoneSemaphore1, t);
};

uint TMC2209_step_dual::get_remaining_steps0() {
    return this->remaining_step_count0;
};

uint TMC2209_step_dual::get_remaining_steps1() {
    return this->remaining_step_count1;
};

void TMC2209_step_dual::set_enable0(bool enable) {
    gpio_put(this->enable0_pin, !enable);
};

void TMC2209_step_dual::set_enable1(bool enable) {
    gpio_put(this->enable1_pin, !enable);
};