#include <instructions/instructions.h>

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

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

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

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

    status->pc += 2;
}

void instruction_shar_r(cpu_status_t* status){
    long temp;

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

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

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

    if (temp == 1)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] |= 0x80000000;
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x7FFFFFFF;

    status->pc += 2;
}

void instruction_shll2_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= 2;
    status->pc += 2;
}

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

    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >>= 1;
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x7FFFFFFF;
    status->pc += 2;
}

void instruction_shlr2_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >>= 2;
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x3FFFFFFF;
    status->pc += 2;
}

void instruction_shld_r_r(cpu_status_t* status){
    int sgn = status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x80000000;

    if (sgn == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= (status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F);
    else if ((status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F) == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = 0;
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = (unsigned)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >> ((~status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F) + 1);

    status->pc += 2;
}

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

    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= 1;
    status->pc += 2;
}

void instruction_shll16_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= 16;
    status->pc += 2;
}

void instruction_shll8_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= 8;
    status->pc += 2;
}

void instruction_rotcr_r (cpu_status_t* status){
    long temp;

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

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

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

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

    status->pc += 2;
}

void instruction_shad_r_r(cpu_status_t* status){
    int sgn = status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x80000000;

    if (sgn == 0)
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] <<= (status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F);
    else if ((status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F) == 0)
    {
        if ((status->r[LO_NIBBLE(cpu_read8(status,status->pc))] & 0x80000000) == 0)
            status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = 0;
        else
            status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = 0xFFFFFFFF;
    }
    else
        status->r[LO_NIBBLE(cpu_read8(status,status->pc))] = (long)status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >> ((~status->r[HI_NIBBLE(cpu_read8(status,status->pc+1))] & 0x1F) + 1);

    status->pc += 2;
}

void instruction_shlr16_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >>= 16;
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x0000FFFF;
    status->pc += 2;
}

void instruction_shlr8_r(cpu_status_t* status){
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] >>= 8;
    status->r[LO_NIBBLE(cpu_read8(status,status->pc))] &= 0x00FFFFFF;
    status->pc += 2;
}