#include <assert.h>
#include <limine.h>
#include <stdbool.h>
#include <stdint.h>
#include <kprint.h>
#include <neobbo.h>
#include <arch/amd64/hal/gdt.h>
#include <smp.h>
#include <arch/amd64/hal/apic.h>
#include <arch/amd64/hal/idt.h>
#include <mm/vmm.h>
#include <mm/kmalloc.h>
#include <lock.h>
#include <arch/amd64/io.h>
#include <string.h>


extern void s_load_idt();
extern void s_load_gdt();
extern volatile struct limine_mp_request smp_request;

static cpu_state *cpus;

static cpu_state bsp_cpu;

/* Returns the CPU structure for this particular CPU */
cpu_state *get_current_cpu_state(){
    return (cpu_state*)rdmsr(GSBASE);
}

cpu_state *get_bsp_cpu_state(){
    return &bsp_cpu;
}

cpu_state *get_cpu_state(int id){

    if(id == 0){
        return &bsp_cpu;
    }

    return &cpus[id];
}

bool get_cpu_state_initialized(){
    if(rdmsr(GSBASE) < get_kinfo()->hhdmoffset){
        return false;
    }

    return true;
}

atomic_flag ap_init_lock = ATOMIC_FLAG_INIT;

void ap_init(struct limine_mp_info *smp_info){

    acquire_spinlock(&ap_init_lock);

    /* Load the GDT */
    s_load_gdt();

    /* Load the IDT */
    s_load_idt();

    /* Set the CR3 context */
    extern uint64_t *kernel_page_map;

    vmm_set_ctx(kernel_page_map);

    asm volatile(
        "movq %%cr3, %%rax\n\
        movq %%rax, %%cr3\n"
        : : : "rax"
    );

    cpu_state cpu_struct = cpus[smp_info->lapic_id];

    cpu_struct.id = smp_info->lapic_id;

    wrmsr(KERNELGSBASE, (uint64_t)&cpu_struct);
    wrmsr(GSBASE, (uint64_t)&cpu_struct);

    /* Initialize APIC & APIC timer */
    ap_apic_init();

    free_spinlock(&ap_init_lock);

    while(!bsp_cpu.scheduler_initialized){
        __builtin_ia32_pause();
    }

    scheduler_init();
}

void smp_init(){

    struct limine_mp_response *smp_response = smp_request.response;

    kprintf("smp: {d} CPUs\n", smp_response->cpu_count);

    cpus = (cpu_state *)kzalloc(sizeof(cpu_state) * smp_response->cpu_count);

    for(uint64_t i = 1; i < smp_response->cpu_count; i++){
        /* Pointer to smp_info is passed in RDI by Limine, so no need to pass any arguments here */
        smp_response->cpus[i]->goto_address = &ap_init;
    }

    bsp_cpu.scheduler_context = (struct context*)kzalloc(sizeof(struct context));
    cpus[bsp_cpu.id] = bsp_cpu; 

    /* If one of the APs has halted, then halt the BSP */
    extern bool kernel_killed;
    
    assert(!kernel_killed && "Some APs failed to init!");

}

void bsp_early_init(){

    assert(smp_request.response != NULL && "Failed to get SMP request");

    struct limine_mp_response *smp_response = smp_request.response;

    bsp_cpu.id = smp_response->cpus[0]->lapic_id;
    
    wrmsr(KERNELGSBASE, (uint64_t)&bsp_cpu);
    wrmsr(GSBASE, (uint64_t)&bsp_cpu);
}