#include <trap.h>
#include <smp.h>
#include <umem.h>
#include <arch/softfloat.h>

static uint32_t ls(uint64_t* addr)
{
	uint32_t r;
	asm ("fld f0, %1; mftx.s %0, f0" : "=r"(r) : "m"(*addr));
	return r;
}

static void ss(uint64_t* addr, uint32_t val)
{
	asm ("mxtf.s f0, %0; fsd f0, %1" : : "r"(val), "m"(*addr));
}

static int emulate_fpu_silly(struct hw_trapframe *state,
                             ancillary_state_t *silly)
{
	int insn;
	if (memcpy_from_user(current, &insn, (void*)state->epc, 4))
	{
		state->cause = CAUSE_FAULT_FETCH;
		handle_trap(state);
	}

	#define DECLARE_INSN(name, match, mask) bool is_##name = (insn & mask) == match;
	#include <arch/opcodes.h>
	#undef DECLARE_INSN

	int rd  = (insn >> 27) & 0x1f;
	int rs1 = (insn >> 22) & 0x1f;
	int rs2 = (insn >> 17) & 0x1f;
	int rs3 = (insn >> 12) & 0x1f;

	int imm = (insn << 10) >> 20;
	int bimm = ((insn >> 10) & 0x7f) | ((insn & 0xf8000000) >> 20);

	void* load_address = (void*)(state->gpr[rs1] + imm);
	void* store_address = (void*)(state->gpr[rs1] + bimm);

	softfloat_t sf;
	sf.float_rounding_mode = silly->fsr >> 5;
	sf.float_exception_flags = silly->fsr & 0x1f;

	if (is_fsqrt_s)
		ss(&silly->fpr[rd], float32_sqrt(&sf, ls(&silly->fpr[rs1])));
	else if (is_fsqrt_d)
		silly->fpr[rd] = float64_sqrt(&sf, silly->fpr[rs1]);
	else if (is_fdiv_s)
		ss(&silly->fpr[rd], float32_div(&sf, ls(&silly->fpr[rs1]), ls(&silly->fpr[rs2])));
	else if (is_fdiv_d)
		silly->fpr[rd] = float64_div(&sf, silly->fpr[rs1], silly->fpr[rs2]);
	/* Eventually, we will emulate the full FPU, including the below insns
	else if (is_mffsr)
	{
		// use sf instead of silly->fsr
		state->gpr[rd] = silly->fsr;
	}
	else if (is_mtfsr)
	{
		// use sf instead of silly->fsr
		int temp = silly->fsr;
		silly->fsr = state->gpr[rs1] & 0xFF;
		state->gpr[rd] = silly->fsr;
	}
	else if (is_fld)
	{
		uint64_t dest;
		if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
		{
			state->cause = CAUSE_FAULT_LOAD;
			state->badvaddr = (long)load_address;
			handle_trap(state);
		}
		silly->fpr[rd] = dest;
	}
	else if (is_flw)
	{
		uint32_t dest;
		if (!memcpy_from_user(current, &dest, load_address, sizeof(dest)))
		{
			state->cause = CAUSE_FAULT_LOAD;
			state->badvaddr = (long)load_address;
			handle_trap(state);
		}
		silly->fpr[rd] = dest;
	}
	else if (is_fsd)
	{
		if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint64_t)))
		{
			state->cause = CAUSE_FAULT_STORE;
			state->badvaddr = (long)store_address;
			handle_trap(state);
		}
	}
	else if (is_flw)
	{
		if (!memcpy_to_user(current, store_address, &silly->fpr[rs2], sizeof(uint32_t)))
		{
			state->cause = CAUSE_FAULT_STORE;
			state->badvaddr = (long)store_address;
			handle_trap(state);
		}
	}
	*/
	else
	  return 1;
	
	silly->fsr = sf.float_rounding_mode << 5 | sf.float_exception_flags;
	return 0;
}

/* For now we can only emulate missing compute insns, not the whole FPU */
int emulate_fpu(struct hw_trapframe *state)
{
	if (!(state->sr & SR_EF))
	{
		state->cause = CAUSE_FP_DISABLED;
		handle_trap(state);
	}

	ancillary_state_t fp_state;
	save_fp_state(&fp_state);
	int code = emulate_fpu_silly(state, &fp_state);
	restore_fp_state(&fp_state);
	return code;
}