.program st506_step
.in 32 left auto 32

// x: current track number
// y: last track number

.wrap_target
	// wait for STEP=1
	wait 1 pin 0
	// check DIR_IN
	jmp pin dir_in

dir_out:
	// if on track 0, don't step
	jmp !x push_track
	// if not on track 0, step out
	jmp x-- push_track

dir_in:
	// if not on last track, step in
	jmp x!=y dir_in_step
	// if on last track, don't step
	jmp push_track
dir_in_step:
	// increment the track number
	mov x, ~x
	jmp x-- dir_in_step_done
dir_in_step_done:
	mov x, ~x

push_track:
	// send the new track number
	in x, 32
	// wait for STEP=0
	wait 0 pin 0
.wrap

% c-sdk {
static inline void st506_step_program_init(PIO pio, uint sm, uint offset, uint pin_step, uint pin_dir_in) {
	pio_sm_config c = st506_step_program_get_default_config(offset);
	sm_config_set_clkdiv_int_frac8(&c, 1, 0);
	sm_config_set_in_pin_base(&c, pin_step);
	sm_config_set_jmp_pin(&c, pin_dir_in);
	pio_sm_init(pio, sm, offset, &c);
}

static int st506_step_chan;

static inline void st506_step_program_start(PIO pio, uint sm, uint *track_cur, uint track_last) {
	// set x, 0
	// pio_sm_exec_wait_blocking(pio, sm, pio_encode_set(pio_x, 0));

	// pull block
	pio_sm_put(pio, sm, *track_cur);
	pio_sm_exec_wait_blocking(pio, sm, pio_encode_pull(false, true));
	// out x, 32
	pio_sm_exec_wait_blocking(pio, sm, pio_encode_out(pio_x, 32));

	// pull block
	pio_sm_put(pio, sm, track_last);
	pio_sm_exec_wait_blocking(pio, sm, pio_encode_pull(false, true));
	// out y, 32
	pio_sm_exec_wait_blocking(pio, sm, pio_encode_out(pio_y, 32));

	pio_sm_set_enabled(pio, sm, true);

	int chan = st506_step_chan = dma_claim_unused_channel(true);

	dma_channel_config config = dma_channel_get_default_config(chan);
	channel_config_set_transfer_data_size(&config, DMA_SIZE_32);
	channel_config_set_read_increment(&config, false);
	channel_config_set_write_increment(&config, false);
	channel_config_set_dreq(&config, pio_get_dreq(pio, sm, false));
	dma_channel_configure(
		chan,
		&config,
		// write to 'track_cur' pointer
		track_cur,
		// read from PIO's RX FIFO
		&pio->rxf[sm],
		// transfer "forever"
		0xFFFFFFFF,
		true
	);
}
%}