#include "shorthand.h"
#include "bd-font.c"
#include "gba-buttons.c"

//
// Memory sections.
//

// Defines for the different memory sections in the GBA.
#define MEM_SROM  0x00000000
#define MEM_EW    0x02000000
#define MEM_IW    0x03000000
#define MEM_IO    0x04000000
#define MEM_PAL   0x05000000
#define MEM_VRAM  0x06000000
#define MEM_OAM   0x07000000
#define MEM_PAK   0x08000000
#define MEM_CART  0x0E000000

//
// Display modes.
//

// Display registers.
#define DISP_CTRL   *((vu32*)(MEM_IO + 0x0000))
#define DISP_STATUS *((vu32*)(MEM_IO + 0x0004))
#define DISP_VCOUNT *((vu32*)(MEM_IO + 0x0006))

// The first three bits in the DISP_CTRL are used to control the video mode.
#define DISP_MODE_0     0x0000
#define DISP_MODE_1     0x0001
#define DISP_MODE_2     0x0002
#define DISP_MODE_3     0x0003
#define DISP_MODE_4     0x0004
#define DISP_MODE_5     0x0005
#define DISP_GB         (1 << 3)
#define DISP_PAGE       (1 << 4)
#define DISP_OAM_HBLANK (1 << 5)
#define DISP_OBJ_1D     (1 << 6)
#define DISP_BLANK      (1 << 7)
#define DISP_BG_0       (1 << 8)
#define DISP_BG_1       (1 << 9)
#define DISP_BG_2       (1 << 10)
#define DISP_BG_3       (1 << 11)
#define DISP_OBJ        (1 << 12)
#define DISP_ENABLE_SPRITES DISP_OBJ | DISP_OBJ_1D

// Registers to control of BG layers.
#define BG_CTRL_0 *((vu16*)(0x04000008 + 0x0000))
#define BG_CTRL_1 *((vu16*)(0x04000008 + 0x0002))
#define BG_CTRL_2 *((vu16*)(0x04000008 + 0x0004))
#define BG_CTRL_3 *((vu16*)(0x04000008 + 0x0006))

// Screen settings.
#define SCREEN_WIDTH 240
#define SCREEN_HEIGHT 160

// The GBA in mode 3 expects rbg15 colors in the VRAM, where each component
// (RGB) have a 0--31 range. For example, pure red would be rgb15(31, 0, 0).
typedef u16 Color;

//
// Tile memory access.
//

// NOTE: Only defining 4bpp tiles for now.
typedef struct Tile {
    u32 data[8];
} Tile;

typedef Tile TileBlock[512];

#define TILE_MEM ((TileBlock*) MEM_VRAM)

// We can treat the screen as a HxW matrix. With the following macro we can
// write a pixel to the screen at the (x, y) position using:
//
//     FRAMEBUFFER[y][x] = color;
//
typedef Color Scanline[SCREEN_WIDTH];
#define FRAMEBUFFER ((Scanline*)MEM_VRAM)
#define SCREEN_BUFFER ((vu16*) MEM_VRAM)
#define PAL_BUFFER_BG ((vu16*) MEM_PAL)
#define PAL_BUFFER_SPRITES ((vu16*) 0x05000200)

//
// Colors.
//

static inline Color
rgb15(u32 red, u32 green, u32 blue ) {
    return (blue << 10) | (green << 5) | red;
}

#define COLOR_RED   rgb15(31, 0, 12)
#define COLOR_BLUE  rgb15(2, 15, 30)
#define COLOR_CYAN  rgb15(0, 30, 30)
#define COLOR_GREY  rgb15(4, 4, 4)
#define COLOR_BLACK rgb15(0, 0, 0)
#define COLOR_WHITE rgb15(28, 28, 28)

//
// Sprites.
//

// Using macros instead of aligned structs for setting up OBJ attributes and
// affine parameters.
// TODO: Benchmark if this would be slower or the same that TONC's
// implementation.
#define OBJ_ATTR_0(N)    *((vu16*)(MEM_OAM + 0 + 8 * (N)))
#define OBJ_ATTR_1(N)    *((vu16*)(MEM_OAM + 2 + 8 * (N)))
#define OBJ_ATTR_2(N)    *((vu16*)(MEM_OAM + 4 + 8 * (N)))
#define OBJ_AFFINE_PA(N) *((vs16*)(MEM_OAM + 6 + 8 * 0 + 8 * 4 * (N)))
#define OBJ_AFFINE_PB(N) *((vs16*)(MEM_OAM + 6 + 8 * 1 + 8 * 4 * (N)))
#define OBJ_AFFINE_PC(N) *((vs16*)(MEM_OAM + 6 + 8 * 2 + 8 * 4 * (N)))
#define OBJ_AFFINE_PD(N) *((vs16*)(MEM_OAM + 6 + 8 * 3 + 8 * 4 * (N)))

// Using bd-font, an 8x8 bitmap font.
static void
put_char(int x, int y, Color clr, u8 chr) {
    for (size_t i = 0; i < 8; ++i) {
        for (size_t j = 0; j < 8; ++j) {
            if ((font[chr][i] >> (7 - j)) & 0x1) {
                FRAMEBUFFER[y + i][x + j] = clr;
            }
        }
    }
}

static void
put_text(int x, int y, Color clr, char *msg) {
    int count = 0;
    while (*msg) {
        put_char(x + count, y, clr, *msg++);
        count += 8;
    }
}

// Draws a line with the given color between (x0,y0) and (x1,y1) using the
// Bresenham's line drawing algorithm using exclusively integer arithmetic.
static void
draw_line(int x0, int y0, int x1, int y1, Color clr) {
    // Pointer to the initial position of the screen buffer where we will start
    // writing our data.
    vu16 *destination = (u16*)(SCREEN_BUFFER + y0 * SCREEN_WIDTH + x0);

    // Adjust the step direction and calculate deltas.
    int x_step;
    int y_step;
    int dx;
    int dy;
    if (x0 > x1) {
        x_step = -1;
        dx = x0 - x1;
    } else {
        x_step = 1;
        dx = x1 - x0;
    }
    if (y0 > y1) {
        y_step = -SCREEN_WIDTH;
        dy = y0 - y1;
    } else {
        y_step = +SCREEN_WIDTH;
        dy = y1 - y0;
    }

    if(dy == 0) {
        // Horizontal line.
        for(int i = 0; i <= dx; i++) {
            destination[i * x_step] = clr;
        }
    } else if(dx == 0) {
        // Vertical line.
        for(int i = 0; i <= dy; i++) {
            destination[i * y_step] = clr;
        }
    } else if (dx >= dy){
        // Positive slope.
        int diff = 2 * dy - dx;
        for (int i = 0; i <= dx; ++i) {
            *destination = clr;
            if (diff >= 0) {
                destination += y_step;
                diff -= 2 * dx;
            }
            destination += x_step;
            diff += 2 * dy;
        }
    } else {
        // Negative slope.
        int diff = 2 * dx - dy;
        for (int i = 0; i <= dy; ++i) {
            *destination = clr;
            if (diff >= 0) {
                destination += x_step;
                diff -= 2 * dy;
            }
            destination += y_step;
            diff += 2 * dx;
        }
    }
}

static inline void
draw_rect(int x0, int y0, int x1, int y1, Color clr) {
    if (x0 > x1) {
        int tmp = x0;
        x0 = x1;
        x1 = tmp;
    }
    if (y0 > y1) {
        int tmp = y0;
        y0 = y1;
        y1 = tmp;
    }
    int dx = x1 - x0;
    int dy = y1 - y0;
    for (int i = 0; i <= dx; ++i) {
        int x = x0 + i;
        FRAMEBUFFER[y0][x] = clr;
        FRAMEBUFFER[y1][x] = clr;
    }
    for (int j = 0; j <= dy; ++j) {
        int y = y0 + j;
        FRAMEBUFFER[y][x0] = clr;
        FRAMEBUFFER[y][x1] = clr;
    }
}

static inline void
draw_fill_rect(int x0, int y0, int x1, int y1, Color clr) {
    if (x0 > x1) {
        int tmp = x0;
        x0 = x1;
        x1 = tmp;
    }
    if (y0 > y1) {
        int tmp = y0;
        y0 = y1;
        y1 = tmp;
    }
    int dx = x1 - x0;
    int dy = y1 - y0;
    for (int i = 0; i <= dx; ++i) {
        for (int j = 0; j <= dy; ++j) {
            int x = x0 + i;
            int y = y0 + j;
            FRAMEBUFFER[y][x] = clr;
        }
    }
}

static inline void
wait_vsync(void) {
    while(DISP_VCOUNT >= 160);
    while(DISP_VCOUNT < 160);
}

//
// Main functions.
//

// In Mode4 the buffer is of 8 bytes per pixel instead of 16. We can't write the
// color directly, instead the color is stored in the palette memory at
// `MEM_PAL`. Note that in this mode MEM_PAL[0] is the background color. This
// plotter takes an index to a color stored in MEM_PAL[col_index]. Because the
// GBA needs to meet memory alignment requirements, we can't write a u8 into
// memory, instead we need to read a u16 word, mask and or the corresponding
// bits and wave the updated u16.
static void
put_pixel_m4(int x, int y, u8 col_index, vu16 *buffer) {
    int buffer_index = (y * SCREEN_WIDTH + x) / 2;
    vu16 *destination = &buffer[buffer_index];
    // Odd pixels will go to the top 8 bits of the destination. Even pixels to
    // the lower 8 bits.
    int odd = x & 0x1;
    if(odd) {
        *destination= (*destination & 0xFF) | (col_index << 8);
    } else {
        *destination= (*destination & ~0xFF) |  col_index;
    }
}

static void
draw_fill_rect_m4(int x0, int y0, int x1, int y1, u8 col_index, vu16 *buffer) {
    int ix, iy;
    for(iy = y0; iy < y1; iy++) {
        for(ix = x0; ix < x1; ix++) {
            put_pixel_m4(ix, iy, col_index, buffer);
        }
    }
}

static inline void
flip_page(void) {
    DISP_CTRL ^= DISP_PAGE;
}

#define SCREEN_PAGE_1 ((vu16*) MEM_VRAM)
#define SCREEN_PAGE_2 ((vu16*) (MEM_VRAM + 0xa000))

//
// Profiling.
//

#define TIMER_DATA_0  *((vu16*) (0x04000100 + 0x04 * 0))
#define TIMER_DATA_1  *((vu16*) (0x04000100 + 0x04 * 1))
#define TIMER_DATA_2  *((vu16*) (0x04000100 + 0x04 * 2))
#define TIMER_DATA_3  *((vu16*) (0x04000100 + 0x04 * 3))
#define TIMER_CTRL_0  *((vu16*) (0x04000102 + 0x04 * 0))
#define TIMER_CTRL_1  *((vu16*) (0x04000102 + 0x04 * 1))
#define TIMER_CTRL_2  *((vu16*) (0x04000102 + 0x04 * 2))
#define TIMER_CTRL_3  *((vu16*) (0x04000102 + 0x04 * 3))

// Timer control bits.
#define TIMER_CTRL_FREQ_0   0
#define TIMER_CTRL_FREQ_1   1
#define TIMER_CTRL_FREQ_2   2
#define TIMER_CTRL_FREQ_3   3
#define TIMER_CTRL_CASCADE (1 << 2)
#define TIMER_CTRL_IRQ     (1 << 6)
#define TIMER_CTRL_ENABLE  (1 << 7)

// We use timers 2 and 3 to count the number of cycles since the profile_start
// functions is called. Don't use if the code we are trying to profile make use
// of these timers.
static inline
void profile_start(void) {
    TIMER_DATA_2 = 0;
    TIMER_DATA_3 = 0;
    TIMER_CTRL_2 = 0;
    TIMER_CTRL_3 = 0;
    TIMER_CTRL_3 = TIMER_CTRL_ENABLE | TIMER_CTRL_CASCADE;
    TIMER_CTRL_2 = TIMER_CTRL_ENABLE;
}

static inline
u32 profile_stop(void) {
   TIMER_CTRL_2 = 0;
   return (TIMER_DATA_3 << 16) | TIMER_DATA_2;
}

//
// Input handling.
//

// Memory address for key input register
#define KEY_INPUTS *((vu16*) 0x04000130)

// Alias for key pressing bits.
#define KEY_A      (1 << 0)
#define KEY_B      (1 << 1)
#define KEY_SELECT (1 << 2)
#define KEY_START  (1 << 3)
#define KEY_RIGHT  (1 << 4)
#define KEY_LEFT   (1 << 5)
#define KEY_UP     (1 << 6)
#define KEY_DOWN   (1 << 7)
#define KEY_R      (1 << 8)
#define KEY_L      (1 << 9)

#define KEY_MASK 0x03FF

// Saving the previous and current key states as globals for now.
static u16 key_curr = 0;
static u16 key_prev = 0;

static inline void
poll_keys(void) {
    key_prev = key_curr;
    key_curr = ~KEY_INPUTS & KEY_MASK;
}

// Returns true if the given key has been pressed at time of calling and was not
// pressed since the previous call. For example, if a key is being held, this
// function will return `true` only on the frame where the key initially
// activated.
static inline u32
key_pressed(u32 key) {
    return (key_curr & key) & ~(key_prev & key);
}

// Check if the given key is pressed and has been since at least one frame.
static inline u32
key_hold(u32 key) {
    return (key_curr & key) & key_prev & key;
}

// Check if the given key/button is currently pressed.
#define KEY_PRESSED(key) (~(KEY_INPUTS) & key)

void
draw_logo(void) {
    int side = 60;
    int line = 35;
    int height = side * 0.5;
    int x = SCREEN_WIDTH / 2 - height / 2;
    int y = SCREEN_HEIGHT / 2;

    // Draw red triangle.
    draw_line(x + height - 1, y - side / 2, x, y - 1, COLOR_RED);
    draw_line(x + height - 1, y + side / 2, x, y + 1, COLOR_RED);
    draw_line(x + height - 1, y - side / 2 + 1, x, y, COLOR_RED);
    draw_line(x + height - 1, y + side / 2 - 1, x, y, COLOR_RED);

    // Draw white triangle.
    draw_line(x, y - side / 2, x, y + side / 2, COLOR_WHITE);
    draw_line(x + 1, y - side / 2, x + height, y - 1, COLOR_WHITE);
    draw_line(x + 1, y + side / 2, x + height, y + 1, COLOR_WHITE);

    // Draw white line at triangle tip.
    draw_line(x + height, y - side / 2, x + height, y + side / 2, COLOR_WHITE);
    draw_line(x + height + 1, y - side / 2, x + height + 1, y + side / 2, COLOR_WHITE);

    // Double triangle line.
    draw_line(x - 1, y - side / 2, x - 1, y + side / 2, COLOR_WHITE);
    draw_line(x + 1, y - side / 2 + 1, x + height, y, COLOR_WHITE);
    draw_line(x + 1, y + side / 2 - 1, x + height, y, COLOR_WHITE);

    // Draw white lines.
    draw_line(x - line, y, x, y, COLOR_WHITE);
    draw_line(x + height, y, x + height + line, y, COLOR_WHITE);
    draw_line(x - line, y + 1, x, y + 1, COLOR_WHITE);
    draw_line(x + height, y + 1, x + height + line, y + 1, COLOR_WHITE);
}

void
copy_font_to_tile_memory(Tile *tile) {
    // Hex to bits translation table.
    const u32 conversion_u32[16] = {
        0x00000000, 0x00001000, 0x00000100, 0x00001100,
        0x00000010, 0x00001010, 0x00000110, 0x00001110,
        0x00000001, 0x00001001, 0x00000101, 0x00001101,
        0x00000011, 0x00001011, 0x00000111, 0x00001111,
    };
    for (size_t i = 0; i < 250; ++i) {
        for (size_t j = 0; j < 8; ++j) {
            u8 row = font[i][j];
            u32 tile_idx = 0x00000000;
            tile_idx = conversion_u32[row & 0xF] << 16;
            tile_idx |= conversion_u32[(row >> 4) & 0xF];
            (tile + i)->data[j] = tile_idx;
        }
    }
}

typedef struct Sprite {
    // A unique sprite identifier.
    size_t id;
    // The number of tiles for a single sprite frame.
    size_t n_tiles;
    // The starting tile of this sprite.
    size_t tile_start;
    // The associated palette bank for this sprite.
    size_t pal_bank;
} Sprite;

typedef struct ButtonSprite {
    int id;
    int x;
    int y;
    int frame;
    BtnState state;
} ButtonSprite;

#define NUM_SPRITES 128

Sprite sprites[NUM_SPRITES];

// Keeping track of unique sprites and current sprite memory pointer using
// global singletons.
size_t sprite_counter = 0;
size_t sprite_tile_counter = 0;
u32 *sprite_memory = NULL;

// Loads the sprite data into video memory and initialize the Sprite structure.
size_t
load_sprite_data(u32 *sprite_data, size_t n_tiles, size_t n_frames) {
    memcpy(sprite_memory, sprite_data, 8 * n_tiles * n_frames * sizeof(u32));
    sprite_memory += 8 * n_tiles * n_frames;
    Sprite sprite = {
        .id = sprite_counter,
        .n_tiles = n_tiles,
        .tile_start = sprite_tile_counter,
    };
    sprite_tile_counter += n_tiles * n_frames;
    sprites[sprite_counter] = sprite;
    return sprite_counter++;
}

int main(void) {
    // Configure the display in mode 0 to show OBJs, where tile memory is
    // sequential.
    DISP_CTRL = DISP_MODE_3 | DISP_ENABLE_SPRITES | DISP_BG_2;

    // Add colors to the sprite color palette. Tiles with color number 0 are
    // treated as transparent.
    for (size_t i = 0; i < 16; ++i) {
        PAL_BUFFER_SPRITES[i] = COLOR_WHITE;
    }

    // Initialize all attributes by disabling rendering. If we don't do this,
    // glitches may appear.
    for (size_t i = 0; i < 128; ++i) {
        OBJ_ATTR_0(i) = (1 << 9);
    }

    sprite_tile_counter = 512;
    sprite_memory = &TILE_MEM[4][sprite_tile_counter];

    // Initialize the A/B button sprites.
    int buttons_x = SCREEN_WIDTH - 64 - 10;
    int buttons_y = 120;
    ButtonSprite btn_b = {
        .id = load_sprite_data(&gba_btn_b_data, 16, 7),
        .x = buttons_x,
        .y = buttons_y,
        .frame = 0,
        .state = BTN_STATE_IDLE,
    };
    OBJ_ATTR_0(btn_b.id) = btn_b.y;
    OBJ_ATTR_1(btn_b.id) = btn_b.x | (1 << 0xF);
    OBJ_ATTR_2(btn_b.id) = sprites[btn_b.id].tile_start;

    ButtonSprite btn_a = {
        .id = load_sprite_data(&gba_btn_a_data, 16, 7),
        .x = buttons_x + 20,
        .y = buttons_y - 16,
        .frame = 0,
        .state = BTN_STATE_IDLE,
    };
    OBJ_ATTR_0(btn_a.id) = btn_a.y;
    OBJ_ATTR_1(btn_a.id) = btn_a.x | (1 << 0xF);
    OBJ_ATTR_2(btn_a.id) = sprites[btn_a.id].tile_start;

    draw_logo();

    int frame_counter = 0;
    while(true) {
        wait_vsync();
        poll_keys();

        // Toggle frame counter when we press down.
        if (key_pressed(KEY_DOWN) || key_hold(KEY_DOWN)) {
        }
        if (key_pressed(KEY_UP) || key_hold(KEY_UP)) {
        }
        if (key_pressed(KEY_LEFT) || key_hold(KEY_LEFT)) {
        }
        if (key_pressed(KEY_RIGHT) || key_hold(KEY_RIGHT)) {
        }
        if (key_pressed(KEY_B)) {
            btn_b.frame = 0;
            btn_b.state = BTN_STATE_PRESSED;
        } else if (key_hold(KEY_B)) {
            size_t n_frames = animation_states[btn_b.state]->n_frames;
            if (btn_b.frame < n_frames - 1) {
                btn_b.frame++;
            }
        } else {
            size_t n_frames = animation_states[btn_b.state]->n_frames;
            // Finish the animation and reset idle state.
            if (btn_b.frame > 0 && btn_b.frame < n_frames - 1) {
                btn_b.frame++;
            } else {
                btn_b.frame = 0;
                btn_b.state = BTN_STATE_IDLE;
            }
        }
        if (key_pressed(KEY_A)) {
            btn_a.frame = 0;
            btn_a.state = BTN_STATE_PRESSED;
        } else if (key_hold(KEY_A)) {
            size_t n_frames = animation_states[btn_a.state]->n_frames;
            if (btn_a.frame < n_frames - 1) {
                btn_a.frame++;
            }
        } else {
            size_t n_frames = animation_states[btn_a.state]->n_frames;
            // Finish the animation and reset idle state.
            if (btn_a.frame > 0 && btn_a.frame < n_frames - 1) {
                btn_a.frame++;
            } else {
                btn_a.frame = 0;
                btn_a.state = BTN_STATE_IDLE;
            }
        }
        if (key_pressed(KEY_L)) {
        }
        if (key_pressed(KEY_R)) {
        }

        OBJ_ATTR_2(btn_b.id) = sprites[btn_b.id].tile_start + animation_states[btn_b.state]->tile_offsets[btn_b.frame];
        OBJ_ATTR_2(btn_a.id) = sprites[btn_a.id].tile_start + animation_states[btn_a.state]->tile_offsets[btn_a.frame];
        frame_counter++;
    };

    return 0;
}