#include "assembler.h"

uint32_t R_type(struct instruction *insn, int opcode, int funct3,
    int funct7)
{
    printf(":: R type (opcode=%d funct3=%d funct7=%d args=[%d,%d,%d])\n",
        opcode, funct3, funct7, insn->args[0], insn->args[1], insn->args[2]);
    return opcode | (insn->args[0] << 7) | (funct3 << 12)
           | (insn->args[1] << 15) | (insn->args[2] << 20) | (funct7 << 25);
}

uint32_t I_type(struct instruction *insn, int opcode, int funct3)
{
    printf(":: I type (opcode=%d funct3=%d args=[%d,%d,%d])\n",
        opcode, funct3, insn->args[0], insn->args[1], insn->args[2]);
    return opcode | (insn->args[0] << 7) | (funct3 << 12)
           | (insn->args[1] << 15) | ((insn->args[2] & 0xfff) << 20);
}

uint32_t S_type(struct instruction *insn, int opcode, int funct3)
{
    printf(":: S type (opcode=%d funct3=%d args=[%d,%d,%d])\n",
        opcode, funct3, insn->args[0], insn->args[1], insn->args[2]);
    int imm_4_0  = (insn->args[1] & 0b000000011111);
    int imm_11_5 = (insn->args[1] & 0b111111100000) >> 5;
    return opcode | (imm_4_0 << 7) | (funct3 << 12) | (insn->args[2] << 15)
           | (insn->args[0] << 20) | (imm_11_5 << 25);
}

uint32_t B_type(struct instruction *insn, int opcode, int funct3)
{
    printf(":: B type (opcode=%d funct3=%d args=[%d,%d,%d])\n",
        opcode, funct3, insn->args[0], insn->args[1], insn->args[2]);
    int imm_4_1  = (insn->args[2] & 0b0000000011110) >> 1;
    int imm_10_5 = (insn->args[2] & 0b0011111100000) >> 5;
    int imm_11   = (insn->args[2] & 0b0100000000000) >> 11;
    int imm_12   = (insn->args[2] & 0b1000000000000) >> 12;
    return opcode | (imm_11 << 7) | (imm_4_1 << 8) | (funct3 << 12)
           | (insn->args[0] << 15) | (insn->args[1] << 20) | (imm_10_5 << 25)
           | (imm_12 << 31);
}

uint32_t J_type(struct instruction *insn, int opcode)
{
    printf(":: J type (opcode=%d args=[%d,%d])\n",
        opcode, insn->args[0], insn->args[1]);
    int imm_10_1  = (insn->args[1] & 0b000000000011111111110) >> 1;
    int imm_11    = (insn->args[1] & 0b000000000100000000000) >> 11;
    int imm_19_12 = (insn->args[1] & 0b011111111000000000000) >> 12;
    int imm_20    = (insn->args[1] & 0b100000000000000000000) >> 20;
    return opcode | (insn->args[0] << 7) | (imm_19_12 << 12) | (imm_11 << 20)
           | (imm_10_1 << 21) | (imm_20 << 31);
}

uint32_t encode(struct instruction *insn)
{
    if(!strcmp(insn->name, "addi"))
        return I_type(insn, 0b0010011, 0b000);
    if(!strcmp(insn->name, "add"))
        return R_type(insn, 0b0110011, 0b000, 0b0000000);
    if(!strcmp(insn->name, "beq"))
        return B_type(insn, 0b1100011, 0b000);
    if(!strcmp(insn->name, "bne"))
        return B_type(insn, 0b1100011, 0b001);
    if(!strcmp(insn->name, "blt"))
        return B_type(insn, 0b1100011, 0b100);
    if(!strcmp(insn->name, "bge"))
        return B_type(insn, 0b1100011, 0b101);
    if(!strcmp(insn->name, "jal"))
        return J_type(insn, 0b1101111);
    if(!strcmp(insn->name, "sub"))
        return R_type(insn, 0b0110011, 0b000, 0b0100000);
    if(!strcmp(insn->name, "sd"))
        return S_type(insn, 0b0100011, 0b011);
    if(!strcmp(insn->name, "ecall"))
        return 0x00000073;

    /* Pour ld on inverse les arguments 2 et 3 pour coller au format */
    if(!strcmp(insn->name, "ld")) {
        int imm = insn->args[1];
        int reg = insn->args[2];
        insn->args[1] = reg;
        insn->args[2] = imm;
        return I_type(insn, 0b0000011, 0b011);
    }

    /* Alias */
    if(!strcmp(insn->name, "j")) {
        insn->args[1] = insn->args[0];
        insn->args[0] = 0;
        return J_type(insn, 0b1101111);
    }
    if(!strcmp(insn->name, "li")) {
        insn->args[2] = insn->args[1];
        insn->args[1] = 0; /* x0/zero */
        return I_type(insn, 0b0010011, 0b000);
    }
    if(!strcmp(insn->name, "mv")) {
        insn->args[2] = 0;
        return I_type(insn, 0b0010011, 0b000);
    }

    printf(":: UNKNOWN INSTRUCTION\n");
    return 0;
}
