#include <stdarg.h>
#include <stdint.h>

// #include <dtb/dtb.h>

#include <opensbi/opensbi.h>
#include <opensbi/extensions/legacy.h>
#include <opensbi/extensions/base.h>
#include <opensbi/extensions/timer.h>
#include <opensbi/extensions/ipi.h>
#include <opensbi/extensions/rfence.h>
#include <opensbi/extensions/hsm.h>
#include <opensbi/extensions/srst.h>
#include <opensbi/extensions/pmu.h>

#include <sunxi_d1_pinctrl/sunxi_d1_pinctrl.h>
#include <uart_16550/uart_16550.h>

void fmt(const char *f, ...) {
    namespace legacy = drivers::opensbi::legacy;

    va_list va;
    va_start(va, f);
    while(*f) {
        if(*f != '{') {
            legacy::console_putchar(*(f++));
            continue;
        }
        f++;

        while(*f != '}') {
            if(!(*f))
                return;
            f++;
        }
        f++;

        unsigned long v = va_arg(va, unsigned long);
        for(int i = 0; i < 16; ++i) {
            const char *digits = "0123456789abcdef";
            int d = (v >> (60 - i * 4)) & 0xF;
            legacy::console_putchar(digits[d]);
        }
    }
    va_end(va);
}

extern "C" void kmain(unsigned long hart, void *dtb) {
    (void)hart;
    (void)dtb;
    // D1 doesnt give me dtb ptr?
    /*
    dtb = (void *)0x5fb38580;

    dtb::DeviceTree dt;
    int res = dtb::validate_and_parse_header(dt, dtb);
    if(!res) {
        fmt("validate_and_parse_header: {}\n", res);
        fmt("magic: {}\n", dt.meta.base);
        dtb::print_reserved_regions(dt);
        dtb::print_structure_block(dt);
    }
    */

    namespace base = drivers::opensbi::base;

    fmt("test\n");

    auto ret = base::get_spec_version();
    if(ret.error) {
        fmt("get_spec_version failed\n");
    } else {
        fmt("spec version: {}\n", ret.value);
    }

    ret = base::get_impl_id();
    if(ret.error) {
        fmt("get_impl_id failed\n");
    } else {
        fmt("impl id: {}\n", ret.value);
    }

    ret = base::get_impl_version();
    if(ret.error) {
        fmt("get_impl_version failed\n");
    } else {
        fmt("impl version: {}\n", ret.value);
    }

    ret = base::get_mvendor_id();
    if(ret.error) {
        fmt("get_mvendor_id failed\n");
    } else {
        fmt("vendor id: {}\n", ret.value);
    }

    ret = base::get_march_id();
    if(ret.error) {
        fmt("get_march_id failed\n");
    } else {
        fmt("arch id: {}\n", ret.value);
    }

    ret = base::get_mimp_id();
    if(ret.error) {
        fmt("get_mimp_id failed\n");
    } else {
        fmt("imp id: {}\n", ret.value);
    }

    fmt("uintmax: {} bits\n", sizeof(uintmax_t) * 8);
    fmt("HART: {}, DTB: {}\n", hart, dtb);

    //  ==================
    //  == Pinctrl test ==
    //  ==================
    namespace sunxi_d1_pinctrl = drivers::sunxi_d1_pinctrl;
    sunxi_d1_pinctrl::SunxiD1Pinctrl pinctrl((void *)0x2000000);

    pinctrl.set_pin_mode(sunxi_d1_pinctrl::Bank::PC, 1, 1);
    pinctrl.set_pin_state(sunxi_d1_pinctrl::Bank::PC, 1, true);

    //  ===============
    //  == UART test ==
    //  ===============
    namespace uart_16550 = drivers::uart_16550;
    uart_16550::Uart16550 uart((void *)0x02500000);
    // uart_16550::Uart16550 uart((void *)0x02500400);
    uart.init(115200, uart_16550::DataBits::BITS_8, uart_16550::Parity::NONE, uart_16550::StopBits::STOP_1);

    while(true) {
        uart.write('X');
        uart.write('\n');
        uint32_t a = 0;
        for(a = 0; a < 0x10000000; a++);
    }

    fmt("HALT\n");
    while(1);

    namespace hsm = drivers::opensbi::hsm;
    hsm::hart_stop();
}