#include "rv32cpu.h"
#include "instruction.h"

#include "memory/memory.h"
#include "memory/mmu/mmu.h"
#include "vriscv.h"

#include <stdlib.h>
#include <stdio.h>

rv32_cpu_t* cpu0;

typedef union RAW_INSTRUCTION
{
	uint32_t data;
	struct
	{
		uint8_t opcode : 7;
		uint16_t rd : 5;
		uint16_t func3 : 3;
		uint16_t rs1 : 5;
		uint16_t rs2 : 5;
		uint16_t func7 : 7;
	} __attribute__((packed));
} __attribute__((packed)) raw_instruction_t;

typedef struct INSTRUCTION
{
	uint8_t opcode;
	uint32_t immediate;
	uint8_t func3;
	uint8_t func7;
	uint8_t rd;
	uint8_t rs1;
	uint8_t rs2;
} instruction_t;

static void cpu_print_instruction(instruction_t* instruction);

void cpu_init()
{
	cpu0 = malloc(sizeof(rv32_cpu_t));
	cpu0->regs.zero = 0;
}

static void cpu_decode(raw_instruction_t raw_instruction, instruction_t* output)
{
	output->opcode = raw_instruction.opcode;
	output->immediate = 0;
	output->func3 = raw_instruction.func3;
	output->func7 = raw_instruction.func7;
	output->rd = raw_instruction.rd;
	output->rs1 = raw_instruction.rs1;
	output->rs2 = raw_instruction.rs2;

	// Decode immediate, and make sure opcode is correct
	switch(raw_instruction.opcode)
	{
		// U-type instructions
		case OPCODE_LUI:
		case OPCODE_AUIPC:
			output->immediate = raw_instruction.data & 0xFFFFF000;
			break;
		// J-type instructions
		case OPCODE_JAL:
			// Last bit (31) of data is immediate bit 20
			output->immediate = (raw_instruction.data & 0x80000000) >> 11;
			// Then following 10 bits (30-21) are immediate bits 10-1
			output->immediate |= (raw_instruction.data & 0x7FE00000) >> 20;
			// Following bit (20) is immediate bit 11
			output->immediate |= (raw_instruction.data & 0x200000) >> 10;
			// Last bits (19-12) are immediate bits 19-12
			output->immediate |= (raw_instruction.data & 0xFF000);
			break;
		// I-type instructions
		case OPCODE_JALR:
		case OPCODE_LOAD:
		case OPCODE_ARITHLOG_IMM:
		case OPCODE_SYSTEM:
			// Bits 31-20 are immediate bits 11-0
			output->immediate = (raw_instruction.data & 0xFFF00000) >> 20;
			break;
		// B-type instructions
		case OPCODE_BRANCH:
			// Last bit (31) of data is immediate bit 12
			output->immediate = (raw_instruction.data & 0x80000000) >> 19;
			// Then following 6 bits (30-25) are immediate bits 10-5
			output->immediate |= (raw_instruction.data & 0x7E000000) >> 20;
			// On rd field, last 4 bits (4:1) are immediate bits 4:1
			output->immediate |= (raw_instruction.rd & 0x1E);
			// On rd field, first bit (0) is immediate bit 11
			output->immediate |= (raw_instruction.rd & 0x01) << 11;
			break;
		// R-type instructions
		case OPCODE_ARITHLOG:
		case OPCODE_ATOMIC:
			break;
		// S-type instructions
		case OPCODE_STORE:
			// Bits 31-25 (func7) are immediate bits 11:5
			output->immediate = raw_instruction.func7 << 5;
			// Bits of rd are immediate bits 4:0
			output->immediate |= raw_instruction.rd;
			break;
		case OPCODE_NOP:
			// TODO : Decode NOP instructions
			break;
		default:
			fprintf(stderr, "Error: Unknown instruction opcode 0x%x, could not decode\n", raw_instruction.opcode);
			exit(EXIT_FAILURE);
			break;
	}
}

static void cpu_execute(rv32_cpu_t* cpu, instruction_t* instruction)
{
	switch(instruction->opcode)
	{
		case OPCODE_LUI:
		{
			// Load Upper Immediate (load immediate(31:12 bits) in rd)
			cpu->regs.x[instruction->rd] = instruction->immediate;
			break;
		}
		case OPCODE_AUIPC:
		{
			// Add Upper Immediate to PC
			cpu->regs.x[instruction->rd] = instruction->immediate + cpu->pc;
			break;
		}
		case OPCODE_JAL:
		{
			// Jump And Link
			cpu->regs.x[instruction->rd] = cpu->pc + 4;
			// Sign extend immediate from 21 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0x1FFFFF) | (instruction->immediate & 0x100000 ? 0xFFE00000 : 0);
			cpu->pc += immediate - 4;
			break;
		}
		case OPCODE_JALR:
		{
			// Jump And Link Register
			cpu->regs.x[instruction->rd] = cpu->pc + 4;
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);
			cpu->pc = ((cpu->regs.x[instruction->rs1] + immediate) & 0xFFFFFFFE) - 4;
			break;
		}
		case OPCODE_BRANCH:
		{
			// Branches ; to know which one, we must analyse func3
			// Sign extend immediate from 13 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x1000 ? 0xFFFFE000 : 0);
			immediate -= 4;

			switch(instruction->func3)
			{
				case FUNC3_BEQ:
					// Branch EQual
					if(cpu->regs.x[instruction->rs1] == cpu->regs.x[instruction->rs2])
						cpu->pc = immediate;
					break;
				case FUNC3_BNE:
					// Branch Not Equal
					if(cpu->regs.x[instruction->rs1] != cpu->regs.x[instruction->rs2])
						cpu->pc = immediate;
					break;
				case FUNC3_BLT:
					// Branch Less Than
					if(((int32_t) cpu->regs.x[instruction->rs1]) < ((int32_t) cpu->regs.x[instruction->rs2]))
						cpu->pc = immediate;
					break;
				case FUNC3_BLTU:
					// Branch Less Than Unsigned
					if(cpu->regs.x[instruction->rs1] < cpu->regs.x[instruction->rs2])
						cpu->pc = immediate;
					break;
				case FUNC3_BGE:
					// Branch Greater Equal
					if(((int32_t) cpu->regs.x[instruction->rs1]) >= ((int32_t) cpu->regs.x[instruction->rs2]))
						cpu->pc = immediate;
					break;
				case FUNC3_BGEU:
					// Branch Greater Equal Unsigned
					if(cpu->regs.x[instruction->rs1] >= cpu->regs.x[instruction->rs2])
						cpu->pc = immediate;
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for branch instruction, could not execute\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		case OPCODE_LOAD:
		{
			// Loads ; to know which one, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);
			uint32_t address = cpu->regs.x[instruction->rs1] + immediate;

			switch(instruction->func3)
			{
				case FUNC3_LB:
					// Load Byte (8-bits)
					cpu->regs.x[instruction->rd] = memory[mmu_translate(address)];
					// Sign extend from 8 bits to 32 bits
					cpu->regs.x[instruction->rd] |= (cpu->regs.x[instruction->rd] & 0x80 ? 0xFFFFFF00 : 0);
					break;
				case FUNC3_LH:
					// Load Halfword (16-bits)
					cpu->regs.x[instruction->rd] = *((uint16_t*) &memory[mmu_translate(address)]);
					// Sign extend from 16 bits to 32 bits
					cpu->regs.x[instruction->rd] |= (cpu->regs.x[instruction->rd] & 0x8000 ? 0xFFFF0000 : 0);
					break;
				case FUNC3_LW:
					// Load Word (32-bits)
					cpu->regs.x[instruction->rd] = *((uint32_t*) &memory[mmu_translate(address)]);
					break;
				case FUNC3_LBU:
					// Load Byte Unsigned (8-bits)
					cpu->regs.x[instruction->rd] = memory[mmu_translate(address)];
					break;
				case FUNC3_LHU:
					// Load Halfword Unsigned (16-bits)
					cpu->regs.x[instruction->rd] = *((uint16_t*) &memory[mmu_translate(address)]);
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for load instruction, could not execute\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		case OPCODE_STORE:
		{
			// Store ; to know which one, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);
			uint32_t address = cpu->regs.x[instruction->rs1] + immediate;

			switch(instruction->func3)
			{
				case FUNC3_SB:
					// Store Byte (8-bits)
					memory[mmu_translate(address)] = cpu->regs.x[instruction->rs2] & 0xFF;
					break;
				case FUNC3_SH:
					// Store Halfword (16-bits)
					*((uint16_t*) &memory[mmu_translate(address)]) = cpu->regs.x[instruction->rs2] & 0xFFFF;
					break;
				case FUNC3_SW:
					// Store Word (32-bits)
					*((uint32_t*) &memory[mmu_translate(address)]) = cpu->regs.x[instruction->rs2];
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for store instruction, could not execute\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}

			break;
		}
		case OPCODE_ARITHLOG_IMM:
		{
			// Arithmetic and logic instructions on immediate values
			// To find out which operation, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);

			switch(instruction->func3)
			{
				case FUNC3_ADDI:
					// ADD Immediate
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] + immediate;
					break;
				case FUNC3_SLTI:
					// Set Less Than Immediate
					if(((int32_t) cpu->regs.x[instruction->rs1]) < ((int32_t) immediate))
						cpu->regs.x[instruction->rd] = 1;
					else
						cpu->regs.x[instruction->rd] = 0;
					break;
				case FUNC3_SLTIU:
					// Set Less Than Immediate Unsigned
					if(cpu->regs.x[instruction->rs1] < immediate)
						cpu->regs.x[instruction->rd] = 1;
					else
						cpu->regs.x[instruction->rd] = 0;
					break;
				case FUNC3_XORI:
					// XOR Immediate
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] ^ immediate;
					break;
				case FUNC3_ORI:
					// OR Immediate
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] | immediate;
					break;
				case FUNC3_ANDI:
					// AND Immediate
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] & immediate;
					break;
				case FUNC3_SLLI:
					// Sign-extend immediate in rs2 from 5 bits to 32 bits
					immediate = (instruction->rs2 & 0x1F) | (instruction->rs2 & 0x10 ? 0xFFFFFFE0 : 0);
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] << immediate;
					break;
				case FUNC3_SRLI_SRAI:
					// Sign-extend immediate in rs2 from 5 bits to 32 bits
					immediate = (instruction->rs2 & 0x1F) | (instruction->rs2 & 0x10 ? 0xFFFFFFE0 : 0);
					// Analyse func7 to know which is it
					switch(instruction->func7)
					{
						case FUNC7_SRLI:
							cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] >> immediate;
							break;
						case FUNC7_SRAI:
							// Arithmetic slide
							cpu->regs.x[instruction->rd] = ((int32_t) cpu->regs.x[instruction->rs1]) >> immediate;
							break;
						default:
							fprintf(stderr, "FATAL: Unknown func7 0x%x for arithlog immediate SRLI/SRAI instruction, could not execute\n", instruction->func7);
							exit(EXIT_FAILURE);
							break;
					}
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for arithlog immediate instruction, could not execute\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		case OPCODE_ARITHLOG:
		{
			// Arithmetic and logic instructions
			// To find out which operation, we must analyse func3 and func7
			switch(instruction->func3)
			{
				case FUNC3_ADD_SUB:
					switch(instruction->func7)
					{
						case FUNC7_ADD:
							cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] + cpu->regs.x[instruction->rs2];
							break;
						case FUNC7_SUB:
							cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] - cpu->regs.x[instruction->rs2];
							break;
						default:
							fprintf(stderr, "FATAL: Unknown func7 0x%x for arithlog ADD/SUB instruction, could not execute\n", instruction->func7);
							exit(EXIT_FAILURE);
							break;
					}
					break;
				case FUNC3_SLL:
					// Slide Left Logical
					uint32_t sll_value = cpu->regs.x[instruction->rs2] & 0x1F;
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] << sll_value;
					break;
				case FUNC3_SLT:
					// Set Less Than
					if(((int32_t) cpu->regs.x[instruction->rs1]) < ((int32_t) cpu->regs.x[instruction->rs2]))
						cpu->regs.x[instruction->rd] = 1;
					else
						cpu->regs.x[instruction->rd] = 0;
					break;
				case FUNC3_SLTIU:
					// Set Less Than Unsigned
					if(cpu->regs.x[instruction->rs1] < cpu->regs.x[instruction->rs2])
						cpu->regs.x[instruction->rd] = 1;
					else
						cpu->regs.x[instruction->rd] = 0;
					break;
				case FUNC3_XOR:
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] ^ cpu->regs.x[instruction->rs2];
					break;
				case FUNC3_OR:
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] | cpu->regs.x[instruction->rs2];
					break;
				case FUNC3_AND:
					cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] & cpu->regs.x[instruction->rs2];
					break;
				case FUNC3_SRL_SRA:
					switch(instruction->func7)
					{
						case FUNC7_SRL:
							// Slide Right Logical
							uint32_t srl_value = cpu->regs.x[instruction->rs2] & 0x1F;
							cpu->regs.x[instruction->rd] = cpu->regs.x[instruction->rs1] >> srl_value;
							break;
						case FUNC7_SRA:
							// Slide Right Arithmetical
							uint32_t sra_value = cpu->regs.x[instruction->rs2] & 0x1F;
							cpu->regs.x[instruction->rd] = ((int32_t) cpu->regs.x[instruction->rs1]) >> sra_value;
							break;
						default:
							fprintf(stderr, "FATAL: Unknown func7 0x%x for arithlog SRL/SRA instruction, could not execute\n", instruction->func7);
							exit(EXIT_FAILURE);
							break;
					}
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for arithlog instruction, could not execute\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		case OPCODE_NOP:
		{
			// TODO : Implement PAUSE, FENCE, FENCE.TSO
			fprintf(stderr, "Warning: Unsupported NOP instruction\n");
			break;
		}
		case OPCODE_SYSTEM:
		{
			switch(instruction->func3)
			{
				case FUNC3_ECALL_EBREAK:
					switch(instruction->immediate)
					{
						case IMM_ECALL:
							fprintf(stderr, "ECALL: a7(sbi ext id) = %d, a6(sbi funct id) = %d\n", cpu->regs.a7, cpu->regs.a6);
							break;
						case IMM_EBREAK:
							fprintf(stderr, "EBREAK\n");
							break;
						default:
							fprintf(stderr, "FATAL: Unknown IMM for ECALL/EBREAK instruction while executing (IMM=0x%x)\n", instruction->immediate);
							exit(EXIT_FAILURE);
							break;
					}
					break;
				case FUNC3_CSRRW:
					fprintf(stderr, "CSRRW\n");
					break;
				case FUNC3_CSRRS:
					fprintf(stderr, "CSRRS\n");
					break;
				case FUNC3_CSRRC:
					fprintf(stderr, "CSRRC\n");
					break;
				case FUNC3_CSRRWI:
					fprintf(stderr, "CSRRWI\n");
					break;
				case FUNC3_CSRRSI:
					fprintf(stderr, "CSRRSI\n");
					break;
				case FUNC3_CSRRCI:
					fprintf(stderr, "CSRRCI\n");
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func3 0x%x for SYSTEM instruction while executing\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}

			break;
		}
		case OPCODE_ATOMIC:
		{
			// Atomic instructions (A extension)
			// To find out which operation, we must analyse func7 (func3 == 0x2)
			if(instruction->func3 != FUNC3_ATOMIC)
			{
				fprintf(stderr, "FATAL: Unknown func3 0x%x for ATOMIC instruction, could not execute\n", instruction->func3);
				exit(EXIT_FAILURE);
			}

			// FUNC7 contains 2 flag bits in lower part ; ignore them, we look for func7_5
			uint32_t address = cpu->regs.x[instruction->rs1];
			uint32_t* ptr = ((uint32_t*) &memory[mmu_translate(address)]);
			switch(instruction->func7 >> 2)
			{
				case FUNC75_LRW:
					// Load-Reserved Word
					cpu->regs.x[instruction->rd] = *ptr;
					// TODO register reservation set that subsumes the bytes in word
					fprintf(stderr, "LR.W\n");
					break;
				case FUNC75_SCW:
					// Store-Conditional Word
					// TODO succeed only if the reservation is still valid and the reservation set contains the bytes written
					*ptr = cpu->regs.x[instruction->rs2];
					cpu->regs.x[instruction->rd] = 0; // TODO write 1 in rd on failure
					fprintf(stderr, "SC.W\n");
					break;
				case FUNC75_AMOSWAPW:
					// Atomic Memory Operation SWAP Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Put in RS1 addr the value of RS2
					*ptr = cpu->regs.x[instruction->rs2];
					// Put in RS2 the value of RS1 addr (which is in RD)
					cpu->regs.x[instruction->rs2] = cpu->regs.x[instruction->rd];
					break;
				case FUNC75_AMOADDW:
					// Atomic Memory Operation ADD Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Add rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] + cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOXORW:
					// Atomic Memory Operation XOR Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Xor rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] ^ cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOANDW:
					// Atomic Memory Operation AND Word
					cpu->regs.x[instruction->rd] = *ptr;
					// AND rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] & cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOORW:
					// Atomic Memory Operation OR Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Or rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] | cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOMINW:
					// Atomic Memory Operation MIN Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Min rs1 addr and value of rs2
					*ptr = ((int32_t) cpu->regs.x[instruction->rd]) < ((int32_t) cpu->regs.x[instruction->rs2]) ? cpu->regs.x[instruction->rd] : cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOMAXW:
					// Atomic Memory Operation MAX Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Max rs1 addr and value of rs2
					*ptr = ((int32_t) cpu->regs.x[instruction->rd]) > ((int32_t) cpu->regs.x[instruction->rs2]) ? cpu->regs.x[instruction->rd] : cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOMINUW:
					// Atomic Memory Operation MIN Unsigned Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Min rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] < cpu->regs.x[instruction->rs2] ? cpu->regs.x[instruction->rd] : cpu->regs.x[instruction->rs2];
					break;
				case FUNC75_AMOMAXUW:
					// Atomic Memory Operation MAX Unsigned Word
					cpu->regs.x[instruction->rd] = *ptr;
					// Max rs1 addr and value of rs2
					*ptr = cpu->regs.x[instruction->rd] > cpu->regs.x[instruction->rs2] ? cpu->regs.x[instruction->rd] : cpu->regs.x[instruction->rs2];
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func7 0x%x for ATOMIC/0x2 instruction, could not execute\n", instruction->func7);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		default:
			fprintf(stderr, "FATAL: Unknown instruction opcode 0x%x while executing; how could this decode ?\n", instruction->opcode);
			exit(EXIT_FAILURE);
			break;
	}
}

void cpu_loop(rv32_cpu_t* cpu)
{
	while(1)
	{
		// Fetch
		raw_instruction_t raw_instruction;
		if(cpu->pc > memory_size - 4)
		{
			fprintf(stderr, "Error: instruction fetch: pc is out of addressable memory\n");
			exit(EXIT_FAILURE);
		}
		raw_instruction.data = *((uint32_t*) (&memory[cpu->pc]));

		// Decode
		instruction_t instruction;
		cpu_decode(raw_instruction, &instruction);

		printf("0x%x: ", cpu->pc);
		cpu_print_instruction(&instruction);

		// Execute
		cpu_execute(cpu, &instruction);
		
		// Reset value of zero, in case instruction tried to modify
		cpu->regs.zero = 0;

		cpu->pc += 4;
	}
}

static void cpu_print_instruction(instruction_t* instruction)
{
	switch(instruction->opcode)
	{
		case OPCODE_LUI:
		{
			// Load Upper Immediate (load immediate(31:12 bits) in rd)
			printf("lui x%u, 0x%x\n", instruction->rd, instruction->immediate);
			break;
		}
		case OPCODE_AUIPC:
		{
			// Add Upper Immediate to PC
			printf("auipc x%u, 0x%x\n", instruction->rd, instruction->immediate);
			break;
		}
		case OPCODE_JAL:
		{
			// Jump And Link
			// Sign extend immediate from 21 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0x1FFFFF) | (instruction->immediate & 0x100000 ? 0xFFE00000 : 0);
			printf("jal x%u, 0x%x\n", instruction->rd, immediate);
			break;
		}
		case OPCODE_JALR:
		{
			// Jump And Link Register
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);
			printf("jalr x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
			break;
		}
		case OPCODE_BRANCH:
		{
			// Branches ; to know which one, we must analyse func3
			// Sign extend immediate from 13 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x1000 ? 0xFFFFE000 : 0);
			immediate -= 4;

			switch(instruction->func3)
			{
				case FUNC3_BEQ:
					// Branch EQual
					printf("beq x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				case FUNC3_BNE:
					// Branch Not Equal
					printf("bne x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				case FUNC3_BLT:
					// Branch Less Than
					printf("blt x%u, x%u, %d\n", instruction->rs1, instruction->rs2, (int32_t) immediate);
					break;
				case FUNC3_BLTU:
					// Branch Less Than Unsigned
					printf("bltu x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				case FUNC3_BGE:
					// Branch Greater Equal
					printf("bge x%u, x%u, %d\n", instruction->rs1, instruction->rs2, (int32_t) immediate);
					break;
				case FUNC3_BGEU:
					// Branch Greater Equal Unsigned
					printf("bgeu x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for branch instruction\n", instruction->func3);
					break;
			}
			break;
		}
		case OPCODE_LOAD:
		{
			// Loads ; to know which one, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);

			switch(instruction->func3)
			{
				case FUNC3_LB:
					printf("lb x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_LH:
					// Load Halfword (16-bits)
					printf("lh x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_LW:
					// Load Word (32-bits)
					printf("lw x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_LBU:
					// Load Byte Unsigned (8-bits)
					printf("lbu x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_LHU:
					// Load Halfword Unsigned (16-bits)
					printf("lhu x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for load instruction\n", instruction->func3);
					break;
			}
			break;
		}
		case OPCODE_STORE:
		{
			// Store ; to know which one, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);

			switch(instruction->func3)
			{
				case FUNC3_SB:
					// Store Byte (8-bits)
					printf("sb x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				case FUNC3_SH:
					// Store Halfword (16-bits)
					printf("sh x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				case FUNC3_SW:
					// Store Word (32-bits)
					printf("sw x%u, x%u, 0x%x\n", instruction->rs1, instruction->rs2, immediate);
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for store instruction\n", instruction->func3);
					break;
			}

			break;
		}
		case OPCODE_ARITHLOG_IMM:
		{
			// Arithmetic and logic instructions on immediate values
			// To find out which operation, we must analyse func3
			// Sign extend immediate from 12 bits to 32 bits
			uint32_t immediate = (instruction->immediate & 0xFFF) | (instruction->immediate & 0x800 ? 0xFFFFF000 : 0);

			switch(instruction->func3)
			{
				case FUNC3_ADDI:
					// ADD Immediate
					printf("addi x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_SLTI:
					// Set Less Than Immediate
					printf("slti x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_SLTIU:
					printf("sltiu x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_XORI:
					// XOR Immediate
					printf("xori x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_ORI:
					// OR Immediate
					printf("ori x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_ANDI:
					// AND Immediate
					printf("andi x%u, x%u, 0x%x\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_SLLI:
					// Sign-extend immediate in rs2 from 5 bits to 32 bits
					immediate = (instruction->rs2 & 0x1F) | (instruction->rs2 & 0x10 ? 0xFFFFFFE0 : 0);
					printf("slli x%u, x%u, %u\n", instruction->rd, instruction->rs1, immediate);
					break;
				case FUNC3_SRLI_SRAI:
					// Sign-extend immediate in rs2 from 5 bits to 32 bits
					immediate = (instruction->rs2 & 0x1F) | (instruction->rs2 & 0x10 ? 0xFFFFFFE0 : 0);
					// Analyse func7 to know which is it
					switch(instruction->func7)
					{
						case FUNC7_SRLI:
							printf("srli x%u, x%u, %u\n", instruction->rd, instruction->rs1, immediate);
							break;
						case FUNC7_SRAI:
							// Arithmetic slide
							printf("srai x%u, x%u, %u\n", instruction->rd, instruction->rs1, immediate);
							break;
						default:
							fprintf(stderr, "Warning: Unknown func7 0x%x for arithlog immediate SRLI/SRAI instruction\n", instruction->func7);
							break;
					}
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for arithlog immediate instruction, could not execute\n", instruction->func3);
					break;
			}
			break;
		}
		case OPCODE_ARITHLOG:
		{
			// Arithmetic and logic instructions
			// To find out which operation, we must analyse func3 and func7
			switch(instruction->func3)
			{
				case FUNC3_ADD_SUB:
					switch(instruction->func7)
					{
						case FUNC7_ADD:
							printf("add x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
							break;
						case FUNC7_SUB:
							printf("sub x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
							break;
						default:
							fprintf(stderr, "Warning: Unknown func7 0x%x for arithlog ADD/SUB instruction, could not execute\n", instruction->func7);
							break;
					}
					break;
				case FUNC3_SLL:
					// Slide Left Logical
					printf("sll x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_SLT:
					// Set Less Than
					printf("slt x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_SLTIU:
					// Set Less Than Unsigned
					printf("sltu x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_XOR:
					printf("xor x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_OR:
					printf("or x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_AND:
					printf("and x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC3_SRL_SRA:
					switch(instruction->func7)
					{
						case FUNC7_SRL:
							// Slide Right Logical
							printf("srl x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
							break;
						case FUNC7_SRA:
							// Slide Right Arithmetical
							printf("sra x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
							break;
						default:
							fprintf(stderr, "Warning: Unknown func7 0x%x for arithlog SRL/SRA instruction\n", instruction->func7);
							exit(EXIT_FAILURE);
							break;
					}
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for arithlog instruction\n", instruction->func3);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		case OPCODE_NOP:
		{
			// TODO : Implement PAUSE, FENCE, FENCE.TSO
			fprintf(stderr, "Warning: Unsupported NOP instruction\n");
			break;
		}
		case OPCODE_SYSTEM:
		{
			switch(instruction->func3)
			{
				case FUNC3_ECALL_EBREAK:
					switch(instruction->immediate)
					{
						case IMM_ECALL:
							printf("ecall\n");
							break;
						case IMM_EBREAK:
							printf("ebreak\n");
							break;
						default:
							fprintf(stderr, "Warning: Unknown IMM for ECALL/EBREAK instruction (IMM=0x%x)\n", instruction->immediate);
							break;
					}
					break;
				case FUNC3_CSRRW:
					printf("csrrw csr[0x%x], x%u, x%u\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				case FUNC3_CSRRS:
					printf("csrrs csr[0x%x], x%u, x%u\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				case FUNC3_CSRRC:
					printf("csrrc csr[0x%x], x%u, x%u\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				case FUNC3_CSRRWI:
					printf("csrrwi csr[0x%x], x%u, 0x%x\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				case FUNC3_CSRRSI:
					printf("csrrsi csr[0x%x], x%u, 0x%x\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				case FUNC3_CSRRCI:
					printf("csrrci csr[0x%x], x%u, 0x%x\n", instruction->immediate, instruction->rd, instruction->rs1);
					break;
				default:
					fprintf(stderr, "Warning: Unknown func3 0x%x for SYSTEM instruction\n", instruction->func3);
					break;
			}
			break;
		}
		case OPCODE_ATOMIC:
		{
			// Atomic instructions (A extension)
			// To find out which operation, we must analyse func7 (func3 == 0x2)
			if(instruction->func3 != FUNC3_ATOMIC)
			{
				fprintf(stderr, "Warning: Unknown func3 0x%x for ATOMIC instruction\n", instruction->func3);
				break;
			}

			// FUNC7 contains 2 flag bits in lower part ; ignore them, we look for func7_5
			switch(instruction->func7 >> 2)
			{
				case FUNC75_LRW:
					// Load-Reserved Word
					printf("lr.w x%u, x%u\n", instruction->rd, instruction->rs1);
					break;
				case FUNC75_SCW:
					// Store-Conditional Word
					printf("sc.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOSWAPW:
					// Atomic Memory Operation SWAP Word
					printf("amoswap.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOADDW:
					// Atomic Memory Operation ADD Word
					printf("amoadd.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOXORW:
					// Atomic Memory Operation XOR Word
					printf("amoxor.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOANDW:
					// Atomic Memory Operation AND Word
					printf("amoand.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOORW:
					// Atomic Memory Operation OR Word
					printf("amoor.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOMINW:
					// Atomic Memory Operation MIN Word
					printf("amomin.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOMAXW:
					// Atomic Memory Operation MAX Word
					printf("amomax.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOMINUW:
					// Atomic Memory Operation MIN Unsigned Word
					printf("amominu.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				case FUNC75_AMOMAXUW:
					// Atomic Memory Operation MAX Unsigned Word
					printf("amomaxu.w x%u, x%u, x%u\n", instruction->rd, instruction->rs1, instruction->rs2);
					break;
				default:
					fprintf(stderr, "FATAL: Unknown func7 0x%x for ATOMIC/0x2 instruction, could not execute\n", instruction->func7);
					exit(EXIT_FAILURE);
					break;
			}
			break;
		}
		default:
			fprintf(stderr, "Warning: Unknown instruction opcode 0x%x while printing\n", instruction->opcode);
			break;
	}
}