#include <efi.h>
#include <efilib.h>
#include <stddef.h>

EFI_SYSTEM_TABLE * g_system_table;

static void write_string(uint16_t * string)
{
    g_system_table->ConOut->OutputString(g_system_table->ConOut, string);
}

static void write_uint(uint32_t n)
{
    uint16_t s[12] = {0};
    size_t i = 0u;
    while ((n != 0u) || (i == 0u))
    {
        s[10u - i] = '0' + (n % 10u);
        n /= 10u;
        i++;
    }
    write_string(&s[11u - i]);
}

static void write_hex64(size_t n)
{
    CHAR16 hexchars[] = L"0123456789ABCDEF";
    CHAR16 s[20] = {0};
    size_t s_i = 0u;
    for (size_t i = 0u; i < 16u; i++)
    {
        if (i == 8u)
        {
            s[s_i++] = L'_';
        }
        s[s_i++] = hexchars[(n >> (60u - 4u * i)) & 0xFu];
    }
    write_string(s);
}

static void check_gop_handle(EFI_HANDLE handle)
{
    EFI_HANDLE interface = NULL;
    EFI_STATUS status = g_system_table->BootServices->HandleProtocol(handle,
            &(EFI_GUID)EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID,
            &interface);
    if (EFI_ERROR(status))
    {
        write_string(L"Error ");
        write_uint(status);
        write_string(L" from HandleProtocol\r\n");
        return;
    }

    if (interface == NULL)
    {
        write_string(L"NULL interface from HandleProtocol\r\n");
        return;
    }

    write_string(L"interface is at ");
    write_hex64((size_t)interface);
    write_string(L"\r\n");

    EFI_GRAPHICS_OUTPUT_PROTOCOL * gop = (EFI_GRAPHICS_OUTPUT_PROTOCOL *)interface;
}

static void dump_memory_map(void)
{
    static uint8_t memory_map_buffer[16384];
    UINTN memory_map_size = sizeof(memory_map_buffer);
    UINTN map_key;
    UINTN descriptor_size;
    UINT32 descriptor_version;
    EFI_STATUS status = g_system_table->BootServices->GetMemoryMap(&memory_map_size,
            memory_map_buffer,
            &map_key,
            &descriptor_size,
            &descriptor_version);
    write_string(L"GetMemoryMap status = ");
    write_uint(status);
    write_string(L"\r\n");
    write_string(L"  memory_map_size = ");
    write_uint(memory_map_size);
    write_string(L"\r\n");
    if (!EFI_ERROR(status))
    {
        write_string(L"  descriptor_size = ");
        write_uint(descriptor_size);
        write_string(L"\r\n");
        size_t n_descriptors = memory_map_size / descriptor_size;
        for (size_t i = 0u; i < n_descriptors; i++)
        {
            EFI_MEMORY_DESCRIPTOR * descriptor = (EFI_MEMORY_DESCRIPTOR *)&memory_map_buffer[i * descriptor_size];
            if (descriptor->Type == EfiConventionalMemory)
            {
                write_string(L"  ");
                write_hex64(descriptor->PhysicalStart);
                write_string(L" : ");
                write_hex64(descriptor->VirtualStart);
                write_string(L", n=");
                write_uint(descriptor->NumberOfPages);
                write_string(L", t=");
                write_uint(descriptor->Type);
                write_string(L", a=");
                write_hex64(descriptor->Attribute);
                write_string(L"\r\n");
            }
        }
    }
}

EFI_STATUS
efi_main(EFI_HANDLE image_handle, EFI_SYSTEM_TABLE * system_table)
{
    EFI_STATUS status;
    EFI_INPUT_KEY key;

    g_system_table = system_table;

    write_string(L"My first EFI loader\r\n");

    status = system_table->ConIn->Reset(system_table->ConIn, FALSE);
    if (EFI_ERROR(status))
        return status;

    write_string(L"system_table is at ");
    write_hex64((size_t)system_table);
    write_string(L"\r\n");

    dump_memory_map();

    EFI_HANDLE buffer[10];
    UINTN buffer_size = sizeof(buffer);
    status = system_table->BootServices->LocateHandle(ByProtocol,
            &(EFI_GUID)EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID,
            NULL,
            &buffer_size,
            buffer);
    buffer_size /= sizeof(EFI_HANDLE);
    write_string(L"LocateHandle status = ");
    write_uint(status);
    write_string(L", num entries = ");
    write_uint(buffer_size);
    write_string(L"\r\n");

    for (size_t i = 0u; i < buffer_size; i++)
    {
        check_gop_handle(buffer[i]);
    }

    write_string(L"Press any key...\r\n");
    while ((status = system_table->ConIn->ReadKeyStroke(system_table->ConIn, &key)) == EFI_NOT_READY)
    {
    }

    return status;
}