#include <math.h>
#include <stddef.h>
#include "ibm.h"
#include "device.h"
#include "mem.h"
#include "thread.h"
#include "video.h"
#include "vid_svga.h"
#include "vid_voodoo.h"
#include "vid_voodoo_common.h"
#include "vid_voodoo_banshee_blitter.h"
#include "vid_voodoo_fb.h"
#include "vid_voodoo_fifo.h"
#include "vid_voodoo_reg.h"
#include "vid_voodoo_regs.h"
#include "vid_voodoo_render.h"
#include "vid_voodoo_texture.h"

#define WAKE_DELAY (TIMER_USEC * 100)
void voodoo_wake_fifo_thread(voodoo_t *voodoo) {
	if (!timer_is_enabled(&voodoo->wake_timer)) {
		/*Don't wake FIFO thread immediately - if we do that it will probably
		  process one word and go back to sleep, requiring it to be woken on
		  almost every write. Instead, wait a short while so that the CPU
		  emulation writes more data so we have more batched-up work.*/
		timer_set_delay_u64(&voodoo->wake_timer, WAKE_DELAY);
	}
}

void voodoo_wake_fifo_thread_now(voodoo_t *voodoo) {
	thread_set_event(voodoo->wake_fifo_thread); /*Wake up FIFO thread if moving from idle*/
}

void voodoo_wake_timer(void *p) {
	voodoo_t *voodoo = (voodoo_t *)p;

	thread_set_event(voodoo->wake_fifo_thread); /*Wake up FIFO thread if moving from idle*/
}

void voodoo_queue_command(voodoo_t *voodoo, uint32_t addr_type, uint32_t val) {
	fifo_entry_t *fifo = &voodoo->fifo[voodoo->fifo_write_idx & FIFO_MASK];

	while (FIFO_FULL) {
		thread_reset_event(voodoo->fifo_not_full_event);
		if (FIFO_FULL) {
			thread_wait_event(voodoo->fifo_not_full_event, 1); /*Wait for room in ringbuffer*/
			if (FIFO_FULL)
				voodoo_wake_fifo_thread_now(voodoo);
		}
	}

	fifo->val = val;
	fifo->addr_type = addr_type;

	voodoo->fifo_write_idx++;

	if (FIFO_ENTRIES > 0xe000)
		voodoo_wake_fifo_thread(voodoo);
}

void voodoo_flush(voodoo_t *voodoo) {
	voodoo->flush = 1;
	while (!FIFO_EMPTY) {
		voodoo_wake_fifo_thread_now(voodoo);
		thread_wait_event(voodoo->fifo_not_full_event, 1);
	}
	voodoo_wait_for_render_thread_idle(voodoo);
	voodoo->flush = 0;
}

void voodoo_wake_fifo_threads(voodoo_set_t *set, voodoo_t *voodoo) {
	voodoo_wake_fifo_thread(voodoo);
	if (SLI_ENABLED && voodoo->type != VOODOO_2 && set->voodoos[0] == voodoo)
		voodoo_wake_fifo_thread(set->voodoos[1]);
}

void voodoo_wait_for_swap_complete(voodoo_t *voodoo) {
	while (voodoo->swap_pending) {
		thread_wait_event(voodoo->wake_fifo_thread, -1);
		thread_reset_event(voodoo->wake_fifo_thread);

		thread_lock_mutex(voodoo->swap_mutex);
		if ((voodoo->swap_pending && voodoo->flush) || FIFO_FULL) {
			/*Main thread is waiting for FIFO to empty, so skip vsync wait and just swap*/
			memset(voodoo->dirty_line, 1, sizeof(voodoo->dirty_line));
			voodoo->front_offset = voodoo->params.front_offset;
			if (voodoo->swap_count > 0)
				voodoo->swap_count--;
			voodoo->swap_pending = 0;
			thread_unlock_mutex(voodoo->swap_mutex);
			break;
		} else
			thread_unlock_mutex(voodoo->swap_mutex);
	}
}

static uint32_t cmdfifo_get(voodoo_t *voodoo) {
	uint32_t val;

	if (!voodoo->cmdfifo_in_sub) {
		while (voodoo->cmdfifo_depth_rd == voodoo->cmdfifo_depth_wr) {
			thread_wait_event(voodoo->wake_fifo_thread, -1);
			thread_reset_event(voodoo->wake_fifo_thread);
		}
	}

	val = *(uint32_t *)&voodoo->fb_mem[voodoo->cmdfifo_rp & voodoo->fb_mask];

	if (!voodoo->cmdfifo_in_sub)
		voodoo->cmdfifo_depth_rd++;
	voodoo->cmdfifo_rp += 4;

//        pclog("  CMDFIFO get %08x\n", val);
	return val;
}

static inline float cmdfifo_get_f(voodoo_t *voodoo) {
	union {
		uint32_t i;
		float f;
	} tempif;

	tempif.i = cmdfifo_get(voodoo);
	return tempif.f;
}

enum {
	CMDFIFO3_PC_MASK_RGB = (1 << 10),
	CMDFIFO3_PC_MASK_ALPHA = (1 << 11),
	CMDFIFO3_PC_MASK_Z = (1 << 12),
	CMDFIFO3_PC_MASK_Wb = (1 << 13),
	CMDFIFO3_PC_MASK_W0 = (1 << 14),
	CMDFIFO3_PC_MASK_S0_T0 = (1 << 15),
	CMDFIFO3_PC_MASK_W1 = (1 << 16),
	CMDFIFO3_PC_MASK_S1_T1 = (1 << 17),

	CMDFIFO3_PC = (1 << 28)
};

void voodoo_fifo_thread(void *param) {
	voodoo_t *voodoo = (voodoo_t *)param;

	while (1) {
		thread_set_event(voodoo->fifo_not_full_event);
		thread_wait_event(voodoo->wake_fifo_thread, -1);
		thread_reset_event(voodoo->wake_fifo_thread);
		voodoo->voodoo_busy = 1;
		while (!FIFO_EMPTY) {
			uint64_t start_time = timer_read();
			uint64_t end_time;
			fifo_entry_t *fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];

			switch (fifo->addr_type & FIFO_TYPE) {
			case FIFO_WRITEL_REG:
				while ((fifo->addr_type & FIFO_TYPE) == FIFO_WRITEL_REG) {
					voodoo_reg_writel(fifo->addr_type & FIFO_ADDR, fifo->val, voodoo);
					fifo->addr_type = FIFO_INVALID;
					voodoo->fifo_read_idx++;
					if (FIFO_EMPTY)
						break;
					fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];
				}
				break;
			case FIFO_WRITEW_FB:voodoo_wait_for_render_thread_idle(voodoo);
				while ((fifo->addr_type & FIFO_TYPE) == FIFO_WRITEW_FB) {
					voodoo_fb_writew(fifo->addr_type & FIFO_ADDR, fifo->val, voodoo);
					fifo->addr_type = FIFO_INVALID;
					voodoo->fifo_read_idx++;
					if (FIFO_EMPTY)
						break;
					fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];
				}
				break;
			case FIFO_WRITEL_FB:voodoo_wait_for_render_thread_idle(voodoo);
				while ((fifo->addr_type & FIFO_TYPE) == FIFO_WRITEL_FB) {
					voodoo_fb_writel(fifo->addr_type & FIFO_ADDR, fifo->val, voodoo);
					fifo->addr_type = FIFO_INVALID;
					voodoo->fifo_read_idx++;
					if (FIFO_EMPTY)
						break;
					fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];
				}
				break;
			case FIFO_WRITEL_TEX:
				while ((fifo->addr_type & FIFO_TYPE) == FIFO_WRITEL_TEX) {
					if (!(fifo->addr_type & 0x400000))
						voodoo_tex_writel(fifo->addr_type & FIFO_ADDR, fifo->val, voodoo);
					fifo->addr_type = FIFO_INVALID;
					voodoo->fifo_read_idx++;
					if (FIFO_EMPTY)
						break;
					fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];
				}
				break;
			case FIFO_WRITEL_2DREG:
				while ((fifo->addr_type & FIFO_TYPE) == FIFO_WRITEL_2DREG) {
					voodoo_2d_reg_writel(voodoo, fifo->addr_type & FIFO_ADDR, fifo->val);
					fifo->addr_type = FIFO_INVALID;
					voodoo->fifo_read_idx++;
					if (FIFO_EMPTY)
						break;
					fifo = &voodoo->fifo[voodoo->fifo_read_idx & FIFO_MASK];
				}
				break;

			default:fatal("Unknown fifo entry %08x\n", fifo->addr_type);
			}

			if (FIFO_ENTRIES > 0xe000)
				thread_set_event(voodoo->fifo_not_full_event);

			end_time = timer_read();
			voodoo->time += end_time - start_time;
		}

		while (voodoo->cmdfifo_enabled && (voodoo->cmdfifo_depth_rd != voodoo->cmdfifo_depth_wr || voodoo->cmdfifo_in_sub)) {
			uint64_t start_time = timer_read();
			uint64_t end_time;
			uint32_t header = cmdfifo_get(voodoo);
			uint32_t addr;
			uint32_t mask;
			int smode;
			int num;
			int num_verticies;
			int v_num;

//                        pclog(" CMDFIFO header %08x at %08x\n", header, voodoo->cmdfifo_rp);

			switch (header & 7) {
			case 0:
//                                pclog("CMDFIFO0\n");
				switch ((header >> 3) & 7) {
				case 0: /*NOP*/
					break;

				case 1: /*JSR*/
//                                        pclog("JSR %08x\n", (header >> 4) & 0xfffffc);
					voodoo->cmdfifo_ret_addr = voodoo->cmdfifo_rp;
					voodoo->cmdfifo_rp = (header >> 4) & 0xfffffc;
					voodoo->cmdfifo_in_sub = 1;
					break;

				case 2: /*RET*/
					voodoo->cmdfifo_rp = voodoo->cmdfifo_ret_addr;
					voodoo->cmdfifo_in_sub = 0;
					break;

				case 3: /*JMP local frame buffer*/
					voodoo->cmdfifo_rp = (header >> 4) & 0xfffffc;
//                                        pclog("JMP to %08x %04x\n", voodoo->cmdfifo_rp, header);
					break;

				default:fatal("Bad CMDFIFO0 %08x\n", header);
				}
				break;

			case 1:num = header >> 16;
				addr = (header & 0x7ff8) >> 1;
//                                pclog("CMDFIFO1 addr=%08x\n",addr);
				while (num--) {
					uint32_t val = cmdfifo_get(voodoo);
					if ((addr & (1 << 13)) && voodoo->type >= VOODOO_BANSHEE) {
//                                                if (voodoo->type != VOODOO_BANSHEE)
//                                                        fatal("CMDFIFO1: Not Banshee\n");
//                                                pclog("CMDFIFO1: write %08x %08x\n", addr, val);
						voodoo_2d_reg_writel(voodoo, addr, val);
					} else {
						if ((addr & 0x3ff) == SST_triangleCMD || (addr & 0x3ff) == SST_ftriangleCMD ||
							(addr & 0x3ff) == SST_fastfillCMD || (addr & 0x3ff) == SST_nopCMD)
							voodoo->cmd_written_fifo++;

						if (voodoo->type >= VOODOO_BANSHEE && (addr & 0x3ff) == SST_swapbufferCMD)
							voodoo->cmd_written_fifo++;
						voodoo_reg_writel(addr, val, voodoo);
					}

					if (header & (1 << 15))
						addr += 4;
				}
				break;

			case 2:
				if (voodoo->type < VOODOO_BANSHEE)
					fatal("CMDFIFO2: Not Banshee\n");
				mask = (header >> 3);
				addr = 8;
				while (mask) {
					if (mask & 1) {
						uint32_t val = cmdfifo_get(voodoo);

						voodoo_2d_reg_writel(voodoo, addr, val);
					}

					addr += 4;
					mask >>= 1;
				}
				break;

			case 3:num = (header >> 29) & 7;
				mask = header;//(header >> 10) & 0xff;
				smode = (header >> 22) & 0xf;
				voodoo_reg_writel(SST_sSetupMode, ((header >> 10) & 0xff) | (smode << 16), voodoo);
				num_verticies = (header >> 6) & 0xf;
				v_num = 0;
				if (((header >> 3) & 7) == 2)
					v_num = 1;
//                                pclog("CMDFIFO3: num=%i verts=%i mask=%02x\n", num, num_verticies, (header >> 10) & 0xff);
//                                pclog("CMDFIFO3 %02x %i\n", (header >> 10), (header >> 3) & 7);

				while (num_verticies--) {
					voodoo->verts[3].sVx = cmdfifo_get_f(voodoo);
					voodoo->verts[3].sVy = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_RGB) {
						if (header & CMDFIFO3_PC) {
							uint32_t val = cmdfifo_get(voodoo);
							voodoo->verts[3].sBlue = (float)(val & 0xff);
							voodoo->verts[3].sGreen = (float)((val >> 8) & 0xff);
							voodoo->verts[3].sRed = (float)((val >> 16) & 0xff);
							voodoo->verts[3].sAlpha = (float)((val >> 24) & 0xff);
						} else {
							voodoo->verts[3].sRed = cmdfifo_get_f(voodoo);
							voodoo->verts[3].sGreen = cmdfifo_get_f(voodoo);
							voodoo->verts[3].sBlue = cmdfifo_get_f(voodoo);
						}
					}
					if ((mask & CMDFIFO3_PC_MASK_ALPHA) && !(header & CMDFIFO3_PC))
						voodoo->verts[3].sAlpha = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_Z)
						voodoo->verts[3].sVz = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_Wb)
						voodoo->verts[3].sWb = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_W0)
						voodoo->verts[3].sW0 = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_S0_T0) {
						voodoo->verts[3].sS0 = cmdfifo_get_f(voodoo);
						voodoo->verts[3].sT0 = cmdfifo_get_f(voodoo);
					}
					if (mask & CMDFIFO3_PC_MASK_W1)
						voodoo->verts[3].sW1 = cmdfifo_get_f(voodoo);
					if (mask & CMDFIFO3_PC_MASK_S1_T1) {
						voodoo->verts[3].sS1 = cmdfifo_get_f(voodoo);
						voodoo->verts[3].sT1 = cmdfifo_get_f(voodoo);
					}
					if (v_num)
						voodoo_reg_writel(SST_sDrawTriCMD, 0, voodoo);
					else
						voodoo_reg_writel(SST_sBeginTriCMD, 0, voodoo);
					v_num++;
					if (v_num == 3 && ((header >> 3) & 7) == 0)
						v_num = 0;
				}
				break;

			case 4:num = (header >> 29) & 7;
				mask = (header >> 15) & 0x3fff;
				addr = (header & 0x7ff8) >> 1;
//                                pclog("CMDFIFO4 addr=%08x\n",addr);
				while (mask) {
					if (mask & 1) {
						uint32_t val = cmdfifo_get(voodoo);

						if ((addr & (1 << 13)) && voodoo->type >= VOODOO_BANSHEE) {
							if (voodoo->type < VOODOO_BANSHEE)
								fatal("CMDFIFO1: Not Banshee\n");
//                                                pclog("CMDFIFO1: write %08x %08x\n", addr, val);
							voodoo_2d_reg_writel(voodoo, addr, val);
						} else {
							if ((addr & 0x3ff) == SST_triangleCMD || (addr & 0x3ff) == SST_ftriangleCMD ||
								(addr & 0x3ff) == SST_fastfillCMD || (addr & 0x3ff) == SST_nopCMD)
								voodoo->cmd_written_fifo++;

							if (voodoo->type >= VOODOO_BANSHEE && (addr & 0x3ff) == SST_swapbufferCMD)
								voodoo->cmd_written_fifo++;
							voodoo_reg_writel(addr, val, voodoo);
						}
					}

					addr += 4;
					mask >>= 1;
				}
				while (num--)
					cmdfifo_get(voodoo);
				break;

			case 5:
//                                if (header & 0x3fc00000)
//                                        fatal("CMDFIFO packet 5 has byte disables set %08x\n", header);
				num = (header >> 3) & 0x7ffff;
				addr = cmdfifo_get(voodoo) & 0xffffff;
				if (!num)
					num = 1;
//                                pclog("CMDFIFO5 addr=%08x num=%i\n", addr, num);
				switch (header >> 30) {
				case 0: /*Linear framebuffer (Banshee)*/
					if (voodoo->texture_present[0][(addr & voodoo->texture_mask) >> TEX_DIRTY_SHIFT]) {
//                                                pclog("texture_present at %08x %i\n", addr, (addr & voodoo->texture_mask) >> TEX_DIRTY_SHIFT);
						flush_texture_cache(voodoo, addr & voodoo->texture_mask, 0);
					}
					if (voodoo->texture_present[1][(addr & voodoo->texture_mask) >> TEX_DIRTY_SHIFT]) {
//                                                pclog("texture_present at %08x %i\n", addr, (addr & voodoo->texture_mask) >> TEX_DIRTY_SHIFT);
						flush_texture_cache(voodoo, addr & voodoo->texture_mask, 1);
					}
					while (num--) {
						uint32_t val = cmdfifo_get(voodoo);
						if (addr <= voodoo->fb_mask)
							*(uint32_t *)&voodoo->fb_mem[addr] = val;
						addr += 4;
					}
					break;
				case 2: /*Framebuffer*/
					while (num--) {
						uint32_t val = cmdfifo_get(voodoo);
						voodoo_fb_writel(addr, val, voodoo);
						addr += 4;
					}
					break;
				case 3: /*Texture*/
					while (num--) {
						uint32_t val = cmdfifo_get(voodoo);
						voodoo_tex_writel(addr, val, voodoo);
						addr += 4;
					}
					break;

				default:fatal("CMDFIFO packet 5 bad space %08x %08x\n", header, voodoo->cmdfifo_rp);
				}
				break;

			default:fatal("Bad CMDFIFO packet %08x %08x\n", header, voodoo->cmdfifo_rp);
			}

			end_time = timer_read();
			voodoo->time += end_time - start_time;
		}
		voodoo->voodoo_busy = 0;
	}
}
