#include "instructions.h"
#include "machine.h"
#include "utils.h"

inline int resolveWordOperand(Machine& m, int ea, AddressingMode mode)
{
    switch (mode)
    {
        case AddressingMode::IMMEDIATE:   return ea;
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT:  return m.getWord(ea);
        case AddressingMode::INDIRECT:    return m.getWord(m.getWord(ea));
        default:                          m.invalidAddressing(); return 0;
    }
}

inline double resolveFloatOperand(Machine& m, int ea, AddressingMode mode)
{
    switch (mode)
    {
        case AddressingMode::IMMEDIATE:   return static_cast<double>(ea);
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT:  return m.getFloat(ea);
        case AddressingMode::INDIRECT:    return m.getFloat(m.getWord(ea));
        default:                          m.invalidAddressing(); return 0.0;
    }
}


inline void writeWordOperand(Machine& m, int ea, AddressingMode mode, int value)
{
    switch (mode)
    {
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT:    m.setWord(ea, value); break;    // direct store
        case AddressingMode::INDIRECT:      m.setWord(m.getWord(ea), value); break; // store via pointer
        default:                            m.invalidAddressing(); break;
    }
}

inline void writeFloatOperand(Machine& m, int ea, AddressingMode mode, double value)
{
    switch (mode)
    {
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT:    m.setFloat(ea, value); break;
        case AddressingMode::INDIRECT:      m.setFloat(m.getWord(ea), value); break;
        default:                            m.invalidAddressing(); break;
    }
}


// For jump-like ops: what PC should become?
inline int resolveJumpTarget(Machine& m, int ea, AddressingMode mode)
{
    switch (mode)
    {
        case AddressingMode::IMMEDIATE:
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT: return ea;              // jump to EA (normal case)
        case AddressingMode::INDIRECT:   return m.getWord(ea);   // jump via pointer
        default:                         m.invalidAddressing(); return m.getPC();
    }
}

void fix_handler(Machine &m)
{
    m.setA(static_cast<int>(m.getF()));
}

void float_handler(Machine &m)
{
    m.setF(static_cast<double>(m.getA()));
}

void addr_handler(Machine &m, int r1, int r2)
{
    m.setReg(r2, m.getReg(r1) + m.getReg(r2));
}

void clear_handler(Machine& m, int r, int unused) {
    m.setReg(r, 0);
}

void compr_handler(Machine &m, int r1, int r2)
{
    m.setSW(sic_comp(m.getReg(r1), m.getReg(r2), m.getSW()));
}

void divr_handler(Machine& m, int r1, int r2) {

    if (m.getReg(r2) == 0) {
        m.invalidOpcode(DIVR);
        return;
    }
    m.setReg(r2, m.getReg(r2) / m.getReg(r1));
}

void mulr_handler(Machine &m, int r1, int r2)
{
    m.setReg(r2, m.getReg(r1) * m.getReg(r2));
}

void rmo_handler(Machine &m, int r1, int r2)
{
    m.setReg(r2, m.getReg(r1));
}


// SHIFTL r1, n  → left *circular* shift n bits
void shiftl_handler(Machine &m, int r1, int n)
{
    unsigned int v = m.getReg(r1) & WORD_MASK;
    n %= WORD_SIZE;
    unsigned int res = ((v << n) | (v >> (WORD_SIZE - n))) & WORD_MASK;
    m.setReg(r1, res);
}

// SHIFTR r1, n  → right shift n bits, fill with original leftmost bit
void shiftr_handler(Machine &m, int r1, int n)
{
    unsigned int v = m.getReg(r1) & WORD_MASK;
    n %= WORD_SIZE;
    unsigned int msb = (v & 0x800000) ? 1u : 0u;
    unsigned int shifted = v >> n;
    unsigned int fill = 0;
    if (msb) {
        fill = (~0u) << (WORD_SIZE - n);
        fill &= WORD_MASK;
    }

    unsigned int res = (shifted | fill) & WORD_MASK;
    m.setReg(r1, res);
}

void subr_handler(Machine &m, int r1, int r2)
{
    m.setReg(r2, m.getReg(r2) - m.getReg(r1));
}

// TODO: implement SVC functionality
void svc_handler(Machine &m, int n, int unused)
{
    m.notImplemented("SVC");
}

void tixr_handler(Machine &m, int r1, int unused)
{
    m.setX(m.getX() + 1);
    int valX = m.getX();
    int valR1 = m.getReg(r1);
    m.setSW(sic_comp(valX, valR1, m.getSW()));
}

void add_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA(m.getA() + val);
}

void addf_handler(Machine &m, int ea, AddressingMode mode)
{
    double val = resolveFloatOperand(m, ea, mode);
    m.setA(m.getA() + val);
}

void and_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA(m.getA() & val);
}

void comp_handler(Machine &m, int ea, AddressingMode mode)
{
    int operand = resolveWordOperand(m, ea, mode);
    m.setSW(sic_comp(m.getA(), operand, m.getSW()));
}

void compf_handler(Machine &m, int ea, AddressingMode mode)
{
    double operand = resolveFloatOperand(m, ea, mode);
    m.setSW(sic_comp(m.getF(), operand, m.getSW()));
}

void div_handler(Machine &m, int ea, AddressingMode mode)
{
    int divisor = resolveWordOperand(m, ea, mode);
    if (divisor == 0) {
        m.divisionByZero(DIV);
        return;
    }
    m.setA(m.getA() / divisor);
}

void divf_handler(Machine &m, int ea, AddressingMode mode)
{
    double divisor = resolveFloatOperand(m, ea, mode);
    if (divisor == 0.0) {
        m.divisionByZero(DIVF);
        return;
    }
    m.setF(m.getF() / divisor);
}

void j_handler(Machine &m, int ea, AddressingMode mode)
{
    int target = resolveJumpTarget(m, ea, mode);
    m.setPC(target);
}

void jeq_handler(Machine &m, int ea, AddressingMode mode)
{
    int sw = m.getSW();
    int cc = getCC(sw);
    if (cc == CC_EQ) {
        int target = resolveJumpTarget(m, ea, mode);
        m.setPC(target);
    }
}

void jgt_handler(Machine &m, int ea, AddressingMode mode)
{
    int sw = m.getSW();
    int cc = getCC(sw);
    if (cc == CC_GT) {
        int target = resolveJumpTarget(m, ea, mode);
        m.setPC(target);
    }
}

void jlt_handler(Machine &m, int ea, AddressingMode mode)
{
    int sw = m.getSW();
    int cc = getCC(sw);
    if (cc == CC_LT) {
        int target = resolveJumpTarget(m, ea, mode);
        m.setPC(target);
    }
}

void jsub_handler(Machine &m, int ea, AddressingMode mode)
{
    int target = resolveJumpTarget(m, ea, mode);
    m.setL(m.getPC());
    m.setPC(target);
}

void lda_handler(Machine& m, int ea, AddressingMode mode)
{
    m.setA(resolveWordOperand(m, ea, mode));
}

void ldb_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setB(resolveWordOperand(m, ea, mode));
}

void ldch_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA((m.getA() & 0xFFFF00) | (val & 0xFF));
}

void ldf_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setF(resolveFloatOperand(m, ea, mode));
}

void ldl_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setL(resolveWordOperand(m, ea, mode));
}

void lds_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setS(resolveWordOperand(m, ea, mode));
}

void ldt_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setT(resolveWordOperand(m, ea, mode));
}

void ldx_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setX(resolveWordOperand(m, ea, mode));
}

void mul_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA(m.getA() * val);
}

void mulf_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setF(m.getF() * resolveFloatOperand(m, ea, mode));
}

void or_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA(m.getA() | val);
}

void rsub_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setPC(m.getL());
}

void sta_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getA());
}

void stb_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getB());
}
// Rightmost byte of A register is stored
void stch_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = m.getA() & 0xFF;
    switch (mode)
    {
        case AddressingMode::SIMPLE:
        case AddressingMode::SIC_DIRECT:    m.setByte(ea, val); break;            // direct store
        case AddressingMode::INDIRECT:      m.setByte(m.getWord(ea), val); break; // store via pointer
        default:                            m.invalidAddressing(); break;
    }
}

void stf_handler(Machine &m, int ea, AddressingMode mode)
{
    writeFloatOperand(m, ea, mode, m.getF());
}

void stl_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getL());
}

void sts_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getS());
}

void stsw_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getSW());
}

void stt_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getT());
}

void stx_handler(Machine &m, int ea, AddressingMode mode)
{
    writeWordOperand(m, ea, mode, m.getX());
}

void sub_handler(Machine &m, int ea, AddressingMode mode)
{
    int val = resolveWordOperand(m, ea, mode);
    m.setA(m.getA() - val);
}

void subf_handler(Machine &m, int ea, AddressingMode mode)
{
    double val = resolveFloatOperand(m, ea, mode);
    m.setF(m.getF() - val);
}

void tix_handler(Machine &m, int ea, AddressingMode mode)
{
    m.setX(m.getX() + 1);
    int valX = m.getX();
    int memVal = resolveWordOperand(m, ea, mode);
    m.setSW(sic_comp(valX, memVal, m.getSW()));
}