#include <instructions/instructions.h>

void instruction_div1_r_r(cpu_status_t* status){
    unsigned long tmp0, tmp2;
    unsigned char old_q, tmp1;

    old_q = status->q;
    status->q = (0x80000000 & status->r[LO_NIBBLE(cpu_read8(status,status->pc))]) != 0;
    tmp2 = status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))];
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= 1;
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= (unsigned long)status->t;

    if (old_q == 0){
        if (status->m == 0){
            tmp0 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))];
            status->r[LO_NIBBLE(cpu_read8(status,status->pc))] -= tmp2;
            tmp1 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))] > tmp0;

            if (status->q == 0)
                status->q = tmp1;
            else if (status->q == 1)
                status->q = tmp1 == 0;
        }

        else if (status->m == 1)
        {
            tmp0 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))];
            status->r[LO_NIBBLE(cpu_read8(status,status->pc))] += tmp2;
            tmp1 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))] < tmp0;

            status->q = tmp1 == 0;
        }

    status->pc += 2;
}
}

void instruction_add_r_r(cpu_status_t* 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_add_imm_r(cpu_status_t* status){
  if ((cpu_read8(status,status->pc+1) & 0x80) == 0)
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] += (0x000000FF & (long)cpu_read8(status,status->pc+1));
  else
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] += (0xFFFFFF00 | (long)cpu_read8(status,status->pc+1));

  status->pc += 2;
}

void instruction_cmp_pz_r(cpu_status_t* status){
    if ((long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >= 0)
        status->t = 1;
    else
        status->t = 0;
    status->pc += 2;
}

void instruction_cmp_gt_r_r(cpu_status_t* status){
    if ((long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] > (long)status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))])
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

void instruction_cmp_hs_r_r(cpu_status_t* status){
    if ((unsigned long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >= (unsigned long)status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))])
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

void instruction_sub_r_r(cpu_status_t* 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_extuw_r_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))];
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x0000FFFF;
    status->pc += 2;
}

void instruction_cmp_pl_r(cpu_status_t* status){
    if ((long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] > 0)
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

void instruction_cmp_eq_imm_r0(cpu_status_t* status){
    long imm;
    int i = cpu_read8(status,status->pc+1);

    if ((i & 0x80) == 0)
        imm = (0x000000FF & (long)i);
    else
        imm = (0xFFFFFF00 | (long)i);

    if (status->r[0] == imm)
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

void instruction_cmp_hi_r_r(cpu_status_t* status){
    if ((unsigned long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] > (unsigned long)status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))])
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

void instruction_cmp_ge_r_r(cpu_status_t* status){
    if ((long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >= (long)status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))])
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}

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

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

    status->pc += 2;
}