#include "pthread.h"

#include <errno.h>
#include <mango/status.h>
#include <mango/task.h>
#include <pthread.h>
#include <string.h>
#include <sys/mman.h>

#define DEFAULT_STACK_SIZE 0x40000

static void thread_launchpad(uintptr_t thread, uintptr_t func, uintptr_t arg)
{
	struct __pthread *self = (struct __pthread *)thread;
	void *(*start_func)(void *) = (void *(*)(void *))func;

	void *result = start_func((void *)arg);

	pthread_exit(result);
}

int pthread_create(
	pthread_t *thread,
	const pthread_attr_t *attr,
	void *(*start_func)(void *),
	void *arg)
{
	kern_status_t status = KERN_OK;
	kern_handle_t task;

	status = task_self(&task);
	if (status != KERN_OK) {
		return __set_errno(EPERM);
	}

	size_t stack_size = DEFAULT_STACK_SIZE;

	void *base
		= mmap(NULL,
	               stack_size,
	               PROT_READ | PROT_WRITE,
	               MAP_PRIVATE | MAP_ANONYMOUS,
	               -1,
	               0);

	if (!base) {
		return __set_errno(ENOMEM);
	}

	virt_addr_t sp = (virt_addr_t)base + DEFAULT_STACK_SIZE;

	sp -= sizeof(struct __pthread);
	struct __pthread *new = (struct __pthread *)sp;
	sp &= ~(virt_addr_t)0x10;

	memset(new, 0x0, sizeof *new);
	new->thr_self = new;
	new->thr_map_base = base;
	new->thr_map_size = stack_size;

	uintptr_t args[] = {
		(uintptr_t)new,
		(uintptr_t)start_func,
		(uintptr_t)arg,
	};
	size_t nr_args = sizeof args / sizeof args[0];

	status = task_create_thread(
		task,
		(virt_addr_t)thread_launchpad,
		sp,
		args,
		nr_args,
		&new->thr_handle);
	if (status != KERN_OK) {
		munmap(base, stack_size);
		return __set_errno(__errno_from_kern_status(status));
	}

	thread_config_set(
		new->thr_handle,
		THREAD_CFG_GSBASE,
		&new,
		sizeof(void *));
	thread_start(new->thr_handle);

	*thread = new;
	return __set_errno(SUCCESS);
}