#include "blink.pio.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/time.h"

#include <math.h>

#include "config.h"
#include "stepper.h"

void stepper_init() {
    PIO pio = pio0;
    uint offset = pio_add_program(pio, &oscillate_program);
    
    start_pulse(pio, SM_A, offset, PULSE_PIN_A, 1);
    start_pulse(pio, SM_B, offset, PULSE_PIN_B, 1);

    gpio_init(DIR_PIN_A);
    gpio_init(DIR_PIN_B);
    gpio_set_dir(DIR_PIN_A, GPIO_OUT);
    gpio_set_dir(DIR_PIN_B, GPIO_OUT);
}

void set_speeds(double a, double b) {
    update_sm(pio0, SM_A, DIR_PIN_A, a);
    update_sm(pio0, SM_B, DIR_PIN_B, b);
}

void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
    oscillate_program_init(pio, sm, offset, pin);
    pio->txf[sm] = 0;
    pio_sm_set_enabled(pio, sm, true);
}

void update_sm(PIO pio, uint sm, const uint pin ,double v) {
    double u_v = fabs(v);
    if(u_v > 0.0005)
        pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
    else
        pio->txf[sm] = 0;
    gpio_put(pin, v < 0);
}