#include <stdint.h>
#include <string.h>
#include <conio.h>
#include <i86.h>
#include "video.h"
#include "memory.h"
#include "globals.h"
#include "keyboard.h"
#include "hercules.h"
#include "cga.h"
#include "pcplus.h"

void HGC_videoInit(int key){
	uint16_t i, j;

	VIDEO_BITSHIFT = 2;
	VIDEO_FILESHIFT = 2;
	VIDEO_SCREENSEG = 0xB000;
	// Select strobing cursor colors
	for (i = 0; i < 32; i++){
		if (i < 9){
			CURSOR_SELECTED_COLOR[i] = 0;
		} else if (i < 12){
			CURSOR_SELECTED_COLOR[i] = 2;
		} else if (i < 15){
			CURSOR_SELECTED_COLOR[i] = 1;
		} else if (i < 25){
			CURSOR_SELECTED_COLOR[i] = 3;
		} else if (i < 28){
			CURSOR_SELECTED_COLOR[i] = 1;
		} else{
			CURSOR_SELECTED_COLOR[i] = 2;
		}
	}
	j = 0;
    VIDEO_MODE = HGC;
    VIDEO_SCREENSIZE = 32406; // 720 x 348
    VIDEO_BITSHIFT = 3;
    VIDEO_FILESHIFT = 3;
    VIDEO_PLANESHIFT = 0;
    VIDEO_FACE_W = 48;//46;
    VIDEO_FACE_H = 45;
    VIDEO_FACE_X = 523;
    VIDEO_FACE_Y = 130;
    VIDEO_TILE_W = 56;//54;
    VIDEO_TILE_H = 42;//24;
    VIDEO_ITEM_W = 104;//100;
    VIDEO_ITEM_H = 50;
    VIDEO_KEY_W = 32;//16;
    VIDEO_KEY_H = 27;//14;
    VIDEO_HEALTH_W = 80;
    VIDEO_HEALTH_H = 80;
    // TRANSPARENCY = 0;
    VIDEO_MINIMAP_PIXEL_HEIGHT = 4;
    VIDEO_MINIMAP_PIXEL_BITSHIFT = 2;
    strncpy(VIDEO_FILEEXT, ".hgc", 5);
    for (i = 0; i < 348; i+=4){
        VIDEO_SCREENOFFSET[i] = j;
        VIDEO_SCREENOFFSET[i + 1] = j + 0x2000;
        VIDEO_SCREENOFFSET[i + 2] = j + 0x4000;
        VIDEO_SCREENOFFSET[i + 3] = j + 0x6000;
        j += 90;
    }
    INFOROW[0] = 300;
    INFOROW[1] = 316;
    INFOROW[2] = 332;
	VIDEO_GAMEOVERSIZE = 7488;
	VIDEO_HUDSIZE = 1312;
	VIDEO_FONT_COLOR_MENU = 1;
	VIDEO_FONT_COLOR_MENU_SELECTED = 3;
	VIDEO_FONT_COLOR_INFO = 1;
	VIDEO_FONT_COLOR_WHITE = 3;
	VIDEO_FONT_COLOR_GAMEOVER = 1;
	VIDEO_HEALTH_COLORS = (uint8_t*)HEALTH_COLORS_CGA;
	VIDEO_USE_PALETTE_ANIMATION = 0;
	MAP_TILE_COLORS = MAP_TILE_COLORS_CGA;
	MINIMAP_X_OFFSET = 7;
    MAP_TILE_COLORS[TILE_RAFT] = 0xFF;
	// Set function pointers
	videoSetMode = &HGC_videoSetMode;
	videoUnsetMode = &HGC_videoUnsetMode;
	videoClearScreen = &PCP_videoClearScreen;
	videoDrawFullScreen = &CGA_videoDrawFullScreen;
	videoPrintFontAsColor = &CGA_videoPrintFontAsColor;
	videoCinemaAddChar = &HGC_videoCinemaAddChar;
	videoCinemaRotate = &HGC_videoCinemaRotate;
	videoGetPixel = &CGA_videoGetPixel;
    videoPrintTile = &HGC_videoPrintTile;
    videoPrintTileMultibyte = &HGC_videoPrintTileMultibyte;
    videoPrintSprite = &HGC_videoPrintSprite;
    videoPrintSpriteAsColor = &HGC_videoPrintSpriteAsColor;
    videoFillRect = &HGC_videoFillRect;
	videoTranslate = NULL;
	videoMaskTranslate = &CGA_videoMaskTranslate;
	videoGetUnitColorForMinimap = &CGA_videoGetUnitColorForMinimap;
	videoScreenShakeStep = &HGC_videoScreenShakeStep;
	minimapSet = &CGA_minimapSet;
	minimapPrepareLine = &CGA_minimapPrepareLine;
	minimapCopyLineToScreen = &CGA_minimapCopyLineToScreen;
}

void HGC_videoSetMode(void){
	uint8_t hercules_crtc_tab[] = {
        // CRTC table for 720x348
		0x35, 0x2d, 0x2e, 0x07, 0x5b, 0x02, 0x57, 0x57, 0x02, 0x03, 0x00, 0x00
	};
	union REGS regs;

	// Save old video mode
	regs.h.ah = 0x0f;
	int86(0x10, &regs, &regs);
	VIDEO_MODE_ON_STARTUP = regs.h.al;

	// switch to Hercules mode
	_asm {
		push ax
		push dx

		// switch to mode 7
		mov ax, 7
		int 0x10

		// disable video (graphics mode with page 0)
		mov dx, 0x3b8
		mov al, 0x02
		out dx, al

		// set CRTC registers

		push si

		lea si, hercules_crtc_tab
		xor ax, ax
		cli				// disable interrupts
		cld

sh_crtc_loop:
		mov dl, 0xb4	// dx=0x3b4
		out dx, al
		inc ax
		push ax
		lodsb
		inc dx
		out dx, al
		pop ax
		cmp al, 12
		jb sh_crtc_loop

		sti				// enable interrupts
		pop si

		// allow graphics mode, upper page disabled
		mov dl, 0xbf	// dx=0x3bf
		mov al, 1		// 3 if upper page enabled
		out dx, al

		// clear screen
		push es			// save es
		mov ax, 0xb000
		mov es, ax		// es=0xb000
		push di			// save di
		xor di, di		// di=0
		xor ax, ax		// ax=0
		push cx			// save cx
		mov cx, 16384
		rep stosw		// clear the entire 32KiB page
		pop cx			// restore cx
		pop di			// restore di
		pop es			// restore es

		// enable video (graphics mode with page 0)
		mov dl, 0xb8	// dx = 0x3b8
		mov al, 0x0a
		out dx, al

		pop dx			// restore dx
		pop ax			// restore ax
	}
}

void HGC_videoUnsetMode(void){
	union REGS regs;

	// Restore old video mode
	regs.h.ah = 0x00;
	regs.h.al = VIDEO_MODE_ON_STARTUP;
	int86(0x10, &regs, &regs);
	// Re-enable port 3b8 protection
	outp(0x3bf, 0);
}

void HGC_videoScreenShakeStep(void){
    uint8_t i, c, y;

	// Decrease screen shake, once we hit 0, we will reset screen back to normal
	// Select register 12 to modify screen pointer
	outp(0x3b4, 12);
	SCREEN_SHAKE--;
	outp(0x3b5, 0);
	outp(0x3b4, 13);
	SCREEN_SHAKE--;
    c = SCREEN_SHAKE_VAL[SCREEN_SHAKE] % 5;
    y = 0;
    for (i = 0; i < c; i++){
        y += 45;
    }
	outp(0x3b5, y);
}

void HGC_videoPrintTile(uint16_t x, uint16_t y, uint16_t tileseg, uint16_t w, uint16_t h){
	_asm{
		// Set segment (DS:SI) to tileseg
		mov ds, ss:tileseg
		// Set si (offset) to 0
		xor si, si
		// Load VIDEO_SCREENOFFSET[y] to get offset of screen row and store in DI
		lea bx, ss:VIDEO_SCREENOFFSET
		mov di, y
		shl di, 1
		mov di, [ss:bx + di]
		// Set ES segment to screen pointer
		mov cx, ss:VIDEO_SCREENSEG
		mov es, cx
		// add x to screen position. SHR to divide by 8 for pixels per byte
		mov dx, x
		shr dx, 1
		shr dx, 1
		shr dx, 1
		add di, dx
        // Get x bitshift amount
        mov cx, x
        and cx, 7
        // Compute mask for bit-shifted pixels
        mov ch, 0xff
        shr ch, cl
        not ch
		// Save number of bytes to copy per row. SHR to divide by 8. Copy it into dh as well
		mov dx, w
		shr dx, 1
		shr dx, 1
		shr dx, 1
        mov dh, dl
		// Set CX to h, the number of times to repeat
		mov bx, h
		// Loop while drawing to the screen
    vpt_hgc_loop:
		// Set number of bytes to copy
        mov dl, dh
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [es:di]
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jz vpt_hgc_loop_next_bitplane
    vpt_hgc_loop2:
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [ds:si - 1]
        ror al, cl
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jnz vpt_hgc_loop2
    vpt_hgc_loop_next_bitplane:
        // Exit loop if we've reached the last row. (Loop until BX is 0)
		dec bx
		jz vpt_hgc_loop_exit
        mov dl, dh
        mov dh, 0
        sub di, dx
        mov dh, dl
        // Go to next bitplane
        add di, 0x2000
        cmp di, 0x8000
        jb vpt_hgc_loop
        sub di, 0x7fa6
        jmp vpt_hgc_loop
    vpt_hgc_loop_exit:
	}
}

void HGC_videoPrintTileMultibyte(uint16_t x, uint16_t y, uint16_t tileseg, uint16_t w, uint16_t h){
	_asm{
		// Set segment (DS:SI) to tileseg
		mov ds, ss:tileseg
		// Set si (offset) to 0
		xor si, si
		// Load VIDEO_SCREENOFFSET[y] to get offset of screen row and store in DI
		lea bx, ss:VIDEO_SCREENOFFSET
		mov di, y
		shl di, 1
		mov di, [ss:bx + di]
		// Set ES segment to screen pointer
		mov cx, ss:VIDEO_SCREENSEG
		mov es, cx
		// add x to screen position. SHR to divide by 8 for pixels per byte
		mov dx, x
		shr dx, 1
		shr dx, 1
		shr dx, 1
		add di, dx
        // Get x bitshift amount
        mov cx, x
        and cx, 7
        // Compute mask for bit-shifted pixels
        mov ch, 0xff
        shr ch, cl
        not ch
		// Save number of bytes to copy per row. SHR to divide by 8. Copy it into dh as well
		mov dx, w
		shr dx, 1
		shr dx, 1
		shr dx, 1
        mov dh, dl
		// Set CX to h, the number of times to repeat
		mov bx, h
		// Loop while drawing to the screen
    vpt_hgc_loop:
		// Set number of bytes to copy
        mov dl, dh
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [es:di]
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jz vpt_hgc_loop_next_bitplane
    vpt_hgc_loop2:
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [ds:si - 1]
        ror al, cl
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jnz vpt_hgc_loop2
    vpt_hgc_loop_next_bitplane:
        mov dl, dh
        mov dh, 0
        sub di, dx
        sub si, dx
        mov dh, dl
        // Go to next bitplane
        add di, 0x2000
        cmp di, 0x8000
        jb vpt_hgc_loop_line2
        sub di, 0x7fa6
    vpt_hgc_loop_line2:
		// Set number of bytes to copy
        mov dl, dh
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [es:di]
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jz vpt_hgc_loop_next_bitplane_line2
    vpt_hgc_loop2_line2:
        // Read byte from the screen for the first byte we write to the screen to preserve screen data
        mov al, BYTE PTR [ds:si - 1]
        ror al, cl
        // Read byte from tile data and shift it to the right the number of needed pixels
        mov ah, BYTE PTR [ds:si]
        shr ah, cl
        // Combine the bytes and write them back to the screen
        and al, ch
        or ah, al
        mov es:[di], ah
        // Move to next byte and exit if last byte
        inc si
        inc di
        dec dl
        jnz vpt_hgc_loop2_line2
    vpt_hgc_loop_next_bitplane_line2:
        // Exit loop if we've reached the last row. (Loop until BX is 0)
		dec bx
		jz vpt_hgc_loop_exit
        mov dl, dh
        mov dh, 0
        sub di, dx
        mov dh, dl
        // Go to next bitplane
        add di, 0x2000
        cmp di, 0x8000
        jb vpt_hgc_loop_nosub
        sub di, 0x7fa6
    vpt_hgc_loop_nosub:
        jmp vpt_hgc_loop
    vpt_hgc_loop_exit:
	}
}

// Fill a rectangular area of the screen with specified color
void HGC_videoFillRect(uint16_t x, uint16_t y, uint16_t w, uint16_t h, uint16_t color){
	
	_asm{
		// Load VIDEO_SCREENOFFSET[y] to get offset of screen row and store in DI
		lea bx, ss:VIDEO_SCREENOFFSET
		mov di, y
		shl di, 1
		mov di, [ss:bx + di]
		// Move x into DS
		mov ax, x
		mov ds, ax
		// Set SI to h, the number of times to repeat
		mov si, h
		// Set DX to color
		mov dx, color
		// Push BP onto the stack
		push bp
		// Move w into BP
		mov ax, w
		mov bp, ax
		// Set destination segment (ES) to start of screen
		mov cx, ss:VIDEO_SCREENSEG
		mov es, cx
		// Make sure the color (DX) falls in the proper range for CGA (make a copy in CX to compare with original color)
		mov cx, dx
		and dx, 3
		jne vfr_cga_nocolorchange
		// If the color after being ANDed is 0 and original color was not 0 then set it to 3 (white)
		cmp cx, 0
		je vfr_cga_nocolorchange
		mov dl, 3
	vfr_cga_nocolorchange:
		// Repeat the color (DL) for all 4 bit-pairs
		mov cl, dl
		shl cl, 1
		shl cl, 1
		or dl, cl
		shl cl, 1
		shl cl, 1
		or dl, cl
		shl cl, 1
		shl cl, 1
		or dl, cl
		// Store FFFF in BX, for starting(BH)/ending(BL) mask bytes
		mov bx, 0xFFFF
		// Get (x + w - 1) formula for getting ending byte/offset and store in AX
		mov ax, ds
		add ax, bp
		dec ax
		// Calculate ending pixel offset ((x + w - 1) & 3) (xor to inverse the bits to shift) and store a mask in BL
		mov cx, ax
		and cx, 3
		xor cx, 3
		je vfr_cga_endingoffsetdone
	vfr_cga_endingoffsetloop:
		shl bl, 1
		shl bl, 1
		dec cl
		jne vfr_cga_endingoffsetloop
	vfr_cga_endingoffsetdone:
		// Get ending byte ((x + w - 1) >> 2) (AL)
		shr ax, 1
		shr ax, 1
		// Calculate starting pixel offset (x & 3) and store a mask in BH
		mov cx, ds
		and cl, 3
		je vfr_cga_startingoffsetdone
	vfr_cga_startingoffsetloop:
		shr bh, 1
		shr bh, 1
		dec cl
		jne vfr_cga_startingoffsetloop
	vfr_cga_startingoffsetdone:
		// Get starting byte (x >> 2) and store in AH
		mov cx, ds
		shr cx, 1
		shr cx, 1
		mov ah, cl
		// Increase DI to X position
		add di, cx
		// Calculate the number of bytes needed to move to the next line, store in BP
		mov dh, al
		sub dh, ah
		mov cx, 89
		sub cl, dh
		mov bp, cx
		// If starting byte == FF and width is 0, then set width to 1
		cmp bh, 0xFF
		jne vfr_cga_startingnotff
		cmp dh, 0
		jne vfr_cga_startingnotff
		inc dh
	vfr_cga_startingnotff:
		// If ending byte == FF, then increase width by 1
		cmp bl, 0xFF
		jne vfr_cga_endingnotff
		inc dh
	vfr_cga_endingnotff:
		// Check if starting byte and ending byte are the same, if so then AND the masks together in the starting mask and fill the ending mask
		cmp al, ah
		jne vfr_cga_bytecalc
		and bh, bl
		mov bl, 0xFF
		cmp dh, 0
		je vfr_cga_bytecalc
		dec dh
	vfr_cga_bytecalc:
		// Calculate number of bytes that is just the filled color (no mask needed) and store in DH
		cmp dh, 0
		je vfr_cga_mainloopstart
		cmp bh, 0xFF
		je vfr_cga_mainloopstart
		dec dh
		// Clear cx, so high byte will be clear within loop, since we only load in the low byte
		mov cx, 0
    vfr_cga_mainloopstart:
        cmp DI, 0x2000
        jl vfr_cga_mainloop
        cmp DI, 0x4000
        jl vfr_cga_mainloop_plane2
        cmp DI, 0x5FFF
        jg vfr_cga_mainloop_plane4_jmp
        jmp vfr_cga_mainloop_plane3
    vfr_cga_mainloop_plane4_jmp:
        jmp vfr_cga_mainloop_plane4
        vfr_cga_mainloop:
		// If starting offset != FF, then we build the first byte using the mask
		cmp bh, 0xFF
		je vfr_cga_skipstartingbyte
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bh
		xor ah, 0xFF
		and al, ah
		mov ah, bh
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipstartingbyte:
		// Skip if we byte fill count is 0
		cmp dh, 0
		je vfr_cga_endingbyte
		// Set number of times to repeat
		mov cl, dh
		// Load fill color
		mov al, dl
		// Copy color to the screen
		rep stosb
	vfr_cga_endingbyte:
		// If ending offset != FF, then we build the last byte using the mask
		cmp bl, 0xFF
		je vfr_cga_skipendingbyte
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bl
		xor ah, 0xFF
		and al, ah
		mov ah, bl
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipendingbyte:
		// Move to next screen row
		add DI, BP
        add DI, 0x2000
		// Move back a row, because we are on plane 2 now
		sub DI, 90
		dec si
		jne vfr_cga_mainloop_plane2
        jmp vfr_cga_endloop

        vfr_cga_mainloop_plane2:
		// If starting offset != FF, then we build the first byte using the mask
		cmp bh, 0xFF
		je vfr_cga_skipstartingbyte_plane2
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bh
		xor ah, 0xFF
		and al, ah
		mov ah, bh
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipstartingbyte_plane2:
		// Skip if we byte fill count is 0
		cmp dh, 0
		je vfr_cga_endingbyte_plane2
		// Set number of times to repeat
		mov cl, dh
		// Load fill color
		mov al, dl
		// Copy color to the screen
		rep stosb
	vfr_cga_endingbyte_plane2:
		// If ending offset != FF, then we build the last byte using the mask
		cmp bl, 0xFF
		je vfr_cga_skipendingbyte_plane2
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bl
		xor ah, 0xFF
		and al, ah
		mov ah, bl
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipendingbyte_plane2:
		// Move to next screen row
		add DI, BP
        add DI, 0x2000
		// Move back a row, because we are on plane 2 now
		sub DI, 90
		dec si
		jne vfr_cga_mainloop_plane3
        jmp vfr_cga_endloop

        vfr_cga_mainloop_plane3:
		// If starting offset != FF, then we build the first byte using the mask
		cmp bh, 0xFF
		je vfr_cga_skipstartingbyte_plane3
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bh
		xor ah, 0xFF
		and al, ah
		mov ah, bh
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipstartingbyte_plane3:
		// Skip if we byte fill count is 0
		cmp dh, 0
		je vfr_cga_endingbyte_plane3
		// Set number of times to repeat
		mov cl, dh
		// Load fill color
		mov al, dl
		// Copy color to the screen
		rep stosb
	vfr_cga_endingbyte_plane3:
		// If ending offset != FF, then we build the last byte using the mask
		cmp bl, 0xFF
		je vfr_cga_skipendingbyte_plane3
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bl
		xor ah, 0xFF
		and al, ah
		mov ah, bl
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipendingbyte_plane3:
		// Move to next screen row
		add DI, BP
        add DI, 0x2000
		// Move back a row, because we are on plane 2 now
		sub DI, 90
		dec si
		je vfr_cga_endloop

        vfr_cga_mainloop_plane4:
		// If starting offset != FF, then we build the first byte using the mask
		cmp bh, 0xFF
		je vfr_cga_skipstartingbyte_plane4
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bh
		xor ah, 0xFF
		and al, ah
		mov ah, bh
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipstartingbyte_plane4:
		// Skip if we byte fill count is 0
		cmp dh, 0
		je vfr_cga_endingbyte_plane4
		// Set number of times to repeat
		mov cl, dh
		// Load fill color
		mov al, dl
		// Copy color to the screen
		rep stosb
	vfr_cga_endingbyte_plane4:
		// If ending offset != FF, then we build the last byte using the mask
		cmp bl, 0xFF
		je vfr_cga_skipendingbyte_plane4
		// Read byte from screen and use mask to join new color data together and write the new byte
		mov al, byte ptr [ES:DI]
		mov ah, bl
		xor ah, 0xFF
		and al, ah
		mov ah, bl
		and ah, dl
		or al, ah
		stosb
	vfr_cga_skipendingbyte_plane4:
		// Move to next screen row
		add DI, BP
        sub DI, 0x6000
		// Move back a row, because we are on plane 2 now
		//sub DI, 90
		dec si
		je vfr_cga_endloop
        jmp vfr_cga_mainloop


    vfr_cga_endloop:
		pop BP
	}
}

// Copy a sprite from ram to the screen, using a mask to avoid overwriting surrounding pixels
// 'x' is the x position on screen to draw the sprite (upper-left corner)
// 'y' is the y position on screen to draw the sprite (upper-left corner)
// 'sprite' should be a pointer to the top-left of the tile in ram
// 'color' should be the color to draw the sprite as
// WIDTH and HEIGHT are hard set to 24x24 for this routine
void HGC_videoPrintSpriteAsColor(uint16_t x, uint16_t y, uint16_t spriteseg, uint16_t color){
	_asm{
		// Load VIDEO_SCREENOFFSET[y] to get offset of screen row as store in DI
		lea bx, ss:VIDEO_SCREENOFFSET
		mov di, y
		shl di, 1
		mov di, [ss:bx + di]
		// Load pointer to sprite [DS:SI]
		mov ds, spriteseg
		xor si, si
		// Set ES segment to CGA screen pointer
		mov cx, ss:VIDEO_SCREENSEG
		mov es, cx
		// add x to screen position. SHR to divide by 8 for pixels per byte
		mov dx, x
		shr dx, 1
		shr dx, 1
		shr dx, 1
        add di, dx
        // get color data and repeat it
        mov bx, ss:color
        and bx, 3
        mov bh, bl
        shl bh, 1
        shl bh, 1
        or bl, bh
        shl bh, 1
        shl bh, 1
        or bl, bh
        shl bh, 1
        shl bh, 1
        or bl, bh
        // Number of times to loop (each loop draws 4 lines)
		mov cx, 40
        // Set plane number we are on 0-3
        mov ah, 4
        mov dx, y
        and dx, 2 
        jz vps_cga_loop
        dec ah 
        dec ah 
	vps_cga_loop:
		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

		// Fetch sprite data into AL
		mov al, byte ptr [DS:SI]
		// Inverse sprite data
        mov dl, al
        xor dl, 0xFF
        // And with color data
        and al, bl
        // Fetch screen data into BH, and with inversed sprite data
		and dl, byte ptr [ES:DI]
        // Combine sprite and screen data
        or al, dl
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Move to next plane
        dec ah
        jz vps_planeloop
        add di, 8185
        dec cx 
        jz vps_done
        jmp vps_cga_loop
    vps_planeloop:
        mov ah, 4
        add di, 41043
        dec cx 
		jz vps_done
		jmp vps_cga_loop
	vps_done:
	}
}

// Copy a sprite from ram to the screen, using a mask to also draw the tile underneath
// 'x' is the x position on screen to draw the sprite (upper-left corner)
// 'y' is the y position on screen to draw the sprite (upper-left corner)
// 'sprite' should be a pointer to the top-left of the tile in ram
// 'tile' should be a pointer to the top-left of the tile in ram
void HGC_videoPrintSprite(uint16_t x, uint16_t y, uint16_t spriteseg, uint16_t tileseg){
	_asm{
		// Load VIDEO_SCREENOFFSET[y] to get offset of screen row as store in DI
		lea bx, ss:VIDEO_SCREENOFFSET
		mov di, y
		shl di, 1
		mov di, [ss:bx + di]
		// Load pointer to sprite [DS:SI]
		mov ds, ss:spriteseg
		xor si, si
		// Set ES segment to CGA screen pointer
		mov cx, ss:VIDEO_SCREENSEG
		mov es, cx
		// add x to screen position. SHR to divide by 8 for pixels per byte
		mov dx, ss:x
		shr dx, 1
		shr dx, 1
		shr dx, 1
        add di, dx
        // Set plane number we are on 0-3
        mov ah, 2
        mov bx, ss:y
        and bx, 2
        jnz vps_init
        mov ah, 4
    vps_init:
		// Save SS segment
		mov dx, ss
		// Disable interrupts
		cli
		// Load pointer to tileseg. Store in SS:SI
		mov bx, ss:tileseg
		mov ss, bx
        // Number of times to loop (each loop draws 1 line)
		mov cx, 42
	vps_loop:
        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        // Fetch mask data into AH
        mov al, byte ptr [DS:SI + 294]
        // Fetch tile data, anding with the mask
        and al, byte ptr [SS:SI]
		// Fetch sprite data into AL
		or al, byte ptr [DS:SI]
		// Output to screen
		stosb
		// Increment to next byte
		inc si

        dec cx 
        jz vps_done
        // Move to next plane
        dec ah
        jz vps_planeloop
        add di, 8185
        jmp vps_loop
    vps_planeloop:
        mov ah, 4
        add di, 41043
		jmp vps_loop
	vps_done:
		// Replace SS segment back to what it was
		mov ss, dx
		// Enable interrupts
		sti
	}
}

// Add a character to the right cinema tile
// letter_segment = a segment pointer to an 8x8 character to add to a tile/memory location
// dest_segment = a segment pointer to the top-left of a destination tile
void HGC_videoCinemaAddChar(uint16_t letter_segment, uint16_t dest_segment){
	_asm{
		// Set source (DS:SI) to letter
		mov ds, ss:letter_segment
		// Set offset to 0
		xor si, si
		// Set destination (ES:DI) to dest
		mov es, ss:dest_segment
		// Add 157 to destination position (56 * 11 + 24) / 8
		mov di, 157
		// Set AX to 8, the number of times to repeat
		mov cx, 8
		// Loop while drawing
	vcac_hgc_loop:
		// Copy 1 byte
		movsw
		// Increase to next destination row
		add di, 5
		// Loop until CX is 0
		loop vcac_hgc_loop
	}
}

// Rotate the cinema tiles
void HGC_videoCinemaRotate(uint16_t tile1, uint16_t tile2, uint16_t tile3){
	_asm{
		// Load in the first tile
		mov ds, ss:tile1
		// Add 154 to destination position (56 * 11) / 8
		mov si, 155
		// Copy source into destination as well
		mov di, si
		mov bx, ds
		mov es, bx
		// Increase source by 4 pixels
		inc si
		// Rotate tile1. Set number of words to copy
        mov cx, 24
		// Start copy
		rep movsw
		// Copy left edge of tile2 to tile1
		mov ds, ss:tile2
		mov si, 154
        mov di, 160
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		// Rotate tile2
        mov si, 155
		mov di, si
		mov bx, ds
		mov es, bx
		dec di
        mov cx, 24
		rep movsw
		// Copy left edge of tile3 to tile2
		mov ds, ss:tile3
		mov si, 154
        mov di, 160
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		add si, 6
		add di, 6
		movsb
		// Rotate tile3
        mov si, 154
		mov di, si
		mov bx, ds
		mov es, bx
		dec di
        mov cx, 24
		rep movsw
	}
}

