#include <stdio.h>

#include <hardware/gpio.h>
#include <pico/stdlib.h>
#include <pico/binary_info.h>

#include "pio_sbw.h"
#include "tap.h"
#include "msp430dbg.h"

void printbuf(const uint8_t* buf, size_t size) {
    for (size_t i = 0; i < size; ++i)
        printf("%02x%c", buf[i], i % 16 == 15 ? '\n' : ' ');
}

int main() {
    gpio_init(PINOUT_SBW_TCK);
    gpio_set_function(PINOUT_SBW_TCK, GPIO_FUNC_SIO);
    gpio_set_dir(PINOUT_SBW_TCK, true);
    gpio_put(PINOUT_SBW_TCK, true);

    stdio_init_all();

    /*sbw_preinit();
    printf("%s\n", "preinited");

    bool s = sbw_init();
    printf("%s\n", s ? "inited" : "failure");*/

    /*uint8_t tdi = 0x0f, tdo = 0;
    sbw_sequence(8, true, &tdi, &tdo);
    printf("seq done, tdo=%02x\n", tdo);

    uint8_t tms = 0xf0;
    sbw_tms_sequence(8, true, &tms);
    printf("tmsseq done\n");

    printf("%s tclk:\n", "set");
    sbw_set_tclk();
    printf("%s tclk:\n", "clr");
    sbw_clr_tclk();
    printf("%s tclk:\n", "set");
    sbw_set_tclk();
    printf("%s tclk:\n", "clr");
    sbw_clr_tclk();

    printf("doing a tclk burst now!\n");
    sbw_tclk_burst(16);
    printf("done.\n");*/

    /*sbw_tap_reset();
    sbw_check_fuse();

    uint8_t id = sbw_tap_shift_ir(msp430_ir_bypass);
    printf("JTAG ID=%02x\n", id);

	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit);
	uint16_t dr = sbw_tap_read_dr();
	printf("dr.0 = %04x\n", dr);
	sbw_clr_tclk(); // FIXME: looks very bad
	sbw_set_tclk(); // FIXME: looks very bad
	dr = sbw_tap_read_dr();
	printf("dr.1 = %04x\n", dr);
	sbw_clr_tclk(); // FIXME: looks very bad
	sbw_set_tclk(); // FIXME: looks very bad
	dr = sbw_tap_read_dr();
	printf("dr.2 = %04x\n", dr);
	sbw_clr_tclk(); // FIXME: looks very bad
	sbw_set_tclk(); // FIXME: looks very bad

	sbw_tap_shift_ir(msp430_ir_data_16bit);
	dr = sbw_tap_shift_dr(0x3fff); // "jmp ."
	printf("dr.10 = %04x\n", dr);
	sbw_clr_tclk(); // looks ok here?
	sbw_tap_shift_ir(msp430_ir_ctrl_sig_16bit); // FIXME: a
	dr = sbw_tap_shift_dr(0x2409); // JTAG halt // FIXME: a
	printf("dr.11 = %04x\n", dr);
	sbw_set_tclk(); // looks ok here?

	printf("test done\n");*/

	uint32_t initv = msp430_device_get(200e3);
	printf("init -> %08lx\n", initv);

    return 0;
}