#include <stdint.h>
#include <os/time.h>
#include <os/thread.h>
#include <util/semihosting.h>

void idle_main() {
	// QEMU does not support SLEEPONEXIT yet, so we need to keep at least one thread ready at all times.
	// By executing wfi in a loop, we force the cpu into sleep until next systick (or other interrupt), regardless of whether other threads are ready or not.
	// This has the consequence that we sleep for up to 1ms each time we've interated through the ready queue, even if other threads are still ready.
	
	while(1) {
		asm volatile("wfi");
		Thread::yield();
	}
}

uint32_t idle_stack[1024];

Thread idle_thread(idle_stack, sizeof(idle_stack), idle_main);

void foo_main() {
	while(1) {
		semihosting_print("foo\n");
		Thread::sleep(2000);
	}
}

uint32_t foo_stack[1024];

Thread foo_thread(foo_stack, sizeof(foo_stack), foo_main);

int main() {
	// Initialize system timer.
	STK.LOAD = 12500000 / 1000; // 1000 Hz.
	STK.CTRL = 0x07;
	
	idle_thread.start();
	foo_thread.start();
	
	for(int i = 0; i < 5; i++) {
		semihosting_print("main\n");
		Thread::sleep(5000);
	}
	
	semihosting_exit();
}