#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_ptr(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 != 0u) && ((i % 4u) == 0u))
        {
            s[s_i++] = L'_';
        }
        s[s_i++] = hexchars[(n >> (60u - 4u * i)) & 0xFu];
    }
    write_string(s);
}

static void check_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_ptr((size_t)interface);
    write_string(L"\r\n");

    EFI_GRAPHICS_OUTPUT_PROTOCOL * gop = (EFI_GRAPHICS_OUTPUT_PROTOCOL *)interface;
}

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_ptr((size_t)system_table);
    write_string(L"\r\n");

    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_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;
}