#include <gint/display.h>
#include <gint/keyboard.h>
#include <gint/mpu/spu.h>
#include <gint/mpu/cpg.h>
#include <gint/mpu/power.h>
#include <gint/clock.h>
#include <gint/intc.h>
#include <stdarg.h>
#include <string.h>

#define POWER SH7305_POWER
#define CPG   SH7305_CPG
#define SPU   SH7305_SPU
#define PRAM ((volatile unsigned char *)0xFE200000)
#define SYNCO() __asm__ volatile("SYNCO\n\t":::"memory");

#define DSP0  SH7305_DSP0
#define DSP1  SH7305_DSP1
#define PRINT_INIT \
    dclear(C_WHITE); \
    uint8_t fontIncr = dfont_default()->line_height + 1, fontPos = 0;

#define PRINT(...) \
    do { \
        dprint(1, fontPos += fontIncr, C_BLACK, __VA_ARGS__); \
        if (fontPos >= DHEIGHT - fontIncr) { \
            dprint(8, DWIDTH - 20, C_BLACK, "F"); \
            dupdate(); \
            getkey(); \
            dclear(C_WHITE); \
            fontPos = 0; \
        } \
        dupdate(); \
    } while (0);

#define PRINT_REG(dev, name) PRINT(#name ": %x", dev.name)


// From https://github.com/amdev01/android_kernel_samsung_afyonlte/
/* dsp core reset register (DSPCORERST) */
#define DSPCORERST_ACTIVE        0x00000000
#define DSPCORERST_RESET        0x00000001

/* cpu interrupt source mask register (IEMASKC) */
#define IEMASKC_DISABLE        0x00000111
#define IEMASKC_ENABLE        0x00001111

/* cpu interrupt signal mask register (IMASKC) */
#define IMASKC_DSP_DISABLE    0x00000111
#define IMASKC_DSP_ENABLE        0x00001111


static volatile bool g_interrupt_occured = false;


static void dsp_interrupt_handler() {
    g_interrupt_occured = true;
}

static void init_spu() {
    // From GINT kernel spu.c
    
    /* Stop both the SPU and FSI clocks */
    CPG.FSICLKCR.lword = 0x00000103;
    CPG.SPUCLKCR.lword = 0x00000100;
    /* Enable the FSI clock, then the SPU/SPURAM clock */
    CPG.FSICLKCR.CLKSTP = 0;
    CPG.SPUCLKCR.CLKSTP = 0;

    /* Power the clocks through MSTPCR2 */
    POWER.MSTPCR2.FSI_SPU = 0;

    // TODO: Probably not necessary?
    /* Reset the SPU */
     SPU.SPUSRST.RST = 0;
    sleep_us_spin(1000);
    SPU.SPUSRST.RST = 1;
    sleep_us_spin(1000);

    /* Initially give all P memory and X memory to DSP0 */
    SPU.PBANKC0 = 0x1f;
    SPU.PBANKC1 = 0x00;
    SPU.XBANKC0 = 0x7f;
    SPU.XBANKC1 = 0x00;
    sleep_us_spin(1000);
}

static void dsp_full_reset() {
    DSP0.DSPRST = 0;
    sleep_us_spin(1000);
}

static void dsp_core_reset() {
    /* Unmask interrupts coming from DSP */
    DSP0.IEMASKC = IEMASKC_DISABLE;
    DSP0.IMASKC = IMASKC_DSP_ENABLE;

//    DSP0.DSPRESTART = 0x10; TODO: needed?
    DSP0.DSPCORERST = 0;
    sleep_us_spin(1000);
}

int main()
{
    PRINT_INIT

    auto load_pram{[&] () {
        for (int i{}; i < 0x20; i++)
            PRINT("PRAM%d: 0x%x", i, (unsigned int)PRAM[i])
    }};

    auto store_pram{[&] () {
        for (int i{}; i < 0x100; i++)
            PRAM[i] = 0xDD;
        
        load_pram();
    }};


    store_pram();
    init_spu();
    PRINT("INIT SPU DONE")

    /* Take DSP block out of reset */
    dsp_full_reset();
    PRINT("DSP RESET DONE")

    /* Register the IRQ handler */
    intc_handler_function(0xcc0, GINT_CALL(dsp_interrupt_handler));
    intc_handler_function(0xcc1, GINT_CALL(dsp_interrupt_handler));

    intc_priority(INTC_SPU_DSP0, 2); // TODO: what priority?
    intc_priority(INTC_SPU_DSP1, 0);
    PRINT("IRQ ENABLE DONE")

    /* Take DSP core out of reset */
    store_pram();
    dsp_core_reset();
    PRINT("DSP CORE RESET DONE")

    // TODO: This will crash unless I comment out a prior store_pram call, is calling PRINT so much resulting in an overflow?
    // store_pram();

    PRINT("IRQ: %d", g_interrupt_occured)

    PRINT_REG(DSP0, DSPRST)
    PRINT_REG(DSP0, DSPCORERST)
    PRINT_REG(DSP0, DSPHOLD)
    PRINT_REG(DSP0, DSPRESTART)
    PRINT_REG(DSP0, IEMASKC)
    PRINT_REG(DSP0, IMASKC)
    PRINT_REG(DSP0, IEVENTC)
    PRINT_REG(DSP0, IEMASKD)
    PRINT_REG(DSP0, IMASKD)
    PRINT_REG(DSP0, IESETD)
    PRINT_REG(DSP0, IECLRD)
    PRINT_REG(DSP0, COM[0])
    PRINT_REG(DSP0, COM[1])
    PRINT_REG(DSP0, COM[2])
    PRINT_REG(DSP0, COM[3])
    PRINT_REG(DSP0, COM[4])
    PRINT_REG(DSP0, COM[5])
    PRINT_REG(DSP0, COM[6])
    PRINT_REG(DSP0, COM[7])
    PRINT_REG(DSP0, SPUSTS)
#undef PRINT_REG

    getkey();
    return 1;
}