#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"
#include "tool78_hw.h"

void piotest(void);

/*static uint16_t DATA_text[0x24] = {
    0xc232,0x43c2,0x0000,0x4031,0x02fe,0x40f2,0x00ff,0x002a,0x40b2,0x5a10,0x0120,
    0x43d2,0x0000,0xd232,0xd032,0x0018,0x3ffd,0xc3d2,0x0002,0xd3d2,0x0000,0xe3d2,
    0x0029,0x4130,0x1300,0x1300,0x1300,0x1300,0x1300,0x1300,0x12b0,0xf822,0x1300,
    0x1300,0x1300,0x3fdc
};
static uint16_t DATA_vectors[0x10] = {
    0xf830,0xf830,0xf832,0xf834,0xf836,0xf830,0xf830,0xf830,
    0xf838,0xf83a,0xf83c,0xf842,0xf830,0xf830,0xf844,0xf846
};

// 2k/8k flash, 256b RAM
static uint16_t dumpmem_ram[256>>1];
static uint16_t dumpmem_flash[8192>>1];*/

static void test_sbw(void) {
    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);

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

	/*msp430_memory_read_block(0x0f00,  16>>1, dumpmem_ram);
	printf("info dump:\n");
	for (size_t i = 0; i < ( 16>>1); i += 4) {
		printf("%04zx: %04x %04x %04x %04x\n",
			0x0f00+(i<<1), dumpmem_ram[i+0], dumpmem_ram[i+1],
			               dumpmem_ram[i+2], dumpmem_ram[i+3]
		);
	}

	msp430_memory_read_block(0x0200, 256>>1, dumpmem_ram);

	printf("RAM dump:\n");
	for (size_t i = 0; i < (256>>1); i += 4) {
		printf("%04zx: %04x %04x %04x %04x\n",
			0x0200+(i<<1), dumpmem_ram[i+0], dumpmem_ram[i+1],
			               dumpmem_ram[i+2], dumpmem_ram[i+3]
		);
	}

	msp430_memory_read_block(0xe000, 8192>>1, dumpmem_flash);

	printf("flash dump:\n");
	for (size_t i = 0; i < (8192>>1); i += 4) {
		printf("%04zx: %04x %04x %04x %04x\n",
			0xe000+(i<<1), dumpmem_flash[i+0], dumpmem_flash[i+1],
			               dumpmem_flash[i+2], dumpmem_flash[i+3]
		);
	}*/

	/*printf("flashing text...\n");
	for (size_t i = 0; i < 0x24; ++i) {
		printf("%zu : %04x\n", DATA_text[i]);
		msp430_flash_write(0xf800 + (i*2), 1, &DATA_text[i]);
	}
	printf("flashing vectors...\n");
	for (size_t i = 0; i < 0x10; ++i) {
		msp430_flash_write(0xffe0 + (i*2), 1, &DATA_vectors[i]);
	}*/

	/*printf("dumping for check...\n");
	msp430_memory_read_block(0xf800, 0x0800>>1, dumpmem_flash);

	printf("flash dump:\n");
	for (size_t i = 0; i < (2048>>1); i += 4) {
		printf("%04zx: %04x %04x %04x %04x\n",
			0xf800+(i<<1), dumpmem_flash[i+0], dumpmem_flash[i+1],
			               dumpmem_flash[i+2], dumpmem_flash[i+3]
		);
	}*/

	/*msp430_memory_write8(0x0024, 0x00); // P2IE =0x00 (no irq)
	msp430_memory_write8(0x002e, 0x00); // P2SEL=0x00 (gpio)
	msp430_memory_write8(0x0042, 0x00); // P2SEL2=0   (gpio)
	msp430_memory_write8(0x002a, 0xff); // P2DIR=0xff (all out)
	msp430_memory_write8(0x0029, 0x01); // P2OUT=0x01 (P2.0 hi)*/

	msp430_device_release(0xfffe/*0xf800*/);
}

static uint8_t buf[32];

static void tool78_testtest() {
	struct tool78_hw* thw = &tool78_hw_test_uart2;

	printf("initing...\n");
	bool r = thw->init();
	printf("init: %s\n", r ? "ok" : "nope");

	size_t rrr = thw->send(7, (const uint8_t*)"hello\r\n", 0);
	printf("sent %zu\n", rrr);

	while (true) {
		size_t n = thw->recv(31, buf, 0);
		buf[n] = 0;
		if (n) {
			printf("got %zu: '%s'\n", n, (const char*)buf);
			thw->send(n, buf, 0);
		}
	}
}

int main() {
	stdio_init_all();
	tool78_testtest();
	//piotest();
	return 0;
}