#include "machine.h"
#include "device.h"
#include "opcode.h"
#include <stdexcept>
using namespace std;

Machine::Machine() {
    //nastavi registre
    A = B = L = T = S = X = PC = SW = 0;
    F = 0.0;
    //nastavi naprave
    devices[0] = new InputDevice(cin);
    devices[1] = new OutputDevice(cout);
    devices[2] = new OutputDevice(cerr);

    for (int i = 3; i < 256; i++) {
        string filename = to_string(i) + ".dev";
        devices[i] = new FileDevice(filename);
    }
}

void Machine::resetRegisters() {
    setA(0);
    setB(0);
    setL(0);
    setT(0);
    setS(0);
    setX(0);
    setPC(0);
    setSW(0);
    setF(0.0);
}

void Machine::resetMemory() {
    std::fill(std::begin(memory), std::end(memory), 0);
}

int Machine::getReg(int reg)
{
    switch (reg) {
        case 0: return A;
        case 1: return X;
        case 2: return L;
        case 3: return B;
        case 4: return S;
        case 5: return T;
        case 8: return PC;
        case 9: return SW;
        default: return -1;
    }
}

void Machine::setReg(int reg, int val)
{
    switch (reg) {
        case 0: A = val; break;
        case 1: X = val; break;
        case 2: L = val; break;
        case 3: B = val; break;
        case 4: S = val; break;
        case 5: T = val; break;
        case 8: PC = val; break;
        case 9: SW = val; break;
    }
}

unsigned char Machine::readByte(unsigned int address) {
    if (address > MAX_ADDRESS || address <0) {
        throw std::out_of_range("Memory read out of range");
    }
    return memory[address];
}

void Machine::writeByte(unsigned int address, unsigned char val) {
    if (address > MAX_ADDRESS) {
        throw std::out_of_range("Memory write out of range");
    }
    memory[address] = val;
}

unsigned int Machine::getWord(unsigned int address) {
    if (address + 2 > MAX_ADDRESS) {
        throw std::out_of_range("Memory read out of range");
    }
    unsigned int B1 = memory[address + 2];
    unsigned int B2 = memory[address + 1];
    unsigned int B3 = memory[address];
    return B1 | (B2 << 8) | (B3 << 16);
}

void Machine::setWord(unsigned int address, unsigned int val) {
    if (address + 2 > MAX_ADDRESS) {
        throw std::out_of_range("Memory write out of range");
    }
    memory[address + 2] = val & 0xFF;
    memory[address + 1] = (val >> 8) & 0xFF;
    memory[address] = (val >> 16) & 0xFF;
}

Device* Machine::getDevice(int num) {
    if (num < 0 || num > 255) { return nullptr; }
    return devices[num];
}

void Machine::setDevice(int num, Device* device) {
    if (num < 0 || num > 255 ) { return; }
    delete devices[num];
    devices[num] = device;
}


void Machine::notImplemented(int opcode) {
    std::cerr << "Instruction: " << opcode << " is not yet implemented." << endl;
}

void Machine::invalidOpcode(int opcode) {
    std::cerr << "Invalid opcode: " << to_string(opcode) << endl;
}

void Machine::invalidAddressing() {
    std::cerr << "Invalid addressing used." << endl;
}

// Nalozi in vrni en bajt z naslova PC
// in ga poveca za 1
int Machine::fetch() {
    int address = getReg(8);
    setReg(8, address + 1);
    return readByte(address);
}

bool Machine::execF1(int opcode){
    switch (opcode) {
        case Opcode::FIX:
            setA(static_cast<int>(getF()));
            return true;
        case Opcode::FLOAT:
            setF(static_cast<double>(getA()));
            return true;
        default:
            notImplemented(opcode);
            break;
    }
    return false;
}

bool Machine::execF2(int opcode, int operand) {
    int r1 = (operand >> 4) & 0xF;
    int r2 = operand & 0xF;
    int n;
    switch (opcode) {
        case Opcode::ADDR:
            setReg(r2, getReg(r2) + getReg(r1));
            return true;
        case Opcode::CLEAR: //v clear je le 1 reg podan in sicer v r1
            setReg(r1, 0);
            return true;
        case Opcode::DIVR:
            setReg(r2, getReg(r2) / getReg(r1));
            return true;
        case Opcode::MULR:
            setReg(r2, getReg(r2) * getReg(r1));
            return true;
        case Opcode::RMO:
            setReg(r2, getReg(r1));
            return true;
        case Opcode::SHIFTL:
            n = r2;
            setReg(r1, getReg(r1) << n);
            return true;
        case Opcode::SHIFTR:
            n = r2;
            setReg(r1, getReg(r1) >> n);
            return true;
        case Opcode::SUBR:
            setReg(r2, getReg(r2) - getReg(r1));
            return true;
        default:
            notImplemented(opcode);
            break;
    }
    return false;
}

bool Machine::execSICF3F4(int opcode, int ni, int operand) {
    int x_val = 0;
    int operand2 = fetch();
    int UA = 0;
    int UV = 0;
    int temp = 0;
    bool ready = false;
    int offset = 0;
    if (((operand >> 7) & 0x1) == 1) { //preverimo ali je indeksno naslavljanje
        x_val = getX();
    }
    if (ni == 0) { // stari SIC (neposredno in enostavno naslavljanje)
        UA = (((operand & 0x7F) << 8) | operand2) + x_val; //izracun uporabnega naslova: operand brez bita x + drugi del naslova + x_val

        if (UA < 0 || UA + 2 > MAX_ADDRESS) {
            invalidAddressing();
            return false;
        }

        UV = (memory[UA] << 16) | (memory[UA + 1] << 8) | memory[UA + 2]; //izracunamo operand oz. uporabno vrednost
    } else if (((operand >> 4) & 0x1) == 0) { // ce e bit ni prizgan e je F3, sicer F4
            bool b = ((operand >> 6) & 0x1) == 1;  //preverimo bit za bazno naslavljanje
            bool p = ((operand >> 5) & 0x1) == 1;
            offset  = (((operand & 0x0F) << 8) | operand2);
            if (offset & 0x800) { // če je bit 11 = 1 (negativno)
                offset |= 0xFFFFF000; // nastavi vse višje bite na 1 (sign extend)
            }
            offset += x_val;
            if (!b && p) { //PC - relativno
                UA = getPC() + offset;
            } else if (b && !p) {
                UA = getB() + offset;
            } else if (!b && !p) { //neposredno
                UA = (((operand & 0x0F) << 8) | operand2);
            } else { // b && p je nedovoljeno!
                invalidAddressing();
            }

            if (UA < 0 || UA + 2 > MAX_ADDRESS) {
                invalidAddressing();
                return false;
            }

            if (ni == 2) { //posredno
                int UV_temp = (memory[UA] << 16) | (memory[UA + 1] << 8) | memory[UA + 2];
                UV =  (memory[UV_temp] << 16) | (memory[UV_temp + 1] << 8) | memory[UV_temp + 2];
            } else if (ni == 1) { //takojšnje
                UV = UA;
            } else if (ni == 3) { //enostavno
                UV = (memory[UA] << 16) | (memory[UA + 1] << 8) | memory[UA + 2]; //izracunamo operand oz. uporabno vrednost
            }
    } else {
        int operand3 = fetch();
        bool b = ((operand >> 6) & 0x1) == 1;  //preverimo bit za bazno naslavljanje
        bool p = ((operand >> 5) & 0x1) == 1;
        offset  = (((operand & 0x0F) << 16) | (operand2 << 8) | operand3);
        if (offset & 0x80000) { // če je bit 11 = 1 (negativno)
            offset |= 0xFFF00000; // nastavi vse višje bite na 1 (sign extend)
        }
        offset += x_val;
        if (!b && p) { //PC - relativno
            UA = getPC() + offset;
        } else if (b && !p) {
            UA = getB() + offset;
        } else if (!b && !p) { //neposredno
            UA = (((operand & 0x0F) << 16) | (operand2 << 8) | operand3);
        } else { // b && p je nedovoljeno!
            invalidAddressing();
        }

        if (UA < 0 || UA + 2 > MAX_ADDRESS) {
            invalidAddressing();
            return false;
        }

        if (ni == 2) { //posredno
            int UV_temp = (memory[UA] << 16) | (memory[UA + 1] << 8) | memory[UA + 2];
            UV =  (memory[UV_temp] << 16) | (memory[UV_temp + 1] << 8) | memory[UV_temp + 2];
        } else if (ni == 1) { //takojšnje
            UV = UA;
        } else if (ni == 3) { //enostavno
            UV = (memory[UA] << 16) | (memory[UA + 1] << 8) | memory[UA + 2]; //izracunamo operand oz. uporabno vrednost
        }
    }
    cout << "UA: " << UA << " UV: " << UV << endl;
    switch (opcode) {
        case Opcode::ADD:
            setA(getA() + UV);
            return true;
        case Opcode::AND:
            setA(getA() & UV);
            return true;
        case Opcode::COMP:
            temp = getA();
            if (temp < UV) {
                setSW(0x40);
            } else if (temp == UV) {
                setSW(0x0);
            } else {
                setSW(0x80);
            }
            return true;
            break;
        case Opcode::DIV:
            if (UV == 0) {
                cerr << "Error: Divison by zero." << endl;
                return false;
            }
            setA(getA() / UV);
            return true;
        case Opcode::J:
            setPC(UA);
            return true;
        case Opcode::JEQ:
            if (getSW() == 0x00) {
                setPC(UA);
            }
            return true;
        case Opcode::JGT:
            if (getSW() == 0x80) {
                setPC(UA);
            }
            return true;
        case Opcode::JLT:
            if (getSW() == 0x40) {
                setPC(UA);
            }
            return true;
        case Opcode::JSUB:
            setL(getPC());
            setPC(UA);
            return true;
        case Opcode::LDA:
            setA(UV);
            return true;
        case Opcode::LDB:
            setB(UV);
            return true;
        case Opcode::LDCH:
            setA((getA() & 0xFFFF00) | (UV & 0xFF));
            return true;
        case Opcode::LDL:
            setL(UV);
            return true;
        case Opcode::LDS:
            setS(UV);
            return true;
        case Opcode::LDT:
            setT(UV);
            return true;
        case Opcode::LDX:
            setX(UV);
            return true;
        case Opcode::MUL:
            setA(getA() * UV);
            return true;
        case Opcode::OR:
            setA(getA() | UV);
            return true;
        case Opcode::RD:
            temp = UV & 0xFF;
            if (devices[temp] != nullptr) {
                setA(devices[temp]->read());
            }
            return true;
        case Opcode::RSUB:
            setPC(getL());
            return true;
        case Opcode::STA:
            temp = getA();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STB:
            temp = getB();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STS:
            temp = getS();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STT:
            temp = getA();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STCH:
            temp = getA();
            memory[UV] = temp & 0xFF;
            return true;
        case Opcode::STL:
            temp = getL();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STSW:
            temp = getSW();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::STX:
            temp = getX();
            memory[UA] = (temp >> 16) & 0xFF;
            memory[UA + 1] = (temp >> 8) & 0xFF;
            memory[UA + 2] = temp & 0xFF;
            return true;
        case Opcode::SUB:
            setA(getA() - UV);
            return true;
        case Opcode::TD:
            temp = UV & 0xFF;

            ready = false;
            if (devices[temp] != nullptr) {
                ready = devices[temp]->test();
            }
            if (ready) {
                setSW(0X40);
            } else {
                setSW(0x00);
            }

            return true;
        case Opcode::TIX:
            temp = getX();
            setX(temp + 1);
            if (temp < UV) setSW(0x40);
            else if (temp == UV) setSW(0x00);
            else setSW(0x80);
            return true;
        case Opcode::WD:
            temp = UV & 0xFF;
            if (devices[temp] != nullptr) {
                devices[temp]->write(getA() & 0xFF);
            }
            return true;
        default:
            notImplemented(opcode);
            break;
    }
    return false;
}

void Machine::execute() {
    int opcode = fetch();
    int operand;
    switch (opcode) {
        // Format 1
        case Opcode::FIX:
        case Opcode::FLOAT:
        case Opcode::NORM:
        case Opcode::HIO:
        case Opcode::TIO:
        case Opcode::SIO:
            execF1(opcode);
            break;

        // Format 2
        case Opcode::ADDR:
        case Opcode::CLEAR:
        case Opcode::DIVR:
        case Opcode::MULR:
        case Opcode::RMO:
        case Opcode::SHIFTL:
        case Opcode::SHIFTR:
        case Opcode::SUBR:
        case Opcode::SVC:
            operand = fetch();
            execF2(opcode, operand);
            break;

        // Format SIC/3/4 (SIC/XE)
        default:
            operand = fetch();
            int ni = opcode & 0x3;
            opcode = opcode & 0xFC;
            execSICF3F4(opcode, ni, operand);
            break;
    }
}