#include <ultra64.h>
#include <global.h>

// TODO move out
#define	OS_CLOCK_RATE		62500000LL
#define	OS_CPU_COUNTER		(OS_CLOCK_RATE*3/4)
#define OS_USEC_TO_CYCLES(n)	(((u64)(n)*(OS_CPU_COUNTER/15625LL))/(1000000LL/15625LL))

vs32 gIrqMgrResetStatus = 0;
volatile OSTime sIrqMgrResetTime = 0;
volatile OSTime sIrqMgrRetraceTime = 0;
s32 sIrqMgrRetraceCount = 0;

void IrqMgr_AddClient(IrqMgr* irqmgr, OSMesgQueueListNode* param_2, OSMesgQueue* param_3) {
    u32 saveMask;

    saveMask = osSetIntMask(1);

    param_2->queue = param_3;
    param_2->next = irqmgr->callbacks;
    irqmgr->callbacks = param_2;

    osSetIntMask(saveMask);

    if (irqmgr->prenmiStage > 0) {
        osSendMesg(param_2->queue, &irqmgr->prenmiMsg.type, 0);
    }
    if (irqmgr->prenmiStage > 1) {
        osSendMesg(param_2->queue, &irqmgr->nmiMsg.type, 0);
    }
}

void IrqMgr_RemoveClient(IrqMgr* irqmgr, OSMesgQueueListNode* remove) {
    OSMesgQueueListNode* iter;
    OSMesgQueueListNode* last;
    u32 saveMask;

    iter = irqmgr->callbacks;
    last = NULL;

    saveMask = osSetIntMask(1);

    while (iter != NULL) {
        if (iter == remove) {
            if (last != NULL) {
                last->next = remove->next;
            } else {
                irqmgr->callbacks = remove->next;
            }
            break;
        }
        last = iter;
        iter = iter->next;
    }

    osSetIntMask(saveMask);
}

void IrqMgr_SendMesgForClient(IrqMgr* irqmgr, OSMesg msg) {
    OSMesgQueueListNode* iter = irqmgr->callbacks;

    while (iter != NULL) {
        osSendMesg(iter->queue, msg, 0);
        iter = iter->next;
    }
}

void IrqMgr_JamMesgForClient(IrqMgr* irqmgr, OSMesg msg) {
    OSMesgQueueListNode* iter = irqmgr->callbacks;

    while (iter != NULL) {
        if (iter->queue->validCount < iter->queue->msgCount) {
            osSendMesg(iter->queue, msg, 0);
        }
        iter = iter->next;
    }
}

void IrqMgr_HandlePreNMI(IrqMgr* irqmgr) {
    gIrqMgrResetStatus = 1;
    irqmgr->prenmiStage = 1;

    sIrqMgrResetTime = irqmgr->lastPrenmiTime = osGetTime();

    // Wait .45 seconds then generate a stage 2 prenmi interrupt
    osSetTimer(&irqmgr->prenmiTimer, OS_USEC_TO_CYCLES(450000), 0, &irqmgr->irqQueue, (OSMesg)0x29F);

    IrqMgr_JamMesgForClient(irqmgr, &irqmgr->prenmiMsg.type);
}

void IrqMgr_CheckStack(void) {
    StackCheck_Check(NULL);
}

void IrqMgr_HandlePRENMI450(IrqMgr* irqmgr) {
    gIrqMgrResetStatus = 2;
    irqmgr->prenmiStage = 2;

    // Wait .03 seconds then generate a stage 3 prenmi interrupt
    osSetTimer(&irqmgr->prenmiTimer, OS_USEC_TO_CYCLES(30000), 0, &irqmgr->irqQueue, (OSMesg)0x2A0);

    IrqMgr_SendMesgForClient(irqmgr, &irqmgr->nmiMsg.type);
}

void IrqMgr_HandlePRENMI480(IrqMgr* irqmgr) {
    // Wait .52 seconds. After this we will have waited an entire second
    osSetTimer(&irqmgr->prenmiTimer, OS_USEC_TO_CYCLES(520000), 0, &irqmgr->irqQueue, (OSMesg)0x2A1);

    osAfterPreNMI();
}

void IrqMgr_HandlePRENMI500(IrqMgr* irqmgr) {
    IrqMgr_CheckStack();
}
void IrqMgr_HandleRetrace(IrqMgr* irqmgr) {
    if (sIrqMgrRetraceTime == 0) {
        if (irqmgr->lastFrameTime == 0) {
            irqmgr->lastFrameTime = osGetTime();
        } else {
            sIrqMgrRetraceTime = osGetTime() - irqmgr->lastFrameTime;
        }
    }

    sIrqMgrRetraceCount += 1;
    IrqMgr_SendMesgForClient(irqmgr,irqmgr);
}

void IrqMgr_ThreadEntry(IrqMgr* irqmgr) {
    u32 interrupt;
    u32 stop;

    interrupt = 0;
    stop = 0;
    while (stop == 0) {
        if (stop);

        osRecvMesg(&irqmgr->irqQueue, (OSMesg*)&interrupt, 1);
        switch (interrupt) {
        case 0x29A:
            IrqMgr_HandleRetrace(irqmgr);
            break;
        case 0x29D:
            IrqMgr_HandlePreNMI(irqmgr);
            break;
        case 0x29F:
            IrqMgr_HandlePRENMI450(irqmgr);
            break;
        case 0x2A0:
            IrqMgr_HandlePRENMI480(irqmgr);
            break;
        case 0x2A1:
            IrqMgr_HandlePRENMI500(irqmgr);
            break;
        }
    }
}

void IrqMgr_Init(IrqMgr* irqmgr, void* stack, OSPri pri, u8 retraceCount) {
    irqmgr->callbacks = NULL;
    irqmgr->verticalRetraceMesg.type = 1;
    irqmgr->prenmiMsg.type = 4;
    irqmgr->nmiMsg.type = 3;
    irqmgr->prenmiStage = 0;
    irqmgr->lastPrenmiTime = 0;

    osCreateMesgQueue(&irqmgr->irqQueue,(OSMesg *)irqmgr->irqBuffer,8);
    osSetEventMesg(0xE, &irqmgr->irqQueue, (OSMesg)0x29D);
    osViSetEvent(&irqmgr->irqQueue, (OSMesg)0x29A, retraceCount);

    osCreateThread(&irqmgr->thread, 0x13, (osCreateThread_func)IrqMgr_ThreadEntry, irqmgr, stack, pri);
    osStartThread(&irqmgr->thread);
}