#include "riscv_emu.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

void riscv_debug_print_binary(uint8_t num)
{
    for (int i = 7; i >= 0; i--)
    {
        printf("%c", (num & (1 << i)) ? '1' : '0');
    }
    // printf("\n");
}

void riscv_debug_print_registers(RiscvEmu *emu)
{
    for (int i = 0; i < 32; i++)
    {
        printf("[r%d] => 0x%x\n", i, emu->registers[i]);
    }
}

void riscv_emu_init(RiscvEmu *emu)
{
    memset(emu->registers, 0, sizeof(emu->registers));
    memset(emu->memory, 0, sizeof(emu->memory));
    emu->pc = 0;
}

void riscv_execute_addi(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("ADDI x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] += instr->imm;
}

void riscv_execute_slti(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SLTI x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] = (SIGNED(emu->registers[instr->rs1]) < SIGNED(instr->imm)) ? 1 : 0;
}

void riscv_execute_sltiu(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SLTIU x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] = (UNSIGNED(emu->registers[instr->rs1]) < UNSIGNED(instr->imm)) ? 1 : 0;
}

void riscv_execute_xori(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("XORI x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] = emu->registers[instr->rs1] ^ instr->imm;
}

void riscv_execute_ori(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("ORI x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] = emu->registers[instr->rs1] | instr->imm;
}

void riscv_execute_andi(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("ANDI x%d, %d\n", instr->rd, instr->imm);
    emu->registers[instr->rd] = emu->registers[instr->rs1] & instr->imm;
}

void riscv_execute_slli(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SLLI x%d, %d\n", instr->rd, instr->imm);
    uint32_t shamt = instr->rs2;
    emu->registers[instr->rd] = emu->registers[instr->rs1] << shamt;
}

void riscv_execute_srli(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SRLI x%d, %d\n", instr->rd, instr->imm);
    uint32_t shamt = instr->rs2;
    uint32_t msr = emu->registers[instr->rs1] & 0x80000000;
    emu->registers[instr->rd] = msr ? ~(~emu->registers[instr->rs1] >> shamt) :
                   emu->registers[instr->rs1] >> shamt;
}


void riscv_execute_add(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("ADD x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] + emu->registers[instr->rs2];
}

void riscv_execute_sub(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SUB x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] - emu->registers[instr->rs2];
}

void riscv_execute_sll(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SLL x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] << emu->registers[instr->rs2];
}

void riscv_execute_slt(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("SLT x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = (SIGNED(emu->registers[instr->rs1]) < SIGNED(emu->registers[instr->rs2])) ? 1 : 0;
}


void riscv_execute_xor(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("XOR x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] ^ emu->registers[instr->rs2];
}

void riscv_execute_or(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("OR x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] | emu->registers[instr->rs2];
}

void riscv_execute_and(RiscvEmu *emu, RiscvInstruction *instr)
{
    printf("AND x%d, x%d, x%d\n", instr->rd, instr->rs1, instr->rs2);
    emu->registers[instr->rd] = emu->registers[instr->rs1] & emu->registers[instr->rs2];
}



void riscv_decode_instruction(uint32_t instr, RiscvInstruction *decoded_instr)
{
    decoded_instr->opcode = instr & 0x7F;
    decoded_instr->rd = (instr >> 7) & 0x1F;
    decoded_instr->funct3 = (instr >> 12) & 0x7;
    decoded_instr->rs1 = (instr >> 15) & 0x1F;
    decoded_instr->rs2 = (instr >> 20) & 0x1F;
    decoded_instr->funct7 = (instr >> 25) & 0x7F;

    // Immediate field decoding
    switch (decoded_instr->opcode)
    {
    case 0x03: // LOAD
    case 0x13: // I-type
    case 0x1B: // I-type (32-bit)
    case 0x67: // JALR
        decoded_instr->imm = (int32_t)(instr & 0xFFF00000) >> 20;
        break;

    case 0x23: // STORE
    case 0x33: // R-type
    case 0x3B: // R-type (32-bit)
        decoded_instr->imm = 0;
        break;

    case 0x63: // B-type
        decoded_instr->imm = ((instr >> 7) & 0x1e) | ((instr >> 20) & 0x7e0) | ((instr << 4) & 0x800);
        decoded_instr->imm = (decoded_instr->imm ^ 0x800) - 0x800; // Sign extend

        break;

    case 0x6F: // JAL
        decoded_instr->imm = ((int32_t)(instr & 0x80000000) >> 19) | (instr & 0xFF000) | ((instr >> 9) & 0x800) | ((instr >> 20) & 0x7FE);

        break;

    default:
        decoded_instr->imm = 0;
        break;
    }
}

void riscv_execute_instruction(RiscvEmu *emu, RiscvInstruction *instruction)
{
    switch (instruction->opcode)
    {
    case 0x13:
        if (instruction->funct3 == 0x00)
        {
        }
        break;
    case 0x33:
        if (instruction->funct3 == 0x00 && instruction->funct7 == 0x0)
        {
            riscv_execute_add(emu, instruction);
        }
        else if (instruction->funct3 == 0x00 && instruction->funct7 == 0x20)
        {
            printf("SUB x%d, x%d, x%d\n", instruction->rd, instruction->rs1, instruction->rs2);
            emu->registers[instruction->rd] = emu->registers[instruction->rs1] - emu->registers[instruction->rs2];
        }
        else
        {
            printf("Unkown function with opcode 0x33, func3: ");
            riscv_debug_print_binary(instruction->funct3);
            printf(" func7: ");
            riscv_debug_print_binary(instruction->funct7);
            printf("\n");
        }
        break;
    case 0x23:
        if (instruction->funct3 == 0x02)
        {
            printf("SW x%d, x%d, x%d\n", instruction->rd, instruction->rs1, instruction->rs2);
        }
        else
        {
            printf("Unkown function with opcode 0x23 func3: ");
            riscv_debug_print_binary(instruction->funct3);
            printf("\n");
        }
        break;
    case 0x63:
        if (instruction->funct3 == 0x4)
        {
            printf("BLT x%d, x%d,%d\n", instruction->rs1, instruction->rs2, (int32_t)(instruction->imm) / 4);
            if (emu->registers[instruction->rs1] < emu->registers[instruction->rs2])
            {
                emu->pc = emu->pc + (int32_t)instruction->imm - 4;
                printf("x9: %d\n", emu->registers[9]);
                printf("x0: %d\n", emu->registers[0]);
                if (emu->registers[9] == 0)
                    exit(1); // + ( - 4;
            }

            printf("pc: %d\n", emu->pc);
        }
        else
        {
            printf("Unkown function with opcode 0x63: ");
            riscv_debug_print_binary(instruction->funct3);
            printf("\n");
        }
        break;
    default:
        printf("UNKOWN Instruction detected! opcode: ");
        riscv_debug_print_binary(instruction->opcode);
        printf(" func3: ");
        riscv_debug_print_binary(instruction->funct3);
        printf(" func7: ");
        riscv_debug_print_binary(instruction->funct7);
        printf("\n");
        break;
    }
}

void riscv_emulate(RiscvEmu *emu)
{
    int running = 1;

    while (running && emu->pc < 7 * 4)
    {
        // Fetch instruction
        uint32_t instruction = *((uint32_t *)(emu->memory + emu->pc));

        // Decode instruction
        RiscvInstruction decoded_instr;
        riscv_decode_instruction(instruction, &decoded_instr);
        riscv_execute_instruction(emu, &decoded_instr);
        // Execute instruction (to be implemented)
        // riscv_execuxte_instruction(emu, &decoded_instr);

        // Update program counter (to be implemented)
        emu->pc += 4;
    }
    riscv_debug_print_registers(emu);
}