#pragma once
#include "hardware/pio.h"
#include "pulser.pio.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 = 1000;
    bool done_flag0;
    bool done_flag1;

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

    void irq_handler0();
    void irq_handler1();

public:
    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);

    bool wait0(int timeout_ms);
    bool wait1(int timeout_ms);
};