#include <instructions/instructions.h>

void instruction_div1_r_r(cpu_status_t* status){
    unsigned long tmp0, tmp2;
    unsigned char old_q, tmp1;
    int n = LO_NIBBLE(cpu_read8(status,status->pc));
    int m = HI_NIBBLE(cpu_read8(status,status->pc+1));

    old_q = status->q;
    status->q = (0x80000000 & status->r[n]) != 0;
    tmp2 = status->r[m];
    status->r[n] <<= 1;
    status->r[n] |= (unsigned long)status->t;

    if (old_q == 0)
    {
        if (status->m == 0)
        {
        tmp0 = status->r[n];
        status->r[n] -= tmp2;
        tmp1 = status->r[n] > 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[n];
        status->r[n] += tmp2;
        tmp1 = status->r[n] < tmp0;

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

    else if (old_q == 1)
    {
        if (status->m == 0)
        {
        tmp0 = status->r[n];
        status->r[n] += tmp2;
        tmp1 = status->r[n] < 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[n];
        status->r[n] -= tmp2;
        tmp1 = status->r[n] > tmp0;

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

    status->t = (status->q == status->m);
    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;
}

void instruction_extub_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))] &= 0x000000FF;
    status->pc += 2;
}

void instruction_dt_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))]--;

    if (status->r[LO_NIBBLE(cpu_read8(status,status->pc))] == 0)
        status->t = 1;
    else status->t = 0;

    status->pc += 2;
}

void instruction_div0s_r_r(cpu_status_t* status){
    if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x80000000) == 0)
        status->q = 0;
    else
        status->q = 1;

    if ((status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x80000000) == 0)
        status->m = 0;
    else
        status->m = 1;

    status->t = ! (status->m == status->q);
    status->pc += 2;
}

void instruction_addc_r_r(cpu_status_t* status){
    unsigned long tmp0, tmp1;
    tmp1 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))] + status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))];
    tmp0 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))];
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = tmp1 + status->t;

    if (tmp0>tmp1)
        status->t = 1;
    else
        status->t = 0;

    if (tmp1 > status->r[LO_NIBBLE(cpu_read8(status,status->pc))])
        status->t = 1;

    status->pc += 2;
}

void instruction_subc_r_r(cpu_status_t* status){
    unsigned long tmp0, tmp1;
    tmp1 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))] - status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))];
    tmp0 = status->r[LO_NIBBLE(cpu_read8(status,status->pc))];
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = tmp1 - status->t;

    if (tmp0 < tmp1)
        status->t = 1;
    else
        status->t = 0;

    if (tmp1 < status->r[LO_NIBBLE(cpu_read8(status,status->pc))])
        status->t = 1;

    status->pc += 2;
}

void instruction_extsb_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))];

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

void instruction_cmp_str_r_r(cpu_status_t* status){
    unsigned long temp;
    long HH, HL, LH, LL;
    temp = status->r[LO_NIBBLE(cpu_read8(status,status->pc))] ^ status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))];
    HH = (temp & 0xFF000000) >> 24;
    HL = (temp & 0x00FF0000) >> 16;
    LH = (temp & 0x0000FF00) >> 8;
    LL = temp & 0x000000FF;
    HH = HH && HL && LH && LL;

    if (HH == 0)
        status->t = 1;
    else
        status->t = 0;

    status->pc += 2;
}