#include "draw_buffers.h"
#include "draw_primitives.h"
#include "graph_vram.h"
#include "gs_psm.h"
#include <tamtypes.h>
#include <sifrpc.h>
#include <debug.h>
#include <unistd.h>
#include <dma.h>
#include <graph.h>
#include <draw.h>
#include <stdio.h>
#include <inttypes.h>
#include <gs_gp.h>
#include <stdlib.h>
#include "gs.h"

#define myftoi4(x) ((x)<<4)

#ifdef DEBUG
#define DEBUG_MSG(...) printf(__VA_ARGS__)
#else
#define DEBUG_MSG(...)
#endif


#define OFFSET_X 2048
#define OFFSET_Y 2048

#define VID_W 640
#define VID_H 448
#define vid_buf_size (100 * 16)

int print_buffer(qword_t *b, int len) {
    DEBUG_MSG("-- buffer\n");
    for(int i = 0; i < len; i++) {
        DEBUG_MSG("%016llx %016llx\n", b->dw[0], b->dw[1]);
        b++;
    }
    DEBUG_MSG("-- /buffer\n");
    return 0;
}

static int verts[] = {
    100, 100, 0,
    300, 100, 0,
    100, 300, 0,
    300, 100, 0,
    300, 300, 0,
    100, 300, 0
};

#define SHIFT_AS_I64(x, b) (((uint64_t)x)<<b);

uint64_t red = 0xf0;
uint64_t blue = 0x0f;
uint64_t green = 0xf0;
qword_t *draw(qword_t *q) {
    q->dw[0] = 0xD000000000000001;
    q->dw[1] = 0x0005151515151510;
    q++;

    q->dw[0] = GS_PRIM_TRIANGLE;
    q->dw[1] = 0;
    q++;

    int cx = myftoi4(2048 - (VID_W/2));
    int cy = myftoi4(2048 - (VID_H/2));

    for(int i = 0; i < 6; i++){
        q->dw[0] = (red&0xff) | (green&0xff)<<32;
        q->dw[1] = (blue&0xff) | SHIFT_AS_I64(0x80, 32);
        q++;

        int base = i*3;
        int x = myftoi4(verts[base+0]) + cx;
        int y = myftoi4(verts[base+1]) + cy;
        int z = myftoi4(verts[base+2]);
        q->dw[0] = x | SHIFT_AS_I64(y, 32);
        q->dw[1] = z; 
        q++;
    }
    return q;
}

int main() {
#ifdef DEBUG
    sceSifInitRpc(0);
    init_scr();
#endif
    DEBUG_MSG("PS2 Initialized\n");
    // initialize DMAC
    dma_channel_initialize(DMA_CHANNEL_GIF, 0, 0);
    dma_channel_fast_waits(DMA_CHANNEL_GIF);
    DEBUG_MSG("DMA Initialized...\n");
    // initialize graphics mode
    struct draw_state state = {0};
    state.width = VID_W;
    state.height = VID_H;
    state.gmode = GRAPH_MODE_INTERLACED;
    state.vmode = GRAPH_MODE_NTSC;
    gs_init(&state, GS_PSM_32, GS_PSMZ_24);
    DEBUG_MSG("GS Initialized...\n");
    // clear screen

    // build buffer with triangle data

    qword_t *buf = malloc(vid_buf_size);
    graph_wait_vsync();
    while(1) {
        dma_wait_fast();
        red++;
        if(red > 0xFF) {
            red = 0;
        }
        qword_t *q = buf;
        q = draw_disable_tests(q, 0, &state.zb);
        q = draw_clear(q, 0, 2048.0f - 320, 2048.0f - 244, VID_W, VID_H, 20, 20, 20);
        q = draw(q);
        q = draw_finish(q);
        dma_channel_send_normal(DMA_CHANNEL_GIF, buf, q-buf, 0, 0);
        draw_wait_finish();

        graph_wait_vsync();
    }
}