#include <instructions/instructions.h>
#include <stdio.h>
#include <syscall.h>

void instruction_jmp_r(cpu_status_t* status){
    unsigned int temp;
    temp = status->pc;
    uint32_t a = status->r[LO_NIBBLE(cpu_read8(status,temp))];
    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;
    if(status->r[LO_NIBBLE(cpu_read8(status,temp))] != 0x80010070){
        status->pc = a;
        if(status->log) printf("\e[34mpc: %8x jump to %08x (r%02d)\e[39m\n", temp, status->r[LO_NIBBLE(cpu_read8(status,temp))],LO_NIBBLE(cpu_read8(status,temp)));
    }
    else{
        syscall_handle(status, temp);
    } 
}

void instruction_bsr_lbl(cpu_status_t* status){
    //int d = LO_NIBBLE(cpu_read8(status,status->pc)) + (HI_NIBBLE(cpu_read8(status,status->pc+1)) << 8) + (LO_NIBBLE(cpu_read8(status,status->pc+1)) << 16);
    //int d = (int)(LO_NIBBLE(cpu_read8(status,status->pc)) << 16) + (int)(HI_NIBBLE(cpu_read8(status,status->pc+1)) << 8) + (int)(LO_NIBBLE(cpu_read8(status,status->pc+1)));
    int d = (int)(LO_NIBBLE(cpu_read8(status,status->pc)) << 8) + (int)(HI_NIBBLE(cpu_read8(status,status->pc+1)) << 4) + (int)(LO_NIBBLE(cpu_read8(status,status->pc+1)));
    int disp;
    unsigned int temp;
    temp = status->pc;

    if ((d & 0x800) == 0)
        disp = (0x00000FFF & d);
    else
        disp = (0xFFFFF000 | d);

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    status->pr = status->pc + 4;
    status->pc = status->pc + 4 + (disp << 1);

    if(status->log) printf("\e[34mpc: %8x jump (bsr) to %08x \e[39m\n", temp, status->pc);

}

void instruction_jsr_ar(cpu_status_t* status){
    unsigned int temp;
    temp = status->pc;
    uint32_t a = status->r[LO_NIBBLE(cpu_read8(status,temp))];

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    if(a == 0x80010070){
        status->pr = temp + 4;
        syscall_handle(status, temp);
    }
    else{
        status->pr = temp + 4;
        status->pc = a;
        if(status->log) printf("\e[34mpc: %8x jump (jsr) to %08x \e[39m\n", temp, status->pc);
    }
}

void instruction_rts(cpu_status_t* status){
    unsigned int temp;
    temp = status->pc;
    uint32_t pr = status->pr;

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    status->pc = pr;

    if(status->log) printf("\e[34mpc: %8x jump (rts) to %08x \e[39m\n", temp, status->pr);
}

void instruction_bf_lbl(cpu_status_t* status){
    int disp;
    int temp = status->pc;
    if ((cpu_read8(status,status->pc+1) & 0x80) == 0)
        disp = (0x000000FF & cpu_read8(status,status->pc+1));
    else
        disp = (0xFFFFFF00 | cpu_read8(status,status->pc+1));

    if (status->t == 0){
        status->pc = status->pc + 4 + (disp << 1);
        if(status->log) printf("\e[34mpc: %8x jump (bf) to %08x \e[39m\n", temp, status->pc);
    }
    else
        status->pc += 2;
}

void instruction_bra_lbl(cpu_status_t* status){
    int d = (int)(LO_NIBBLE(cpu_read8(status,status->pc)) << 8) + (int)(HI_NIBBLE(cpu_read8(status,status->pc+1)) << 4) + (int)(LO_NIBBLE(cpu_read8(status,status->pc+1)));
    int disp;
    unsigned int temp;
    temp = status->pc;

    if ((d & 0x800) == 0)
        disp = (0x00000FFF & d);
    else
        disp = (0xFFFFF000 | d);

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    status->pc = status->pc + 4 + (disp << 1);
    if(status->log) printf("\e[34mpc: %8x jump (bra) to %08x \e[39m\n", temp, status->pc);
}

void instruction_bt_lbl(cpu_status_t* status){
    int disp;
    int temp = status->pc;
    if ((cpu_read8(status,status->pc+1) & 0x80) == 0)
        disp = (0x000000FF & cpu_read8(status,status->pc+1));
    else
        disp = (0xFFFFFF00 | cpu_read8(status,status->pc+1));

    if (status->t == 1){
        status->pc = status->pc + 4 + (disp << 1);
        if(status->log) printf("\e[34mpc: %8x jump (bt) to %08x \e[39m\n", temp, status->pc);
    } 
    else
        status->pc += 2;
}

void instruction_bts_lbl(cpu_status_t* status){
    int disp;
    unsigned temp;
    temp = status->pc;

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    if ((cpu_read8(status,status->pc+1) & 0x80) == 0)
        disp = (0x000000FF & cpu_read8(status,status->pc+1));
    else
        disp = (0xFFFFFF00 | cpu_read8(status,status->pc+1));

    if (status->t == 1){
        status->pc = status->pc + 4 + (disp << 1);
        if(status->log) printf("\e[34mpc: %8x jump (bt/s) to %08x \e[39m\n", temp, status->pc);
    }
        
    else
        status->pc += 4;
}

void instruction_bfs_lbl(cpu_status_t* status){   
    int d = cpu_read8(status,status->pc+1);
    int disp;
    unsigned int temp;
    temp = status->pc;

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    if ((d & 0x80) == 0)
        disp = (0x000000FF & d);
    else
        disp = (0xFFFFFF00 | d);

    if (status->t == 0){
        status->pc = status->pc + 4 + (disp << 1);
        if(status->log) printf("\e[34mpc: %8x jump (bf/s) to %08x \e[39m\n", temp, status->pc);
    }  
    else
        status->pc += 4;
}

void instruction_braf_r(cpu_status_t* status){
    unsigned int temp;
    temp = status->pc;
    uint32_t a = status->r[LO_NIBBLE(cpu_read8(status,status->pc))];

    status->pc += 2;
    cpu_execute(status);
    status->pc = temp;

    status->pc = temp + 4 + a;
}

