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

void instruction_movb_ar_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = cpu_read8(status,status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x80) == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x000000FF;
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= 0xFFFFFF00;

    status->pc += 2;
}

void instruction_movb_r_ar(cpu_status_t* status){
    cpu_write8(status, status->r[LO_NIBBLE(cpu_read8(status,status->pc))], status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    status->pc += 2;
}

void instruction_movb_arp_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = cpu_read8(status, status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x80) == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x000000FF;
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= 0xFFFFFF00;

    if (LO_NIBBLE(cpu_read8(status,status->pc)) != HI_NIBBLE(cpu_read8(status,status->pc+1)))
        status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] += 1;

    status->pc += 2;
}

void instruction_movb_r_amr(cpu_status_t* status){
    cpu_write8(status, status->r[LO_NIBBLE(cpu_read8(status,status->pc))] - 1, status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] -= 1;
    status->pc += 2;
}

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

    if ((status->r[0] & 0x80) == 0)
        status->r[0] &= 0x000000FF;
    else
        status->r[0] |= 0xFFFFFF00;

    status->pc += 2;
}

void instruction_movb_r0_r_r(cpu_status_t* status){
  status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = cpu_read8(status, status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] + status->r[0]);

  if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x80) == 0)
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x000000FF;
  else status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= 0xFFFFFF00;

  status->pc += 2;
}

void instruction_movb_r_r0_r(cpu_status_t* status){
    cpu_write8(status, status->r[LO_NIBBLE(cpu_read8(status,status->pc))] + status->r[0], status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))]);
    status->pc += 2;
}

void instruction_movb_disp_gbr_r0(cpu_status_t* status){
    unsigned int disp = (0x000000FF & cpu_read8(status,status->pc+1));
    status->r[0] = cpu_read8(status, status->gbr + disp);

    if ((status->r[0] & 0x80) == 0)
        status->r[0] &= 0x000000FF;
    else
        status->r[0] |= 0xFFFFFF00;

    status->pc += 2;
}

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

void instruction_movb_r0_disp_gbr(cpu_status_t* status){
    int d = LO_NIBBLE(cpu_read8(status,status->pc+1));
    unsigned int disp = (0x000000FF & d);
    cpu_write8(status, status->gbr + disp, status->r[0]);
    status->pc += 2;
}