#include "Vcpu.h"
#include "Vcpu_cpu.h"
#include "verilated.h"

#include "SDL/SDL.h"

#include <sys/time.h>

SDL_Surface *screen;
Vcpu *top;

void putpixel(SDL_Surface *surface, uint16_t x, uint16_t y, uint32_t value)
{
    uint8_t *pixel = (uint8_t *)surface->pixels;
    pixel += (y * surface->pitch) + (x * sizeof(uint32_t));
    (*((uint32_t *)pixel)) = value;
}

uint8_t collision[64*32];
void putpixel_c8(SDL_Surface *surface, uint8_t x, uint8_t y, uint8_t value)
{
    value = collision[x + y * 64] ^ value;
    if (collision[x + y * 64] == 0 && value == 1)
        top->v->vr[15] = 1;
    else
        top->v->vr[15] = 0;
    collision[x + y * 64] = value;
    for (uint32_t xx = x*10; xx < (x+1)*10; xx++)
    {
        for (uint32_t yy = y * 10; yy < (y+1)*10; yy++)
        {
            if (value > 0)
                putpixel(surface, xx, yy, 0xFFFFFF);
            else
                putpixel(surface, xx, yy, 0);
        }
    }
}

void dump_regs(void)
{
    printf("-> regs:\n");
    for (int i = 0; i < 8; i++)
        printf("  %i: 0x%02x\n", i, top->v->vr[i]);
}

void step(void)
{
    top->cpu_clock = 0;
    top->eval();
    top->cpu_clock = 1;
    top->eval();
    // emulate draw instructions :>
    if (top->v->c == 2)
    {
        uint16_t ins = top->v->ins;
        if (((ins >> 12) & 0xF) == 0xD)
        {
            uint8_t x = top->v->vr[(ins >> 8) & 0xF];
            uint8_t y = top->v->vr[(ins >> 4) & 0xF];
            uint8_t n = ins & 0xF;
            uint16_t i = top->v->i;
            printf("[i] Drawing %i bytes from ram[%i] at %i, %i.\n", n, i, x, y);
            for (uint8_t nn = 0; nn < n; nn++)
            {
                uint8_t byte = top->v->ram[i];
                i++;
                for (uint8_t bit = 0; bit < 8; bit++)
                {
                    uint8_t pixel = (byte >> (7-bit)) & 1;
                    putpixel_c8(screen, x+bit, y+nn, pixel);
                }
            }
        }
    }
}

void step3(void)
{
    do
    {
        step();
    } while (top->v->c != 0);
}

void run_instruction(uint16_t instruction)
{
    top->v->ram[0x200] = instruction >> 8;
    top->v->ram[0x201] = instruction & 0xFF;
    top->v->pc = 0x200;
}

void reset(void)
{
    top->hertz_clock = 0;
    top->reset = 0;
    step3(); step3();
    top->reset = 1;
}

int main(int argc, char **argv)
{
    memset(collision, 0, 64*32);
    SDL_Init(SDL_INIT_EVERYTHING);
    screen = SDL_SetVideoMode( 640, 320, 32, SDL_SWSURFACE );

    Verilated::commandArgs(argc, argv);
    top = new Vcpu;
    reset();

    // load digits into memory starting from zero
    // 0
    top->v->ram[0] = 0xF0;
    top->v->ram[1] = 0x90;
    top->v->ram[2] = 0x90;
    top->v->ram[3] = 0x90;
    top->v->ram[4] = 0xF0;

    // 1
    top->v->ram[5] = 0x20;
    top->v->ram[6] = 0x60;
    top->v->ram[7] = 0x20;
    top->v->ram[8] = 0x20;
    top->v->ram[9] = 0x70;

    // 2
    top->v->ram[10] = 0xF0;
    top->v->ram[11] = 0x10;
    top->v->ram[12] = 0xF0;
    top->v->ram[13] = 0x80;
    top->v->ram[14] = 0xF0;

    // 3
    top->v->ram[15] = 0xF0;
    top->v->ram[16] = 0x10;
    top->v->ram[17] = 0xF0;
    top->v->ram[18] = 0x10;
    top->v->ram[19] = 0xF0;
    
    // 4
    top->v->ram[20] = 0x90;
    top->v->ram[21] = 0x90;
    top->v->ram[22] = 0xF0;
    top->v->ram[23] = 0x10;
    top->v->ram[24] = 0x10;
    
    // 5
    top->v->ram[25] = 0xF0;
    top->v->ram[26] = 0x80;
    top->v->ram[27] = 0xF0;
    top->v->ram[28] = 0x10;
    top->v->ram[29] = 0xF0;
    
    // 6
    top->v->ram[30] = 0xF0;
    top->v->ram[31] = 0x80;
    top->v->ram[32] = 0xF0;
    top->v->ram[33] = 0x90;
    top->v->ram[34] = 0xF0;
    
    // 7
    top->v->ram[35] = 0xF0;
    top->v->ram[36] = 0x10;
    top->v->ram[37] = 0x20;
    top->v->ram[38] = 0x40;
    top->v->ram[39] = 0x40;
    
    // 8
    top->v->ram[40] = 0xF0;
    top->v->ram[41] = 0x90;
    top->v->ram[42] = 0xF0;
    top->v->ram[43] = 0x90;
    top->v->ram[44] = 0xF0;
    
    // 9
    top->v->ram[45] = 0xF0;
    top->v->ram[46] = 0x90;
    top->v->ram[47] = 0xF0;
    top->v->ram[48] = 0x10;
    top->v->ram[49] = 0xF0;
    
    // a
    top->v->ram[50] = 0xF0;
    top->v->ram[51] = 0x90;
    top->v->ram[52] = 0xF0;
    top->v->ram[53] = 0x90;
    top->v->ram[54] = 0x90;
    
    // b
    top->v->ram[55] = 0xE0;
    top->v->ram[56] = 0x90;
    top->v->ram[57] = 0xE0;
    top->v->ram[58] = 0x90;
    top->v->ram[59] = 0xE0;
    
    // c
    top->v->ram[60] = 0xF0;
    top->v->ram[61] = 0x80;
    top->v->ram[62] = 0x80;
    top->v->ram[63] = 0x80;
    top->v->ram[64] = 0xF0;
    
    // d
    top->v->ram[65] = 0xE0;
    top->v->ram[66] = 0x90;
    top->v->ram[67] = 0x90;
    top->v->ram[68] = 0x90;
    top->v->ram[69] = 0xE0;
    
    // e
    top->v->ram[70] = 0xF0;
    top->v->ram[71] = 0x80;
    top->v->ram[72] = 0xF0;
    top->v->ram[73] = 0x80;
    top->v->ram[74] = 0xF0;
    
    // f
    top->v->ram[75] = 0xF0;
    top->v->ram[76] = 0x80;
    top->v->ram[77] = 0xF0;
    top->v->ram[78] = 0x80;
    top->v->ram[79] = 0x80;

    if (argc < 2)
    {
        fprintf(stderr, "Usage: %s romfile\n", argv[0]);
        return 1;
    }
    FILE *f = fopen(argv[1], "r");
    fread(top->v->ram + 0x200, 1, 0xe00, f);
    fclose(f);

    top->v->pc = 0x200;

    int last_tick = 0;
    int ticks_per_sec = 0;
    while (true)
    {
        struct timeval tv;
        gettimeofday(&tv, NULL); 
        int time = tv.tv_sec;

        ticks_per_sec++;
        if (time != last_tick)
        {
            printf("[i] %i Hz emulation\n", ticks_per_sec);
            ticks_per_sec = 0;
            printf("[i] DT: %i\n", top->v->dt);
            top->hertz_clock = 1;
        }
        last_tick = time;
        step3();
        if (top->hertz_clock)
            top->hertz_clock = 0;
        SDL_Flip(screen);
    }

}