use crate::state::CpuState;
use std::num::FpCategory;

pub fn add(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1].wrapping_add(state.registers[rs2]);
}

pub fn sub(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1].wrapping_sub(state.registers[rs2]);
}

pub fn xor(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1] ^ state.registers[rs2];
}

pub fn or(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1] | state.registers[rs2];
}

pub fn and(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1] & state.registers[rs2];
}

pub fn sll(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1] << (state.registers[rs2] & 0b11111);
}

pub fn srl(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1] >> (state.registers[rs2] & 0b11111);
}

pub fn sra(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] =
        ((state.registers[rs1] as i32) >> (state.registers[rs2] & 0b11111)) as u32;
}

pub fn slt(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let rs1_signed = state.registers[rs1] as i32;
    let rs2_signed = state.registers[rs2] as i32;

    state.registers[rd] = if rs1_signed < rs2_signed { 1 } else { 0 };
}

pub fn sltu(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.registers[rs1] < state.registers[rs2] {
        1
    } else {
        0
    };
}

pub fn mul(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = state.registers[rs1].wrapping_mul(state.registers[rs2]);
}

pub fn mulh(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = (((state.registers[rs1] as i32 as i64)
        * (state.registers[rs2] as i32 as i64))
        >> 32) as u32;
}

pub fn mulhsu(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let rs1_signed = (state.registers[rs1] as i32) as i64;
    let rs2_unsigned = state.registers[rs2] as i64;

    state.registers[rd] = ((rs1_signed * rs2_unsigned) >> 32) as u32;
}

pub fn mulhu(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] =
        ((state.registers[rs1] as u64 * state.registers[rs2] as u64) >> 32) as u32;
}

pub fn div(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let a = state.registers[rs1] as i32;
    let b = state.registers[rs2] as i32;

    state.registers[rd] = if b == 0 {
        u32::MAX
    } else if a == i32::MIN && b == -1 {
        a as u32
    } else {
        (a / b) as u32
    };
}

pub fn divu(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.registers[rs2] == 0 {
        u32::MAX
    } else {
        state.registers[rs1] / state.registers[rs2]
    };
}

pub fn rem(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let a = state.registers[rs1] as i32;
    let b = state.registers[rs2] as i32;

    state.registers[rd] = if b == 0 {
        a as u32
    } else if a == i32::MIN && b == -1 {
        0
    } else {
        (a % b) as u32
    };
}

pub fn remu(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.registers[rs2] == 0 {
        state.registers[rs1]
    } else {
        state.registers[rs1] % state.registers[rs2]
    };
}

pub fn addi(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1].wrapping_add(imm);
}

pub fn xori(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1] ^ imm;
}

pub fn ori(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1] | imm;
}

pub fn andi(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1] & imm;
}

pub fn slli(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1] << (imm & 0b11111);
}
pub fn srli(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.registers[rs1] >> (imm & 0b11111);
}
pub fn srai(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = ((state.registers[rs1] as i32) >> (imm & 0b11111)) as u32;
}

pub fn slti(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = if (state.registers[rs1] as i32) < (imm as i32) {
        1
    } else {
        0
    };
}

pub fn sltiu(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = if state.registers[rs1] < imm { 1 } else { 0 };
}
pub fn lb(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    let byte = state
        .memory
        .read((state.registers[rs1] + imm) as usize, 99)
        .unwrap();
    state.registers[rd] = (byte as i8) as u32;
}

pub fn lh(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    let bytes = state
        .memory
        .read_multiple_bytes((state.registers[rs1] + imm) as usize, 2, 99)
        .unwrap();

    state.registers[rd] = (((bytes[1] as u16) << 8 | bytes[0] as u16) as i16) as u32;
}

pub fn lw(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    let addr = (state.registers[rs1] + imm) as usize;
    let slice = state.memory.read_multiple_bytes(addr, 4, 99).unwrap();
    let number = u32::from_le_bytes(slice[0..4].try_into().unwrap());

    state.registers[rd] = number;
}

pub fn lbu(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state
        .memory
        .read((state.registers[rs1] + imm) as usize, 99)
        .unwrap() as u32;
}

pub fn lhu(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    let bytes = state
        .memory
        .read_multiple_bytes((state.registers[rs1] + imm) as usize, 2, 99)
        .unwrap();

    state.registers[rd] = ((bytes[1] as u16) << 8 | bytes[0] as u16) as u32;
}
pub fn sb(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    state
        .memory
        .write(
            (state.registers[rs1] + imm) as usize,
            99,
            state.registers[rs2] as u8,
        )
        .unwrap();
}

pub fn sh(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    let halfword = state.registers[rs2] & 0xFFFF;
    let index = (state.registers[rs1] + imm) as usize;

    state
        .memory
        .write_multiple_bytes(index, &[(halfword & 0xFF) as u8, (halfword >> 8) as u8], 99)
        .unwrap();
}

pub fn sw(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    let index = (state.registers[rs1] + imm) as usize;

    state
        .memory
        .write_multiple_bytes(index, &state.registers[rs2].to_le_bytes(), 99)
        .unwrap();
}

pub fn beq(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if state.registers[rs1] == state.registers[rs2] {
        state.pc += imm;
    }
}

pub fn bne(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if state.registers[rs1] != state.registers[rs2] {
        state.pc += imm;
    }
}

pub fn blt(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if (state.registers[rs1] as i32) < (state.registers[rs2] as i32) {
        state.pc += imm;
    }
}

pub fn bge(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if (state.registers[rs1] as i32) >= (state.registers[rs2] as i32) {
        state.pc += imm;
    }
}

pub fn bltu(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if state.registers[rs1] < state.registers[rs2] {
        state.pc += imm;
    }
}

pub fn bgeu(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
    if state.registers[rs1] >= state.registers[rs2] {
        state.pc += imm;
    }
}
pub fn jal(state: &mut CpuState, rd: usize, imm: u32) {
    state.registers[rd] = state.pc + 4;
    state.pc += imm;
}

pub fn jalr(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
    state.registers[rd] = state.pc + 4;
    state.pc = state.registers[rs1] + imm;
}
pub fn lui(state: &mut CpuState, rd: usize, imm: u32) {
    state.registers[rd] = imm << 12;
}

pub fn lr_w(state: &mut CpuState, rd: usize, rs1: usize) {
    let addr = state.registers[rs1] as usize;
    state.registers[rd] = load_word(state, addr);
    state.acquire_reservations(&[addr, addr + 1, addr + 2, addr + 3]);
}

pub fn sc_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    if state.reservations.contains(&addr) {
        store_word(state, addr, state.registers[rs2]);
        state.registers[rd] = 0;
    } else {
        state.registers[rd] = 1;
    }
}

pub fn amoswap_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = number;
    store_word(state, addr, state.registers[rs2]);
}

pub fn amoadd_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = (number as i32 + state.registers[rs2] as i32) as u32;
    store_word(state, addr, state.registers[rd]);
}

pub fn amoand_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = number & state.registers[rs2];
    store_word(state, addr, state.registers[rd]);
}

pub fn amoor_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = number | state.registers[rs2];
    store_word(state, addr, state.registers[rd]);
}

pub fn amoxor_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = number ^ state.registers[rs2];
    store_word(state, addr, state.registers[rd]);
}

pub fn amomax_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = std::cmp::max(number as i32, state.registers[rs2] as i32) as u32;
    store_word(state, addr, state.registers[rd]);
}

pub fn amomin_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = std::cmp::min(number as i32, state.registers[rs2] as i32) as u32;
    store_word(state, addr, state.registers[rd]);
}

pub fn amomaxu_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = std::cmp::max(number, state.registers[rs2]);
    store_word(state, addr, state.registers[rd]);
}

pub fn amominu_w(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let addr = state.registers[rs1] as usize;
    let number: u32 = load_word(state, addr);
    state.registers[rd] = std::cmp::min(number, state.registers[rs2]);
    store_word(state, addr, state.registers[rd]);
}

fn load_word(state: &CpuState, addr: usize) -> u32 {
    let slice = state.memory.read_multiple_bytes(addr, 4, 99).unwrap();
    u32::from_le_bytes(slice[0..4].try_into().unwrap())
}
fn store_word(state: &mut CpuState, addr: usize, value: u32) {
    state
        .memory
        .write_multiple_bytes(addr, &value.to_le_bytes(), 99)
        .unwrap();
}

pub fn auipc(state: &mut CpuState, rd: usize, imm: u32) {
    state.registers[rd] = state.pc + (imm << 12);
}

pub fn fmadd_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize, rs3: usize) {
    state.f_registers[rd] =
        state.f_registers[rs1] * state.f_registers[rs2] + state.f_registers[rs3];
}

pub fn fmsub_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize, rs3: usize) {
    state.f_registers[rd] =
        state.f_registers[rs1] * state.f_registers[rs2] - state.f_registers[rs3];
}

pub fn fnmadd_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize, rs3: usize) {
    state.f_registers[rd] =
        -state.f_registers[rs1] * state.f_registers[rs2] + state.f_registers[rs3];
}

pub fn fnmsub_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize, rs3: usize) {
    state.f_registers[rd] =
        -state.f_registers[rs1] * state.f_registers[rs2] - state.f_registers[rs3];
}

pub fn fadd_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1] + state.f_registers[rs2];
}

pub fn fsub_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1] - state.f_registers[rs2];
}

pub fn fmul_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1] * state.f_registers[rs2];
}

pub fn fdiv_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    let a = state.f_registers[rs1];
    let b = state.f_registers[rs2];

    state.f_registers[rd] = if b == 0.0 {
        f32::NAN
        // todo: raise division by zero in csr
    } else {
        a / b
    }
}

pub fn fsqrt_s(state: &mut CpuState, rd: usize, rs1: usize) {
    state.f_registers[rd] = state.f_registers[rs1].sqrt();
}

pub fn fsgnj_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1].abs() * state.f_registers[rs2].signum();
}
pub fn fsgnjn_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1].abs() * -state.f_registers[rs2].signum();
}

pub fn fsgnjx_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1] * state.f_registers[rs2].signum();
}
pub fn fmin_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1].min(state.f_registers[rs2]);
}
pub fn fmax_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.f_registers[rd] = state.f_registers[rs1].max(state.f_registers[rs2]);
}

pub fn fcvt_s_w(state: &mut CpuState, rd: usize, rs1: usize) {
    state.f_registers[rd] = state.registers[rs1] as i32 as f32;
}

pub fn fcvt_s_wu(state: &mut CpuState, rd: usize, rs1: usize) {
    state.f_registers[rd] = state.registers[rs1] as f32;
}

pub fn fcvt_w_s(state: &mut CpuState, rd: usize, rs1: usize) {
    state.registers[rd] = state.f_registers[rs1] as i32 as u32;
}

pub fn fcvt_wu_s(state: &mut CpuState, rd: usize, rs1: usize) {
    state.registers[rd] = state.f_registers[rs1] as u32;
}

pub fn fmv_x_w(state: &mut CpuState, rd: usize, rs1: usize) {
    state.registers[rd] = u32::from_le_bytes(state.f_registers[rs1].to_le_bytes());
}

pub fn fmv_w_x(state: &mut CpuState, rd: usize, rs1: usize) {
    state.f_registers[rd] = f32::from_le_bytes(state.registers[rs1].to_le_bytes());
}

pub fn feq_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.f_registers[rs1] == state.f_registers[rs2] {
        1
    } else {
        0
    }
}

pub fn flt_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.f_registers[rs1] < state.f_registers[rs2] {
        1
    } else {
        0
    }
}

pub fn fle_s(state: &mut CpuState, rd: usize, rs1: usize, rs2: usize) {
    state.registers[rd] = if state.f_registers[rs1] <= state.f_registers[rs2] {
        1
    } else {
        0
    }
}

pub fn fclass_s(state: &mut CpuState, rd: usize, rs1: usize) {
    let number = state.f_registers[rs1];
    let class = number.classify();
    let frac_msb = (number.to_bits() >> 22) & 0x01;

    state.registers[rd] = match class {
        FpCategory::Nan => {
            if frac_msb == 0 {
                0b1
            } else {
                0b10
            }
        }
        FpCategory::Infinite => {
            if number.is_sign_negative() {
                0b100
            } else {
                0b100000000
            }
        }
        FpCategory::Zero => {
            if number.is_sign_negative() {
                0b10000
            } else {
                0b100000
            }
        }
        FpCategory::Subnormal => {
            if number.is_sign_negative() {
                0b1000
            } else {
                0b1000000
            }
        }
        FpCategory::Normal => {
            if number.is_sign_negative() {
                0b100
            } else {
                0b10000000
            }
        }
    }
}