diff --git a/c_build_helper.sh b/c_build_helper.sh
index 31c76f1..ffd68bb 100644
--- a/c_build_helper.sh
+++ b/c_build_helper.sh
@@ -1,4 +1,4 @@
-CFLAGS="-g -c -m32 -fno-pie -mno-sse -O3 -D __bits__=32 -Wno-int-to-pointer-cast -Wno-pointer-to-int-cast -Wno-incompatible-pointer-types -Wno-address-of-packed-member -fno-stack-protector -mno-red-zone -mno-sse -mno-sse2 -ffreestanding -nostdlib -mno-mmx"
+CFLAGS="-g -c -m32 -fno-pie -mno-sse -O3 -D __bits__=32 -Wno-int-to-pointer-cast -Wno-pointer-to-int-cast -Wno-incompatible-pointer-types -Wno-address-of-packed-member -Wno-discarded-qualifiers -fno-stack-protector -mno-red-zone -mno-sse -mno-sse2 -ffreestanding -nostdlib -mno-mmx"
 LDFLAGS_i386="-Ttext 0xc0000000 --oformat binary -melf_i386"
 
 cd src/kernel/
diff --git a/src/kernel/arch_i386/idt.s b/src/kernel/arch_i386/idt.s
index 1503bd1..1fcd3e9 100644
--- a/src/kernel/arch_i386/idt.s
+++ b/src/kernel/arch_i386/idt.s
@@ -344,7 +344,7 @@ _irq_common_stub:
     push esp;push argument
     call _irq_handler
     pop esp
-    
+    mov esp, eax
     pop gs
     pop fs
     pop es
diff --git a/src/kernel/arch_i386/interrupts.c b/src/kernel/arch_i386/interrupts.c
index 43e8d0e..d953db0 100644
--- a/src/kernel/arch_i386/interrupts.c
+++ b/src/kernel/arch_i386/interrupts.c
@@ -35,7 +35,8 @@ inline void set_idt_entry(uint32_t index, void *ptr, uint8_t flags, uint16_t seg
 }
 
 
-void _irq_handler(cpu_registers_t *regs){
+cpu_registers_t *_irq_handler(cpu_registers_t *regs){
+    regs->esp = (uint32_t)regs;
     if(regs->int_no == 0x80){
         regs = syscall(regs);
     }
@@ -50,7 +51,7 @@ void _irq_handler(cpu_registers_t *regs){
     if(regs->int_no >= 0x28){
         outb(0xa0, 0x20);
     }
-    return;
+    return regs;
 }
 void _isr_handler(cpu_registers_t *regs){
     printf("Error!! %x\n", regs->eip);
diff --git a/src/kernel/kmain.c b/src/kernel/kmain.c
index cfddb72..2192852 100644
--- a/src/kernel/kmain.c
+++ b/src/kernel/kmain.c
@@ -4,8 +4,15 @@
 #include "shared/interrupts.h"
 #include "drivers/serial.h"
 #include "drivers/pic.h"
+#include "drivers/cpuio.h"
 #include "system/scheduler.h"
 
+void test(){
+    printf("Hi from another thread!\n");
+    // *((uint16_t*)0xb8000) = 0xf41;
+    for(;;);
+}
+
 extern void kmain(kernel_info_t *kernel_info){
     init_serial();
     pm_init(kernel_info);
@@ -16,6 +23,9 @@ extern void kmain(kernel_info_t *kernel_info){
     pic_setmask(0xfe, PIC1_DATA);
     enable_interrupts();
     // printf("done\n");
+    thread_start(test, 0);
+    // printf("%x", test);
+    // void test();
     for(;;);
     return;
 }
\ No newline at end of file
diff --git a/src/kernel/shared/string.h b/src/kernel/shared/string.h
index a51a35f..aa95c08 100644
--- a/src/kernel/shared/string.h
+++ b/src/kernel/shared/string.h
@@ -11,4 +11,5 @@ inline uint32_t strlen(char *str){
     }
     return c;
 }
-void strcpy(char *src, char *dest);
\ No newline at end of file
+void strcpy(char *src, char *dest);
+void memcpy(char *src, char *dest, uint32_t c);
\ No newline at end of file
diff --git a/src/kernel/system/scheduler.c b/src/kernel/system/scheduler.c
index 74050af..be59ccf 100644
--- a/src/kernel/system/scheduler.c
+++ b/src/kernel/system/scheduler.c
@@ -1,34 +1,63 @@
 #include "../shared/interrupts.h"
 #include "scheduler.h"
+#include "../shared/string.h"
 #include "../shared/kstdlib.h"
+#include "../shared/memory.h"
 
 process_t volatile processes[0x10000];
 uint32_t volatile process_queue[0x10000];
+uint32_t active_processes;
 uint32_t volatile current_pid;
-
+uint32_t volatile queue_length;
+uint32_t volatile current_queue_index;
 void init_scheduler(){
     for(uint32_t i = 0; i < 0x10000; i++){
         processes[i].flags.present = 0;
         process_queue[i] = 0;
     }
     current_pid = 0;
+    queue_length = 0;
+    current_queue_index = 0;
+    active_processes = 0;
     install_irq_handler(schedule, 0);
-    printf("Installing scheduler");
+    // printf("Installing scheduler");
 }
 
 cpu_registers_t *schedule(cpu_registers_t *regs){
-    uint32_t i = current_pid;
-    // printf("Test");
-    while(processes[process_queue[i]].flags.blocked){
-        i++;
-        if(i == current_pid) return regs;
-        if(i >= 0x10000) i = 0;
+    if(queue_length == 0){
+        return regs;
     }
-    current_pid = i;
-    return &(processes[i].cpuregs);
+    uint32_t i = current_queue_index;
+    for(; i < queue_length; i++){
+        if(processes[process_queue[i%queue_length]].flags.blocked) continue;
+        else break;
+    }
+    current_pid = process_queue[i];
+    // printf("\n%d", current_pid);
+    // *regs = processes[current_pid].cpuregs;
+    cpu_registers_t *newregs = (cpu_registers_t*)processes[current_pid].cpuregs.esp;
+    return newregs;
     // return regs;
 }
 
+void add_process_queue(uint32_t pid){
+    process_queue[queue_length] = pid;
+    queue_length++;
+}
+void remove_process_queue(uint32_t pid){
+    uint32_t last_queue_index = queue_length-1;
+    uint32_t old_index = 0;
+    for(uint32_t i = 0; i < last_queue_index; i++){
+        if(process_queue[i] == pid){
+            old_index = i;
+            process_queue[i] = 0;
+        }
+    }
+    if(old_index == last_queue_index) return;
+    process_queue[old_index] = process_queue[last_queue_index];
+    process_queue[last_queue_index] = 0;
+}
+
 void spawn_new_process(cpu_registers_t defaultregs, char **argv, uint32_t argc, void *cr3){
     uint32_t i = current_pid;
     while(processes[i].flags.present){
@@ -36,6 +65,9 @@ void spawn_new_process(cpu_registers_t defaultregs, char **argv, uint32_t argc,
         if(i >= 0x10000) i = 0;
         if(i == current_pid) return;
     }
+    i += !active_processes;
+    active_processes++;
+    // printf("Found %d", i);
     process_t new_proc = {0};
     new_proc.argc = argc;
     new_proc.argv = argv;
@@ -46,6 +78,8 @@ void spawn_new_process(cpu_registers_t defaultregs, char **argv, uint32_t argc,
     new_proc.flags.cpu_lvl = 1;
     new_proc.flags.priority = 0;
     new_proc.flags.system = processes[current_pid].flags.system;
+    processes[i] = new_proc;
+    add_process_queue(i);
     return;
 }
 
@@ -59,7 +93,17 @@ void exit(){
     
 }
 void thread_start(void (*function)(), uint32_t *exit_code){
-    
+    cpu_registers_t regs = {0};
+    regs.cs = 0x10;
+    regs.ds = 0x18;
+    regs.es = 0x18;
+    regs.ss = 0x18;
+    // regs.ebp = 
+    regs.esp = 0x7000 - sizeof(regs) + 8;
+    regs.ebp = regs.esp;
+    regs.eip = (uint32_t)function;
+    memcpy((char *)&regs, (char*)regs.esp, sizeof(regs));
+    spawn_new_process(regs, 0, 0, (void *)0x10000);
 }
 void thread_join(uint32_t thread_id, uint32_t **exit_code){
     
diff --git a/src/kernel/system/scheduler.h b/src/kernel/system/scheduler.h
index 00e90f9..651bca6 100644
--- a/src/kernel/system/scheduler.h
+++ b/src/kernel/system/scheduler.h
@@ -21,4 +21,5 @@ typedef struct{
 }process_t;
 
 cpu_registers_t* schedule(cpu_registers_t *regs);
-void init_scheduler();
\ No newline at end of file
+void init_scheduler();
+void thread_start(void (*function)(), uint32_t *exit_code);
\ No newline at end of file
diff --git a/src/kernel/system/string.c b/src/kernel/system/string.c
index 326a0cd..79ea325 100644
--- a/src/kernel/system/string.c
+++ b/src/kernel/system/string.c
@@ -34,7 +34,11 @@ void itoa(int64_t data, char *res, uint8_t base){
     res[0] += negative * '-';
     return;
 }
-
+void memcpy(char *src, char *dest, uint32_t c){
+    for(uint32_t i = 0; i < c; i++){
+        dest[i] = src[i];
+    }
+}
 void strcpy(char *src, char *dest){
     if(src == 0 || dest == 0){
         return;