#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include <msp430.h>

#include "compiler.h"

void stdio_msp_init(void); // stdio_msp.c

static void setup_clocks(void) {
	CSCTL0_H = CSKEY_H;
	CSCTL1 = DCOFSEL_0/*3*/ | DCORSEL;
	CSCTL2 = SELA__VLOCLK | SELS__DCOCLK | SELM__DCOCLK;
	CSCTL3 = DIVA__1 | DIVS__1 | DIVM__1;
	CSCTL0_H = 0;
}
static void setup_io(void) {
	WDTCTL = WDTPW | WDTHOLD;   // Stop WDT

	// Configure GPIO
	P2SEL0 &= ~(BIT0 | BIT1);   // port 2.[01] to eUSCI_A0
	P2SEL1 |=   BIT0 | BIT1;

	P1OUT &= ~(BIT0|BIT1|BIT2|BIT3); // P1.[0-3]: status LED and LA trigger debug stuff
	P1DIR |= (BIT0|BIT1|BIT2|BIT3);

	PM5CTL0 &= ~LOCKLPM5;       // Disable the GPIO power-on default high-impedance mode
	                            // to activate previously configured port settings
}

// ---

__attribute__((__persistent__)) // put in FRAM (BSL clears RAM)
static uint32_t regbak[16]={0};
__attribute__((__persistent__))
static uint8_t stackbak[16]={0};

extern uint16_t curticks, curticks_;
__attribute__((__persistent__, __used__))
uint16_t curticks = 1;
__attribute__((__persistent__, __used__))
uint16_t curticks_ = 1;

extern uint16_t done_irq;
__attribute__((__persistent__, __used__))
uint16_t done_irq = 0;


extern uint16_t curaddr;
__attribute__((__persistent__, __used__))
uint16_t curaddr  = 0x1000;

/*extern uint16_t traceaddr;
__attribute__((__persistent__))
uint16_t traceaddr = 0x1000; // changeme*/

typedef void (*bsl_fn)(void);

#define START_HARD 1/*36000*/
/*#define START_SOFT 36990*/
#define END_CYC 0xffffu
/*#define SKIP_CYC_OFF 5*/

// DUMP_MODE == 0 => insn trace mode
#define DUMP_MODE 1
#define DUMP_CYC_OFF 14
#define DUMP_ADDR_START 0x1000
#define DUMP_ADDR_END   0x1800

void do_trace(void);
__attribute__((__no_inline__)) void do_trace(void) {
	// TODO: chain 2 timers for 32 bit tick number
	// TODO: continue instead of restarting?
	//       ^: mightn't be possible: instruction exec restarts after irq

	// init timer TA0
	__bic_SR_register(GIE);
#if DUMP_MODE
	//traceaddr = 0x1002;
	TA1CCR0 = DUMP_CYC_OFF;
#else
#ifdef SKIP_CYC_OFF
	if (curticks > SKIP_CYC_OFF) {
		done_irq = 0;
		TA1CCR0 = SKIP_CYC_OFF;
		curticks_ = curticks - SKIP_CYC_OFF /*+ 2*/;
	} else
#endif
	{
		done_irq = 1;
		TA1CCR0 = curticks;
	}
#endif
	//TA1CCTL0 |= CCIFG;
	//TA1CCTL0 &= ~(CCIE|CCIFG);
	TA1CCTL0 = CCIE;
	// exec bsl
	asm volatile(
		"mov.a #(__stack-8), sp\n"
		"mov.w #0xaaaa, r4\n"
		"mov.w #0xaaaa, r5\n"
		"mov.w #0xaaaa, r6\n"
		"mov.w #0xaaaa, r7\n"
		"mov.w #0xaaaa, r8\n"
		"mov.w #0xaaaa, r9\n"
		"mov.w #0xaaaa, r10\n"
		"mov.w #0xaaaa, r11\n"
		"mov.w #0x0002, r12\n"
		"mov.w #0xdead, r13\n"
		"mov.w #0xbeef, r14\n"
		"mov.w #0xaaaa, r15\n"

#if DUMP_MODE
		// extra 0x1002 magic
		"mov.w curaddr, sp\n"
#endif

		//TA1CTL = TASSEL__SMCLK | ID__1 | MC__UP | TACLR | TAIE;
		"mov.w #0x0216, TA1CTL\n"
		"eint\n"
		"call #0x1002\n" // CHANGEME (address to trace insn flow of)
		/*"nop\n"
		"nop\n"
		"nop\n"
		"nop\n"
		"nop\n"
		"nop\n"
		//"mov.w #0x1337, r8\n"
		"dint\nnop\n"
		"add.w #-1, r4\n"
		"add.w #1, r5\n"
		"add.w #2, r6\n"
		"add.w #4, r7\n"
		"add.w #8, r8\n"
		"add.w #-1, r4\n"
		"add.w #1, r5\n"
		"add.w #2, r6\n"
		"add.w #4, r7\n"
		"add.w #8, r8\n"
		"add.w #-1, r4\n"
		"add.w #1, r5\n"
		"add.w #2, r6\n"
		"add.w #4, r7\n"
		"add.w #8, r8\n"
		"add.w #-1, r4\n"
		"add.w #1, r5\n"
		"add.w #2, r6\n"
		"add.w #4, r7\n"
		"add.w #8, r8\n"
		"1: jmp 1b\n"*/
	);
	//while (1) ;
	__builtin_unreachable();
}

void do_collect(uint16_t* sp);
__attribute__((__used__, __no_inline__))
void do_collect(uint16_t* sp) {
	//P1OUT=0;
	TA1CTL &= ~(uint16_t)(TAIE|MC__UP);

	// 0x1bc2/4 and 0x1bd6/8 contain a bic #GIE, sr instruction! these should be 2 bytes in size
	/*if (pc16 == 0x1bc2 || pc16 == 0x1bc4 || pc16 == 0x1bd6 || pc16 == 0x1bd8) {
		sp[24] |= 
	}*/
	// 0x1b96 does a jump to ???? (probably from a memory location?)

#if DUMP_MODE
	uint16_t v1 = sp[2*(12-4)];
	uint16_t v2 = sp[2*(13-4)];

	if (!(curaddr & 0xf)) {
		iprintf("%04x:  ", curaddr);
	}
	iprintf("%02x %02x %02x %02x ",
		v1 & 0xff, (v1 >> 8) & 0xff,
		v2 & 0xff, (v2 >> 8) & 0xff
	);
	if ((curaddr & 0xf) == 0xc) {
		iprintf("\r\n");
	}
#else
#ifdef START_SOFT
	if (curticks < START_SOFT) goto next_iter;
#endif

	uint16_t sr = sp[24];
	uint32_t pc = sp[25];
	//uint16_t pc16 = pc;

	// general purpose registers
	for (int i = 0; i < 12; ++i) {
		uint32_t v = sp[(i*2)+0] | ((uint32_t)sp[(i*2)+1] << 16);
		regbak[i+4] = v;
	}

	pc |= ((uint32_t)(sr & 0xf000) << (16-12));
	sr &= 0xfff;

	sp = sp + 26;

	regbak[0] = pc;
	regbak[1] = sp;
	regbak[2] = sr;
	regbak[3] = 0; // cg

	memcpy(stackbak, sp, 16);

	// pc=r0 sp=r1 sr=r2 ; gp: r4-r15
	iprintf("- %u\r\n", curticks);
	iprintf("%05lx %05lx %03lx\r\n", regbak[0], regbak[1], regbak[2]);
	iprintf("%05lx %05lx %05lx %05lx %05lx %05lx %05lx %05lx %05lx %05lx %05lx %05lx\r\n",
		regbak[ 4], regbak[ 5], regbak[ 6], regbak[ 7], regbak[ 8], regbak[ 9],
		regbak[10], regbak[11], regbak[12], regbak[13], regbak[14], regbak[15]);
	iprintf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
			stackbak[0], stackbak[1], stackbak[2], stackbak[3],
			stackbak[4], stackbak[5], stackbak[6], stackbak[7],
			stackbak[8], stackbak[9], stackbak[10], stackbak[11],
			stackbak[12], stackbak[13], stackbak[14], stackbak[15]);
	//fflush(stdout);
#endif

next_iter:
#if DUMP_MODE
	if (curaddr == DUMP_ADDR_END) while(1); // start of info mem
	curaddr += 4;
#else
	if (curticks == END_CYC) while(1);
	++curticks;
#endif
	do_trace();
	__builtin_unreachable();
}

__attribute__((__interrupt__(TIMER1_A0_VECTOR), __naked__))
void Timer_A1_ISR(void) {
	asm volatile(
		".extern do_collect\n"
		".extern done_irq\n"
		".extern curticks\n"

		//"bis #0x3, P1OUT\n"
#if DUMP_MODE
		// get a stack we can do stuff with
		// (sp is currently in bsl space)
		"mov.a #(__stack-8), sp\n"
#endif

#if !DUMP_MODE && defined(SKIP_CYC_OFF)
		// do some hackery
		"cmp.w #0, done_irq\n"
		"jne .Lregular\n"

		//"add.w #4, P1OUT\n"

		// skip pc forward by 4
		"add.w #4, 2(sp)\n"
		// set/force GIE in sr
		"bis.w #8, 0(sp)\n"
		// done it now
		"mov.w #1, done_irq\n"
		// set timer counter for next IRQ for trace
		"mov.w curticks_, TA1CCR0\n"
		// clear TAIE, TAIFG, MC
		"bic.w #0x0033, TA1CTL\n"
		// clear irq flag, enable compare irq
		"mov.w #16, TA1CCTL0\n"
		// restart timer
		"mov.w #0x0216, TA1CTL\n"
		// continue as usual
		"reti\n"
#endif

	".Lregular:\n"
		//"add.w #8, P1OUT\n"
		"pushm.a #12, r15\n"
		"mov.a sp, r12\n"
		"call #do_collect\n"
		"popm.a #12, r15\n"
		"reti\n"
	);
}

__attribute__((__interrupt__(UNMI_VECTOR)))
void NMI_ISR(void) {
	SFRIFG1 &= ~NMIIE;
	SYSUNIV = 0;
	++P1OUT;
}

int main(void) {
	setup_io();
	setup_clocks();
	stdio_msp_init();
	//SFRIE1 = NMIIE;
	//SFRRPCR = SYSRSTRE__ENABLE | SYSRSTUP__PULLUP | SYSNMIIES__FALLING | SYSNMI__NMI;

	memset(regbak, 0, sizeof regbak);

	__bis_SR_register(GIE); // enable irq

#if DUMP_MODE
	curticks = DUMP_CYC_OFF;
	curaddr  = DUMP_ADDR_START;
#else
	curticks = START_HARD;
#endif

	/*uint8_t zarea[8];
	memcpy(zarea, (void*)0x1000, 8);
	iprintf("zarea: %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
		zarea[0], zarea[1], zarea[2], zarea[3],
		zarea[4], zarea[5], zarea[6], zarea[7]
	);*/

	puts("hello world!\r\n");

	done_irq = 0;
	do_trace();
	__builtin_unreachable();
}