#include "../../internal-include/BootLoader/boot_loader.hpp"
#include "../../internal-include/GPU/gpu_internal.hpp"
#include <stdio.hpp>

#include "../../internal-include/SPU/spu_mmu.hpp"
#include <PSX/GTE/gte.hpp>
#include <PSX/SPU/spu.hpp>
#include <PSX/System/IOPorts/spu_io.hpp>
#include <PSX/System/syscalls.hpp>

extern "C" uint32_t __heap_start;
extern "C" uint32_t __bss_start;
extern "C" uint32_t __bss_end;
extern "C" uint32_t __bss_len;
extern "C" uint32_t __planschi_start;
extern "C" uint32_t __planschi_end;

namespace JabyEngine {
    static void test_gte_scale() {
        auto matrix = GTE::ROTMATRIX::identity();

        GTE::scale_matrix(matrix, GTE::VECTOR::create(1.5_gf, 2.0_gf, 1.0_gf));
        printf("|%i|%i|%i|\n", matrix.matrix[0][0], matrix.matrix[0][1], matrix.matrix[0][2]);
        printf("|%i|%i|%i|\n", matrix.matrix[1][0], matrix.matrix[1][1], matrix.matrix[1][2]);
        printf("|%i|%i|%i|\n", matrix.matrix[2][0], matrix.matrix[2][1], matrix.matrix[2][2]);
    }

    static void test_bios_font() {
        static constexpr uint16_t SJIS = 0x83B5;

        const auto*      font     = SysCall::Krom2RawAdd(SJIS);
        const auto*const font_end = font + 16;
        printf("Loading SJIS from @0x%p\n", font);
        
        for(; font < font_end; font++) {
            const auto cur_char = __builtin_bswap16(*font);
            for(uint16_t shift = 1 << 15; shift; shift = shift >> 1) {
                printf(cur_char & shift ? "#" : " ");
            }

            printf("\n");
        }
    }

    namespace boot {
        namespace Start {
            // Thanks to Nicolas Noble!
            static void enable_vanilla_bios() {
                SysCall::FlushCache();
                SysCall::DequeueCdIntr();
                SysCall::SetDefaultExitFromException();
            }

            static void setup() {
                static constexpr auto DebugX     = 1;
                static constexpr auto DebugY     = 0;
                static constexpr auto DebugScale = 1.0;

                BIOS::identify();
                enable_vanilla_bios();
                // Not used anymore
                //Callbacks::setup();

                __debug_boot_color_at(::JabyEngine::GPU::Color24::Grey(), DebugX, DebugY, DebugScale);
                DMA::setup();

                __debug_boot_color_at(::JabyEngine::GPU::Color24::White(), DebugX, DebugY, DebugScale);
                Periphery::setup();

                __debug_boot_color_at(::JabyEngine::GPU::Color24::Red(), DebugX, DebugY, DebugScale);
                SPU::stop_voices();
                
                __debug_boot_color_at(::JabyEngine::GPU::Color24::Green(), DebugX, DebugY, DebugScale);
                CD::setup();
                __debug_boot_color_at(::JabyEngine::GPU::Color24::Blue(), DebugX, DebugY, DebugScale);
                Timer::setup();

                __debug_boot_color_at(::JabyEngine::GPU::Color24::Yellow(), DebugX, DebugY, DebugScale);
                GPU::setup();
                GPU::display_logo();
                GTE::setup();
                test_bios_font();
                test_gte_scale();

                SPU::setup();
            }
        }
    }

    void start() {
        static constexpr auto DebugX     = 0;
        static constexpr auto DebugY     = 0;
        static constexpr auto DebugScale = 1.0;

        __debug_boot_print_at(GPU::Color24::White(), DebugX, DebugY, DebugScale, "Starting Planschbecken\n");
        printf("Heap starts @0x%p\n", &__heap_start);
        printf("BSS      from 0x%p to 0x%p (%u)\n", &__bss_start, &__bss_end, __bss_len);
        __debug_boot_print_at(GPU::Color24::Green(), DebugX, DebugY, DebugScale, "PLANSCHI from 0x%p to 0x%p\n", &__planschi_start, &__planschi_end);
        boot::Start::setup();
        
        __debug_boot_print_at(GPU::Color24::Red(), DebugX, DebugY, DebugScale, "Running main...\n");
        GPU::internal::wait_vsync(0);
        run();
    }
}