#define PAGE_MASK   0xfff
#define PAGE_SHIFT  12

inline void __writecr0(CEfiU64 cr0)
{
    asm volatile("mov %%rax, %%cr0" :: "a"(cr0));
}

inline CEfiU64 __readcr0()
{
    CEfiU64 cr0;
    asm("mov %%cr0, %%rax" : "=a"(cr0));
    return cr0;
}

inline CEfiU64 __readcr3()
{
    CEfiU64 cr3;
    asm("mov %%cr3, %%rax" : "=a"(cr3));
    return cr3;
}

CEFICALL CEfiStatus efi_main([[__maybe_unused__]] CEfiHandle handle,
                             CEfiSystemTable *systemtable) {
    h = handle;
    st = systemtable;

    st->con_out->reset(st->con_out, false);

    st->con_out->set_attribute(st->con_out,
                               C_EFI_BACKGROUND_RED | C_EFI_YELLOW);

    CEfiGuid lip_guid = C_EFI_LOADED_IMAGE_PROTOCOL_GUID;
    CEfiLoadedImageProtocol* lip = C_EFI_NULL;
    CEfiStatus status = st->boot_services->open_protocol(
        h, &lip_guid, (void**)&lip, h, C_EFI_NULL,
        C_EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL);
    if (C_EFI_ERROR(status)) {
        error(u"Could not open Loaded Image Protocol\r\n");
    }

    CEfiGuid sfsp_guid = C_EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_GUID;
    CEfiSimpleFileSystemProtocol* sfsp = C_EFI_NULL;
    status = st->boot_services->open_protocol(
        lip->device_handle, &sfsp_guid, (void**)&sfsp, h, C_EFI_NULL,
        C_EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL);
    if (C_EFI_ERROR(status)) {
        error(u"Could not open Simple File System Protocol\r\n");
    }

    CEfiFileProtocol* root = C_EFI_NULL;
    status = sfsp->openVolume(sfsp, &root);
    if (C_EFI_ERROR(status)) {
        error(u"Could not Open Volume for root directory in ESP\r\n");
    }

    CEfiFileProtocol* file = C_EFI_NULL;
    status = root->open(root, &file, u"\\EFI\\BOOT\\kernel.bin", C_EFI_FILE_MODE_READ, C_EFI_FILE_READ_ONLY);
    if (C_EFI_ERROR(status)) {
        error(u"Could not Open File\r\n");
    }

    CEfiFileInfo file_info;
    CEfiGuid fi_guid = C_EFI_FILE_INFO_ID;
    CEfiUSize file_info_size = sizeof(file_info);
    status = file->getInfo(file, &fi_guid, &file_info_size, &file_info);
    if (C_EFI_ERROR(status)) {
        error(u"Could not get file info\r\n");
    }

    CEfiUSize file_size = file_info.fileSize;
    print_string(u"file size: %llu\r\n", file_size);

    CEfiUSize pages = (file_size >> PAGE_SHIFT) + (file_size & PAGE_MASK ? 1 : 0);
    CEfiPhysicalAddress phys_krnl;

    status = st->boot_services->allocate_pages(C_EFI_ALLOCATE_ANY_PAGES, C_EFI_LOADER_DATA, pages, &phys_krnl);
    if (C_EFI_ERROR(status)) {
        error(u"Could not allocate kernel\r\n");
    }

    memset((void*)phys_krnl, 0, pages << PAGE_SHIFT);

    print_string(u"Physical address: 0x%llx - 0x%llx\r\n", phys_krnl, phys_krnl + (pages << PAGE_SHIFT) - 1);
    print_string(u"Allocated %llu pages\r\n", pages);

    file->setPosisition(file, 0);
    CEfiUSize read_bytes = file_size;
    print_string(u"Attempting to read %llu bytes...", read_bytes);
    file->read(file, &read_bytes, (void*)phys_krnl);
    print_string(u"Read %llu bytes.\r\n", read_bytes);

#define CR0_WP (1 << 16)

    // Clear WP flag to allow rewriting page tables
    __writecr0(__readcr0() & ~CR0_WP);

    // This is missing...
    void *virt_krnl = map_kernel_and_framebuffer();

    __writecr0(__readcr0() | CR0_WP);

    jumpIntoKernel((void*)virt_krnl);

    return C_EFI_SUCCESS;
}