#pragma once
#include "hardware/pio.h"
#include "pulser.pio.h"

#include "FreeRTOS.h"
#include "semphr.h"

class TMC2209_step_dual {
private:
    uint step0_pin;
    uint step1_pin;
    uint dir0_pin;
    uint dir1_pin;
    uint enable0_pin;
    uint enable1_pin;

    const int sm_num_divider0 = 0;
    const int sm_num_divider1 = 1;
    const int sm_num_counter0 = 2;
    const int sm_num_counter1 = 3;

    PIO pio_core;

    pio_sm_config sm_config_divider0;
    pio_sm_config sm_config_counter0;
    pio_sm_config sm_config_divider1;
    pio_sm_config sm_config_counter1;

    uint pulser_program_offset;
    const uint subprogram_offset_divider = 0;
    const uint subprogram_offset_counter = 6;

    const float base_clock_freq_khz = 500.0;

    const uint32_t sleep_time_us = 500;

    SemaphoreHandle_t xDoneSemaphore0;
    SemaphoreHandle_t xDoneSemaphore1;

    uint remaining_step_count0;
    uint remaining_step_count1;

protected:
    void init_pio();
    void init_gpio();


public:
    void irq_handler0();
    void irq_handler1();

    TMC2209_step_dual(
        uint step0_pin, 
        uint step1_pin, 
        uint dir0_pin, 
        uint dir1_pin, 
        uint enable0_pin, 
        uint enable1_pin, 
        PIO pio_core
    );

    void set_conf0(uint frequency, bool dir);
    void set_conf1(uint frequency, bool dir);

    void pulse0(uint n);
    void pulse1(uint n);

    void pulse0_int(int n);
    void pulse1_int(int n);

    void wait0(int timeout_ms);
    void wait1(int timeout_ms);

    uint get_remaining_steps0();
    uint get_remaining_steps1();
};

extern TMC2209_step_dual * g_step_driver_instance;
void step_irq0_handler();
void step_irq1_handler();