#include <cpu.h>
#include <instructions/instructions.h>

void instruction_movw_r0_disp_r(cpu_status_t* status){
    long disp = (0x0000000F & (long)status->r[LO_NIBBLE(cpu_read8(status,status->pc+1))]);
    cpu_write16(status, status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] + (disp << 1), status->r[0]);
    status->pc += 2;
}

void instruction_movw_disp_r_r0(cpu_status_t* status){
    long disp = (0x0000000F & (long)status->r[LO_NIBBLE(cpu_read8(status,status->pc+1))]);
    status->r[0] = cpu_read16(status, status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] + (disp << 1));

    if ((status->r[0] & 0x8000) == 0)
        status->r[0] &= 0x0000FFFF;
    else
        status->r[0] |= 0xFFFF0000;

    status->pc += 2;
}

void instruction_movw_disp_pc_r0 (cpu_status_t* status){
    int d = cpu_read8(status,status->pc+1);
    unsigned int disp = (0x000000FF & d);
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = cpu_read16(status,status->pc + 4 + (disp << 1));
    if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x8000) == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x0000FFFF;
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= 0xFFFF0000;

    status->pc += 2;
}

void instruction_movw_r_ar(cpu_status_t* status){
    cpu_write16(status, status->r[LO_NIBBLE(cpu_read8(status,status->pc))], status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    //printf("%d -> @%d\n", HI_NIBBLE(cpu_read8(status,status->pc+1)), LO_NIBBLE(cpu_read8(status,status->pc)));
    status->pc += 2;
}