#include "emulator/hart.h"

#include "emulator/bits.h"
#include "emulator/instruction.h"
#include "emulator/csr.h"

#include <stdio.h>
#include <inttypes.h>
#include <assert.h>
#include <stdbool.h>
#include <string.h>

#define REG_ZERO 0
#define REG_RA   1
#define REG_SP   2
#define REG_GP   3
#define REG_TP   4
#define REG_T0   5
#define REG_T1   6
#define REG_T2   7
#define REG_S0   8
#define REG_S1   9
#define REG_A0   10
#define REG_A1   11
#define REG_A2   12
#define REG_A3   13
#define REG_A4   14
#define REG_A5   15
#define REG_A6   16
#define REG_A7   17
#define REG_S2   18
#define REG_S3   19
#define REG_S4   20
#define REG_S5   21
#define REG_S6   22
#define REG_S7   23
#define REG_S8   24
#define REG_S9   25
#define REG_S10  26
#define REG_S11  27
#define REG_T3   28
#define REG_T4   29
#define REG_T5   30
#define REG_T6   31

static void handle_ecall(Hart* hart)
{
}

static inline uint32_t load_size(Hart* hart, uint32_t address, uint32_t size)
{
    if ((address & 0x80000000) == 0)
    {
        assert(address + size < hart->mem_size);
        uint32_t value = 0;
        memcpy(&value, hart->mem + address, size);
        return value;
    }
    
    return 0;
}

static uint32_t load_byte(Hart* hart, uint32_t address)
{
    return load_size(hart, address, 1);
}

static uint32_t load_half(Hart* hart, uint32_t address)
{
    return load_size(hart, address, 2);
}

static uint32_t load_word(Hart* hart, uint32_t address)
{
    return load_size(hart, address, 4);
}

static inline void store_size(Hart* hart, uint32_t address, uint32_t value, uint32_t size)
{
    if ((address & 0x80000000) == 0)
    {
        assert(address + size < hart->mem_size);
        memcpy(hart->mem + address, &value, size);
    }
    else if (address == 0x80000000)
    {
        fwrite(&value, 1, size, stdout);
    }
}

static void store_byte(Hart* hart, uint32_t address, uint8_t value)
{
    store_size(hart, address, value, 1);
}

static void store_half(Hart* hart, uint32_t address, uint16_t value)
{
    store_size(hart, address, value, 2);
}

static void store_word(Hart* hart, uint32_t address, uint32_t value)
{
    store_size(hart, address, value, 4);
}

static void execute_op_imm(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_i_type(instruction);
    if (inst.rd == 0) return;
    
    switch (inst.funct3)
    {
        case 0: // ADDI
            hart->regs[inst.rd] = hart->regs[inst.rs1] + inst.imm;
            break;
        case 1: // SLLI
            hart->regs[inst.rd] = hart->regs[inst.rs1] << (inst.imm & 0x1F);
            break;
        case 2: // SLTI
            hart->regs[inst.rd] = (int32_t)hart->regs[inst.rs1] < (int32_t)inst.imm ? 1 : 0;
            break;
        case 3: // SLTIU
            hart->regs[inst.rd] = hart->regs[inst.rs1] < inst.imm ? 1 : 0;
            break;
        case 4: // XORI
            hart->regs[inst.rd] = hart->regs[inst.rs1] ^ inst.imm;
            break;
        case 5: // SRLI, SRAI
        {
            const uint32_t shamt = inst.imm & 0x1F;
            uint32_t res = hart->regs[inst.rs1] >> shamt;
            if ((inst.imm & 0x400) && shamt > 0) { res = sign_extend(res, 32 - shamt); }
            hart->regs[inst.rd] = res;
            break;
        }
        case 6: // ORI
            hart->regs[inst.rd] = hart->regs[inst.rs1] | inst.imm;
            break;
        case 7: // ANDI
            hart->regs[inst.rd] = hart->regs[inst.rs1] & inst.imm;
            break;
        default:
            assert(!"Unhandled OP-IMM");
    }
}

static void execute_op(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_r_type(instruction);
    if (inst.rd == 0) return;

    uint32_t bank = inst.funct7 & 0x5F;
    if (bank == 0)
    {
        switch (inst.funct3)
        {
            case 0: // ADD, SUB
                if (inst.funct7 & 0x20)
                {
                    hart->regs[inst.rd] = hart->regs[inst.rs1] - hart->regs[inst.rs2];
                }
                else
                {
                    hart->regs[inst.rd] = hart->regs[inst.rs1] + hart->regs[inst.rs2];
                }
                break;
            case 1: // SLL
                hart->regs[inst.rd] = hart->regs[inst.rs1] << (hart->regs[inst.rs2] & 0x1F);
                break;
            case 2: // SLT
                hart->regs[inst.rd] = (int32_t)hart->regs[inst.rs1] < (int32_t)hart->regs[inst.rs2] ? 1 : 0;
                break;
            case 3: // SLTU
                hart->regs[inst.rd] = hart->regs[inst.rs1] < hart->regs[inst.rs2] ? 1 : 0;
                break;
            case 4: // XOR
                hart->regs[inst.rd] = hart->regs[inst.rs1] ^ hart->regs[inst.rs2];
                break;
            case 5: // SRL, SRA
            {
                const uint32_t shamt = hart->regs[inst.rs2] & 0x1F;
                uint32_t res = hart->regs[inst.rs1] >> shamt;
                if ((inst.funct7 & 0x20) && shamt > 0) { res = sign_extend(res, 32 - shamt); }
                hart->regs[inst.rd] = res;
                break;
            }
            case 6: // OR
                hart->regs[inst.rd] = hart->regs[inst.rs1] | hart->regs[inst.rs2];
                break;
            case 7: // AND
                hart->regs[inst.rd] = hart->regs[inst.rs1] & hart->regs[inst.rs2];
                break;
            default:
                assert(!"Unhandled OP, bank = 0");
                break;
        }
    }
    else if (bank == 1)
    {
        if ((inst.funct3 & 0x4) == 0)
        {
            uint64_t a = hart->regs[inst.rs1];
            uint64_t b = hart->regs[inst.rs2];

            switch (inst.funct3 & 0x3)
            {
                case 1: // MULH
                    b = sign_extend_64(b, 32);
                case 2: // MULHSU
                    a = sign_extend_64(a, 32);
                case 0: // MUL
                case 3: // MULHU
                    break;
            }

            uint64_t r = a * b;
            hart->regs[inst.rd] = (inst.funct3 == 0) ? r & 0xFFFFFFFF : r >> 32;
        }
        else
        {
            uint32_t a = hart->regs[inst.rs1];
            uint32_t b = hart->regs[inst.rs2];

            switch (inst.funct3 & 0x3)
            {
                case 0: // DIV
                {
                    if (b == 0)
                    {
                        hart->regs[inst.rd] = 0xFFFFFFFF;
                    }
                    else if (a == 0x80000000 && b == 0xFFFFFFFF)
                    {
                        hart->regs[inst.rd] = 0x80000000;
                    }
                    else
                    {
                        hart->regs[inst.rd] = (uint32_t)((int32_t)a / (int32_t)b);
                    }
                    break;
                }
                case 1: // DIVU
                {
                    if (b == 0)
                    {
                        hart->regs[inst.rd] = 0xFFFFFFFF;
                    }
                    else
                    {
                        hart->regs[inst.rd] = a / b;
                    }
                    break;
                }
                case 2: // REM
                {
                    if (b == 0)
                    {
                        hart->regs[inst.rd] = a;
                    }
                    else if (a == 0x80000000 && b == 0xFFFFFFFF)
                    {
                        hart->regs[inst.rd] = 0;
                    }
                    else
                    {
                        hart->regs[inst.rd] = (uint32_t)((int32_t)a % (int32_t)b);
                    }
                    break;
                }
                case 3: // REMU
                {
                    if (b == 0)
                    {
                        hart->regs[inst.rd] = a;
                    }
                    else
                    {
                        hart->regs[inst.rd] = a % b;
                    }
                    break;
                }
            }
        }
    }
    else
    {
        assert(!"Unhandled OP bank");
    }
}

static void execute_branch(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_b_type(instruction);
    const uint32_t r1 = hart->regs[inst.rs1];
    const uint32_t r2 = hart->regs[inst.rs2];
    bool take_branch = false;

    switch (inst.funct3)
    {
        case 0: take_branch = (r1 == r2); break;
        case 1: take_branch = (r1 != r2); break;
        case 4: take_branch = (r1 < r2); break;
        case 5: take_branch = (r1 >= r2); break;
        case 6: take_branch = ((int32_t)r1 < (int32_t)r2); break;
        case 7: take_branch = ((int32_t)r1 >= (int32_t)r2); break;
    }

    if (take_branch)
    {
        hart->pc += inst.imm;
    }
    else
    {
        hart->pc += 4;
    }
}

static void execute_load(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_i_type(instruction);
    const uint32_t address = hart->regs[inst.rs1] + inst.imm;
    
    uint32_t value = 0;
    
    switch (inst.funct3 & 0x03)
    {
        case 0:
            value = load_byte(hart, address);
            if ((inst.funct3 & 0x40) == 0)
            {
                value = sign_extend(value, 8);
            }
            break;
        case 1:
            value = load_half(hart, address);
            if ((inst.funct3 & 0x40) == 0)
            {
                value = sign_extend(value, 16);
            }
            break;
        case 2:
            value = load_byte(hart, address);
            break;
        default:
            assert(!"Unhandled load size");
            break;
    }

    if (inst.rd != 0)
    {
        hart->regs[inst.rd] = value;
    }
}

static void execute_store(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_s_type(instruction);
    const uint32_t address = hart->regs[inst.rs1] + inst.imm;
    const uint32_t value = hart->regs[inst.rs2];

    switch (inst.funct3 & 0x03)
    {
        case 0: store_byte(hart, address, value & 0xFF); break;
        case 1: store_half(hart, address, value & 0xFFFF); break;
        case 2: store_word(hart, address, value); break;
        default:
            assert(!"Unhandled store size");
            break;
    }
}

static void execute_misc_mem(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_i_type(instruction);
    
    if (inst.funct3 == 0)
    {
        // FENCE
    }
    else
    {
        assert(!"Unhandled MISC-MEM instruction");
    }
}

static void execute_system(Hart* hart, uint32_t instruction)
{
    const Instruction inst = decode_i_type(instruction);

    switch (inst.funct3)
    {
        case 0:
        {
            if (inst.rs1 == 0 && inst.rd == 0)
            {
                if (inst.imm == 0)
                {
                    handle_ecall(hart);
                }
                else if (inst.imm == 1)
                {
                    // EBREAK
                }
                else
                {
                    assert(!"Unhandled SYSTEM/PRIV instruction");
                }
            }
            else
            {
                assert(!"Unhandled SYSTEM/PRIV instruction");
            }
            break;
        }
        case 1: // CSRRW
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            
            if (inst.rd == 0)
            {
                csr_action(hart, csr_id, hart->regs[inst.rs1], CSR_W);
            }
            else
            {
                hart->regs[inst.rd] = csr_action(hart, csr_id, hart->regs[inst.rs1], CSR_RW);
            }
            break;
        }
        case 2: // CSRRS
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            uint32_t value = 0;
            
            if (inst.rs1 == 0)
            {
                value = csr_action(hart, csr_id, 0, CSR_R);
            }
            else
            {
                value = csr_action(hart, csr_id, hart->regs[inst.rs1], CSR_RS);
            }
            
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = value;
            }
            break;
        }
        case 3: // CSRRC
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            uint32_t value = 0;
            
            if (inst.rs1 == 0)
            {
                value = csr_action(hart, csr_id, 0, CSR_R);
            }
            else
            {
                value = csr_action(hart, csr_id, hart->regs[inst.rs1], CSR_RC);
            }
            
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = value;
            }
            break;
        }
        case 5: // CSRRWI
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            
            if (inst.rd == 0)
            {
                csr_action(hart, csr_id, inst.rs1, CSR_W);
            }
            else
            {
                hart->regs[inst.rd] = csr_action(hart, csr_id, inst.rs1, CSR_RW);
            }
            break;
        }
        case 6: // CSRRSI
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            uint32_t value = 0;
            
            if (inst.rs1 == 0)
            {
                value = csr_action(hart, csr_id, 0, CSR_R);
            }
            else
            {
                value = csr_action(hart, csr_id, inst.rs1, CSR_RS);
            }
            
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = value;
            }
            break;
        }
        case 7: // CSRRCI
        {
            uint16_t csr_id = inst.imm & 0xFFF;
            uint32_t value = 0;
            
            if (inst.rs1 == 0)
            {
                value = csr_action(hart, csr_id, 0, CSR_R);
            }
            else
            {
                value = csr_action(hart, csr_id, inst.rs1, CSR_RC);
            }
            
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = value;
            }
            break;
        }
        default:
            assert(!"Unhandled SYSTEM instruction");
            break;
    }
}

void execute(Hart* hart, uint32_t instruction)
{
    // @Todo Better time (same on all cores)
    // @Todo Memory-map mtime and mtimecmp
    hart->time = __rdtsc();
    
    switch (instruction & 0x7f)
    {
        case 0x03:
            execute_load(hart, instruction);
            hart->pc += 4;
            break;
        case 0x0F:
            execute_misc_mem(hart, instruction);
            hart->pc += 4;
            break;
        case 0x13:
            execute_op_imm(hart, instruction);
            hart->pc += 4;
            break;
        case 0x17: // AUIPC
        {
            Instruction inst = decode_u_type(instruction);
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = inst.imm + hart->pc;
            }
            hart->pc += 4;
            break;
        }
        case 0x23:
            execute_store(hart, instruction);
            hart->pc += 4;
            break;
        case 0x33:
            execute_op(hart, instruction);
            hart->pc += 4;
            break;
        case 0x37: // LUI
        {
            Instruction inst = decode_u_type(instruction);
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = inst.imm;
            }
            hart->pc += 4;
            break;
        }
        case 0x63:
            execute_branch(hart, instruction);
            break;
        case 0x67: // JALR
        {
            Instruction inst = decode_i_type(instruction);
            assert(inst.funct3 == 0);
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = hart->pc + 4;
            }
            hart->pc = (hart->regs[inst.rs1] + inst.imm) & 0xFFFFFFFE;
            break;
        }
        case 0x6F: // JAL
        {
            Instruction inst = decode_j_type(instruction);
            if (inst.rd != 0)
            {
                hart->regs[inst.rd] = hart->pc + 4;
            }
            hart->pc += inst.imm;
            break;
        }
        case 0x73:
        {
            execute_system(hart, instruction);
            hart->pc += 4;
            break;
        }
        default:
            assert(!"Unhandled opcode");
    }

    assert(hart->regs[0] == 0);

    hart->instret += 1;
}

void execute_from(Hart* hart, uint32_t start_address)
{
    hart->pc = start_address;

    while (true)
    {
        uint32_t instruction = load_word(hart, hart->pc);
        execute(hart, instruction);
    }
}

void csr_init(Hart* hart);

void hart_init(Hart* hart, uint32_t id, char* mem, uint32_t mem_size)
{
    memset(hart, 0, sizeof(Hart));
    hart->hartid = id;
    hart->priv = PRIV_M;
    hart->mem = mem;
    hart->mem_size = mem_size;
    
    csr_init(hart);
}