#include <nds.h>
#undef REG_MBK1
#undef REG_MBK2
#undef REG_MBK3
#undef REG_MBK4
#undef REG_MBK5
#undef REG_MBK6
#undef REG_MBK7
#undef REG_MBK8
#undef REG_MBK9
#include "memTwlWram.h"
#include "memNtrWram.h"
#include "memVram.h"
#include "card.h"
#include "../include/picoLoader7.h"
#include "fat/dldi.h"
#include "fat/ff.h"

#define PICO_LOADER_9_PATH    "/_pico/picoLoader9.bin"
#define PICO_LOADER_7_PATH    "/_pico/picoLoader7.bin"

typedef void (*pico_loader_9_func_t)(void);

FATFS gFatFs;
FIL gFile;

static void loadFile(const char* path, void* destination)
{
    if (f_open(&gFile, path, FA_OPEN_EXISTING | FA_READ) != FR_OK)
    {
        // Red screen: could not find pico loader file
        *(vu16*)0x05000000 = 15;
        while (1);
    }
    u32 size = f_size(&gFile);
    UINT bytesRead;
    f_read(&gFile, destination, size, &bytesRead);
    f_close(&gFile);
}

static void clearArm9IoRegisters(void)
{
    *(vu16*)0x05000000 = 0;
    *(vu16*)0x05000400 = 0;
    *(vu32*)0x04000000 = 0x10000;
    *(vu32*)0x04001000 = 0x10000;
    *(vu16*)0x0400006C = 0;
    *(vu16*)0x0400106C = 0;

    for (int i = 0; i < 4; i++)
    {
        // clear dma
        DMA_CR(i) = 0;
        // clear ndma
        for (int reg = 0; reg < 0x1C; reg += 4)
        {
            *((vu32*)(0x04004104 + ((i * 0x1C) + reg))) = 0;
        }
    }

    REG_POWERCNT = 0x820f;
    WRAM_CR = 0x03;
    REG_EXMEMCNT = 0xE880;

    REG_IE = 0;
    REG_IF = ~0;
}

static bool trySendIpcMessage(u32 channel, u32 data)
{
    if (REG_IPC_FIFO_CR & (IPC_FIFO_SEND_FULL | IPC_FIFO_ERROR))
    {
        return false;
    }

    REG_IPC_FIFO_TX = (channel & 0x1F) | (data << 6);
    return true;
}

static void bootstrapArm7(void)
{
    // unlock twl wram
    while (!trySendIpcMessage(18, (13 << 20) | (0 << 18) | 0xF));
    while (!trySendIpcMessage(18, (13 << 20) | (1 << 18) | 0xFF));
    while (!trySendIpcMessage(18, (13 << 20) | (2 << 18) | 0xFF));

    while (!mem_isTwlWramUnlocked());

    mem_setTwlWramAMapping(MEM_TWL_WRAM_A_SLOT_ALL, 0x037C0000, 256 * 1024);
    mem_setTwlWramBMapping(MEM_TWL_WRAM_BC_SLOT_ALL, 0x03780000, 256 * 1024);
    mem_setTwlWramCMapping(MEM_TWL_WRAM_BC_SLOT_ALL, 0x03740000, 256 * 1024);

    for (int i = 7; i >= 0; i--)
    {
        mem_setTwlWramCBlockMapping(MEM_TWL_WRAM_C_ARM9, i, i);
        for (vu32* wram = (vu32*)(0x03740000 + i * 32 * 1024); wram < (vu32*)(0x03740000 + (i + 1) * 32 * 1024); )
        {
            *wram++ = 0xE3A0F406;
        }
        mem_setTwlWramCBlockMapping(MEM_TWL_WRAM_C_ARM7, i, i);
    }
}

int main(void)
{
    clearArm9IoRegisters();

    REG_EXMEMCNT &= ~0x800; // cartridge to ARM9

    card_romSetCmd(0xFC00000000000000ull);
    card_romStartXfer(MCCNT1_DIR_READ | MCCNT1_RESET_OFF | MCCNT1_CLK_6_7_MHZ | MCCNT1_LEN_0 | MCCNT1_CMD_SCRAMBLE |
        MCCNT1_LATENCY2(0) | MCCNT1_CLOCK_SCRAMBLER | MCCNT1_LATENCY1(24), false);
    card_romWaitBusy();
    REG_MCCNT1 = 0;
    REG_MCSCR0 = 0;
    REG_MCSCR1 = 0;
    REG_MCSCR2 = 0;
    REG_MCCNT1 = MCCNT1_RESET_OFF | MCCNT1_APPLY_SCRAMBLE_SEED | MCCNT1_CLOCK_SCRAMBLER | MCCNT1_READ_DATA_DESCRAMBLE;

    if (f_mount(&gFatFs, "fat:", 1) != FR_OK)
    {
        // Blue screen: mount failed
        *(vu16*)0x05000000 = 15 << 10;
        while (1);
    }

    mem_setVramAMapping(MEM_VRAM_AB_LCDC);
    mem_setVramCMapping(MEM_VRAM_C_LCDC);
    mem_setVramDMapping(MEM_VRAM_D_LCDC);

    loadFile(PICO_LOADER_9_PATH, (void*)0x06800000);
    loadFile(PICO_LOADER_7_PATH, (void*)0x06840000);

    ((pload_header7_t*)0x06840000)->bootDrive = PLOAD_BOOT_DRIVE_DLDI;
    ((pload_header7_t*)0x06840000)->dldiDriver = gDldiStub;
    vu32* romPath = (vu32*)((pload_header7_t*)0x06840000)->loadParams.romPath;
    // fat:/_picoboot.nds
    romPath[0] = 0x3A746166; // :taf
    romPath[1] = 0x69705F2F; // ip_/
    romPath[2] = 0x6F626F63; // oboc
    romPath[3] = 0x6E2E746F; // n.to
    romPath[4] = 0x7364; //sd

    mem_setVramCMapping(MEM_VRAM_C_ARM7_00000);
    mem_setVramDMapping(MEM_VRAM_D_ARM7_20000);

    REG_EXMEMCNT |= 0x800; // cartridge to ARM7

    bootstrapArm7();
    ((pico_loader_9_func_t)0x06800000)();
}