diff --git a/headers/scheduler.h b/headers/scheduler.h
index b345f02..24deced 100644
--- a/headers/scheduler.h
+++ b/headers/scheduler.h
@@ -1,3 +1,5 @@
 #pragma once
 
-void scheduler(void);
+#include <stdint.h>
+
+void scheduler(uint32_t *esp);
diff --git a/headers/thread.h b/headers/thread.h
index 9f5eadb..8e6672f 100644
--- a/headers/thread.h
+++ b/headers/thread.h
@@ -24,4 +24,4 @@ struct tcb {
 
 struct tcb *create_thread(struct pcb *process, void (*entry)(void));
 void delete_thread(struct tcb *thread);
-void switch_thread(struct tcb *thread_to_switch);
+void switch_thread(uint32_t *esp);
diff --git a/src/drivers/clock.c b/src/drivers/clock.c
index 16e02fb..8707ea0 100644
--- a/src/drivers/clock.c
+++ b/src/drivers/clock.c
@@ -40,7 +40,7 @@ static void clock_handler(struct registers *regs)
 {
 	(void)regs;
 	if (scheduler_counter % 10 == 0)
-		scheduler();
+		scheduler((uint32_t *)regs->esp);
 	scheduler_counter++;
 	sleep_counter--;
 }
diff --git a/src/multitasking/process.c b/src/multitasking/process.c
index ed873a0..332e0f0 100644
--- a/src/multitasking/process.c
+++ b/src/multitasking/process.c
@@ -6,31 +6,6 @@
 
 #include "string.h"
 
-// int8_t create_kernel_process(void)
-// {
-// 	struct pcb *new_pcb = vmalloc(sizeof(struct pcb));
-// 	if (!new_pcb)
-// 		return -1;
-// 	new_pcb->pid = 0;
-// 	new_pcb->uid = 0;
-// 	new_pcb->heap = kernel_pd;
-// 	new_pcb->cr3 = (uint32_t *)((uint32_t)kernel_pd - HEAP_END);
-// 	new_pcb->next = new_pcb;
-// 	new_pcb->prev = new_pcb;
-// 	struct tcb *kern_thread = vmalloc(sizeof(struct tcb));
-// 	if (!kern_thread) {
-// 		vfree(new_pcb);
-// 		return -1;
-// 	}
-// 	kern_thread->esp = ;
-// 	kern_thread->next = NULL;
-// 	kern_thread->process = new_pcb;
-// 	kern_thread->state = RUNNING;
-// 	kern_thread->tid = 1;
-
-// 	return 0;
-// }
-
 struct pcb *create_process(uint8_t uid)
 {
 	static uint32_t pid = 1;
@@ -47,14 +22,16 @@ struct pcb *create_process(uint8_t uid)
 	}
 	memcpy(new_pcb->heap, current_pcb->heap, 4096);
 
+	new_pcb->next = new_pcb;
+	new_pcb->prev = new_pcb;
+
 	if (current_pcb) {
 		new_pcb->next = current_pcb->next;
 		new_pcb->prev = current_pcb;
 		current_pcb->next = new_pcb;
 		if (current_pcb->prev == current_pcb)
 			current_pcb->prev = new_pcb;
-	}
-	else {
+	} else {
 		current_pcb = new_pcb;
 	}
 	new_pcb->signals.pending = SIG_IGN;
diff --git a/src/multitasking/scheduler.c b/src/multitasking/scheduler.c
index ecd7d4c..a6bcb52 100644
--- a/src/multitasking/scheduler.c
+++ b/src/multitasking/scheduler.c
@@ -11,19 +11,32 @@
 struct pcb *current_pcb;
 struct tcb *current_tcb;
 
-void scheduler(void)
+static struct tcb *get_thread_to_switch(void)
 {
-	struct tcb *thread_to_switch;
-	if (!current_tcb) {
-		thread_to_switch = current_pcb->thread_list;
-	} else {
-		if (!current_tcb->next) {
-			current_pcb = current_pcb->next;
-			// TODO switch context
-			thread_to_switch = current_pcb->thread_list;
-		} else {
-			thread_to_switch = current_tcb->next;
+	struct pcb *it_p;
+	struct tcb *it_t;
+
+	it_p = current_pcb;
+	it_t = current_tcb == NULL ? NULL : current_tcb->next;
+	while (it_p) {
+		while (it_t != NULL) {
+			if (it_t != NULL && it_t->state != WAITING)
+				return it_t;
+			it_t = it_t->next;
 		}
+		it_p = it_p->next;
+		it_t = it_p->thread_list;
 	}
-	switch_thread(thread_to_switch);
+	return NULL;
+}
+
+void scheduler(uint32_t *esp)
+{
+	struct tcb *thread_to_switch = get_thread_to_switch();
+	if (!thread_to_switch)
+		kpanic("No existing threads \n");
+	if (current_tcb)
+		current_tcb->esp = esp;
+	current_tcb = thread_to_switch;
+	switch_thread(thread_to_switch->esp);
 }
diff --git a/src/multitasking/switch_to_thread.s b/src/multitasking/switch_to_thread.s
index 26fca10..281960c 100644
--- a/src/multitasking/switch_to_thread.s
+++ b/src/multitasking/switch_to_thread.s
@@ -1,27 +1,21 @@
 .intel_syntax noprefix
+.set	CLEAR_ERRNO_INTNO, 0x08
 
 .section .text
 	.global switch_thread
 
 switch_thread:
-	mov edx, DWORD PTR [esp]
-	cmp DWORD PTR [current_tcb], 0
-	je .LABEL1
-	mov eax, [current_tcb]
-
-	// save the current stack pointer to the old stack
-	mov [eax+0], esp
-
-	// stack pointer + the 4 regs pushed
-	// and + 1 to get the argument (next thread)
-.LABEL1:
-	mov esi, [esp+4]
-	mov [current_tcb], esi
-
-	mov eax, [current_tcb]
-
-	mov esp, [eax+0] // get esp
-
-	push edx
 	
-	ret
+	mov eax, [esp + 4]
+	mov esp, eax	
+
+	pop eax
+	mov ds, eax
+
+	popa
+	
+	call pic_send_eoi
+
+	add esp, CLEAR_ERRNO_INTNO
+
+	iret
diff --git a/src/multitasking/thread.c b/src/multitasking/thread.c
index bcba614..f764c31 100644
--- a/src/multitasking/thread.c
+++ b/src/multitasking/thread.c
@@ -19,15 +19,27 @@ struct tcb *create_thread(struct pcb *process, void (*entry)(void))
 		vfree(new_tcb);
 		return NULL;
 	}
-	// set esp to "skip" the 4 GPRs and eip later to be used as the context
-	// of the thread
-	uint32_t *stack =
-	    (uint32_t *)((uint8_t *)new_tcb->esp0 + STACK_SIZE - 5 * 4);
+	uint32_t *stack = (uint32_t *)((uint8_t *)new_tcb->esp0 + STACK_SIZE);
+	uint32_t *esp = stack;
 
 	// testing out some stuff
 	*(--stack) = 0x202; // EFLAGS
 	*(--stack) = 0x08;  // CS = kernel code segment
 	*(--stack) = (uint32_t)entry;
+	// Error code and interrupt number (skipped by add $8, %esp)
+	*(--stack) = 0; // err_code
+	*(--stack) = 0; // int_no
+
+	// General purpose registers (for popa)
+	*(--stack) = 0;             // EAX
+	*(--stack) = 0;             // ECX
+	*(--stack) = 0;             // EDX
+	*(--stack) = 0;             // EBX
+	*(--stack) = (uint32_t)esp; // ESP (original - points to exit_process)
+	*(--stack) = 0;             // EBP
+	*(--stack) = 0;             // ESI
+	*(--stack) = 0;             // EDI
+	*(--stack) = 0x10;          // kernel DS
 	new_tcb->esp = stack;
 	new_tcb->process = process;
 	new_tcb->next = NULL;