#include "Machine.h"

#include <chrono>
#include <cstring>
#include <iostream>

#include "FileDevice.h"
#include "InputDevice.h"
#include "Opcode.h"
#include "OutputDevice.h"

Machine::Machine() {
    A = 0;
    X = 0;
    L = 0;
    B = 0;
    S = 0;
    T = 0;
    F = 0.0;
    PC = 0;
    SW = 0;

    running = false;
    timerThread = nullptr;
    speed = 1000;  // default 1000 kHz = 1 MHz

    for (int i = 0; i <= MAX_ADDRESS; i++) {
        memory[i] = 0;
    }

    for (int i = 0; i < MAX_DEVICES; i++) {
        devices[i] = nullptr;
    }

    devices[0] = new InputDevice(std::cin);
    devices[1] = new OutputDevice(std::cout);
    devices[2] = new OutputDevice(std::cerr);
}

Machine::~Machine() {
    stop();
    
    for (int i = 0; i < MAX_DEVICES; i++) {
        if (devices[i] != nullptr) {
            delete devices[i];
            devices[i] = nullptr;
        }
    }
}

int Machine::getA() const {
    return A;
}

int Machine::getX() const {
    return X;
}

int Machine::getL() const {
    return L;
}

int Machine::getB() const {
    return B;
}

int Machine::getS() const {
    return S;
}

int Machine::getT() const {
    return T;
}

double Machine::getF() const {
    return F;
}

int Machine::getPC() const {
    return PC;
}

int Machine::getSW() const {
    return SW;
}

// mask to 24 bits

void Machine::setA(int val) {
    A = val & 0xFFFFFF;
}

void Machine::setX(int val) {
    X = val & 0xFFFFFF;
}

void Machine::setL(int val) {
    L = val & 0xFFFFFF;
}

void Machine::setB(int val) {
    B = val & 0xFFFFFF;
}

void Machine::setS(int val) {
    S = val & 0xFFFFFF;
}

void Machine::setT(int val) {
    T = val & 0xFFFFFF;
}

void Machine::setF(double val) {
    F = val;
}

void Machine::setPC(int val) {
    PC = val & 0xFFFFFF;
}

void Machine::setSW(int val) {
    SW = val & 0xFFFFFF;
}

void Machine::setCC_less() {
    SW = (SW & 0xFFFF3F) | 0x0;
}

void Machine::setCC_equal() {
    SW = (SW & 0xFFFF3F) | 0x40;
}

void Machine::setCC_greater() {
    SW = (SW & 0xFFFF3F) | 0x80;
}

int Machine::getCC() const {
    return SW & 0xC0;
}

int Machine::getReg(int reg) const {
    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 6:
            return (int)F;
        case 8:
            return PC;
        case 9:
            return SW;
        default:
            return 0;
    }
}

void Machine::setReg(int reg, int val) {
    switch (reg) {
        case 0:
            setA(val);
            break;
        case 1:
            setX(val);
            break;
        case 2:
            setL(val);
            break;
        case 3:
            setB(val);
            break;
        case 4:
            setS(val);
            break;
        case 5:
            setT(val);
            break;
        case 6:
            setF((double)val);
            break;
        case 8:
            setPC(val);
            break;
        case 9:
            setSW(val);
            break;
        default:
            break;
    }
}

int Machine::getByte(int addr) const {
    if (addr < 0 || addr > MAX_ADDRESS) {
        return 0;
    }
    return memory[addr];
}

void Machine::setByte(int addr, int val) {
    if (addr >= 0 && addr <= MAX_ADDRESS) {
        memory[addr] = val & 0xFF;
    }
}

int Machine::getWord(int addr) const {
    if (addr < 0 || addr + 2 > MAX_ADDRESS) {
        return 0;
    }
    // big-endian
    return (memory[addr] << 16) | (memory[addr + 1] << 8) | memory[addr + 2];
}

void Machine::setWord(int addr, int val) {
    if (addr >= 0 && addr + 2 <= MAX_ADDRESS) {
        val = val & 0xFFFFFF;
        // big-endian
        memory[addr] = (val >> 16) & 0xFF;
        memory[addr + 1] = (val >> 8) & 0xFF;
        memory[addr + 2] = val & 0xFF;
    }
}

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

void Machine::setDevice(int num, Device* device) {
    if (num >= 0 && num < MAX_DEVICES) {
        devices[num] = device;
    }
}

void Machine::notImplemented(const char* mnemonic) {
    std::cerr << "Error: instruction " << mnemonic << " not implemented" << std::endl;
}

void Machine::invalidOpcode(int opcode) {
    std::cerr << "Error: invalid opcode 0x" << std::hex << opcode << std::endl;
}

void Machine::invalidAddressing() {
    std::cerr << "Error: invalid addressing mode" << std::endl;
}

int Machine::fetch() {
    int byte = getByte(PC);
    PC = (PC + 1) & 0xFFFFFF;
    return byte;
}

void Machine::execute() {
    int byte1 = fetch();
    int opcode = byte1 & 0xFC;
    int ni = byte1 & 0x03;

    // format 1 (no operand)
    if (execF1(opcode)) {
        return;
    }

    int byte2 = fetch();

    // format 2 (register operand)
    if (execF2(opcode, byte2)) {
        return;
    }

    // format 3 or 4
    int xbpe = (byte2 >> 4) & 0x0F;
    int disp = byte2 & 0x0F;

    int byte3 = fetch();
    disp = (disp << 8) | byte3;

    // format 4 (extended)
    if (xbpe & 0x01) {
        int byte4 = fetch();
        disp = (disp << 8) | byte4;
    }

    int addr = disp;

    // base relative
    if (xbpe & 0x04) {
        addr = B + disp;
    }
    // pc relative
    else if (xbpe & 0x02) {
        // sign extend 12-bit displacement for format 3
        if (!(xbpe & 0x01) && (disp & 0x800)) {
            disp = disp | 0xFFFFF000;
        }
        addr = PC + disp;
    }

    // indexed
    if (xbpe & 0x08) {
        addr = addr + X;
    }

    addr = addr & 0xFFFFFF;

    execSICF3F4(opcode, ni, addr);
}

bool Machine::execF1(int opcode) {
    switch (opcode) {
        case Opcode::FIX:
            A = (int)F;
            return true;
        case Opcode::FLOAT:
            F = (double)A;
            return true;
        case Opcode::NORM:
            notImplemented("NORM");
            return true;
        case Opcode::SIO:
        case Opcode::HIO:
        case Opcode::TIO:
            notImplemented("SIO/HIO/TIO");
            return true;
    }
    return false;
}

bool Machine::execF2(int opcode, int operand) {
    int r1 = (operand >> 4) & 0x0F;
    int r2 = operand & 0x0F;

    switch (opcode) {
        case Opcode::ADDR:
            setReg(r2, getReg(r1) + getReg(r2));
            return true;
        case Opcode::SUBR:
            setReg(r2, getReg(r2) - getReg(r1));
            return true;
        case Opcode::MULR:
            setReg(r2, getReg(r1) * getReg(r2));
            return true;
        case Opcode::DIVR:
            setReg(r2, getReg(r2) / getReg(r1));
            return true;
        case Opcode::COMPR: {
            int val1 = getReg(r1);
            int val2 = getReg(r2);
            if (val1 < val2)
                setCC_less();
            else if (val1 == val2)
                setCC_equal();
            else
                setCC_greater();
            return true;
        }
        case Opcode::SHIFTL:
            setReg(r1, getReg(r1) << (r2 + 1));
            return true;
        case Opcode::SHIFTR:
            setReg(r1, getReg(r1) >> (r2 + 1));
            return true;
        case Opcode::RMO:
            setReg(r2, getReg(r1));
            return true;
        case Opcode::CLEAR:
            setReg(r1, 0);
            return true;
        case Opcode::TIXR: {
            X = X + 1;
            int val = getReg(r1);
            if (X < val)
                setCC_less();
            else if (X == val)
                setCC_equal();
            else
                setCC_greater();
            return true;
        }
        case Opcode::SVC:
            notImplemented("SVC");
            return true;
    }
    return false;
}

bool Machine::execSICF3F4(int opcode, int ni, int operand) {
    int addr = operand;
    int value;

    // get value based on addressing mode
    if (ni == 0x00) {
        // SIC format (simple)
        value = getWord(addr);
    } else if (ni == 0x01) {
        // immediate
        value = addr;
    } else if (ni == 0x02) {
        // indirect
        addr = getWord(addr);
        value = getWord(addr);
    } else {
        // simple (ni == 0x03)
        value = getWord(addr);
    }

    switch (opcode) {
        case Opcode::LDA:
            A = value;
            return true;
        case Opcode::LDX:
            X = value;
            return true;
        case Opcode::LDL:
            L = value;
            return true;
        case Opcode::LDB:
            B = value;
            return true;
        case Opcode::LDS:
            S = value;
            return true;
        case Opcode::LDT:
            T = value;
            return true;
        case Opcode::LDCH:
            if (ni == 0x01) {
                A = (A & 0xFFFF00) | (addr & 0xFF);
            } else {
                A = (A & 0xFFFF00) | getByte(addr);
            }
            return true;

        case Opcode::STA:
            setWord(addr, A);
            return true;
        case Opcode::STX:
            setWord(addr, X);
            return true;
        case Opcode::STL:
            setWord(addr, L);
            return true;
        case Opcode::STB:
            setWord(addr, B);
            return true;
        case Opcode::STS:
            setWord(addr, S);
            return true;
        case Opcode::STT:
            setWord(addr, T);
            return true;
        case Opcode::STSW:
            setWord(addr, SW);
            return true;
        case Opcode::STCH:
            setByte(addr, A & 0xFF);
            return true;

        case Opcode::ADD:
            A = A + value;
            return true;
        case Opcode::SUB:
            A = A - value;
            return true;
        case Opcode::MUL:
            A = A * value;
            return true;
        case Opcode::DIV:
            A = A / value;
            return true;
        case Opcode::AND:
            A = A & value;
            return true;
        case Opcode::OR:
            A = A | value;
            return true;

        case Opcode::COMP:
            if (A < value)
                setCC_less();
            else if (A == value)
                setCC_equal();
            else
                setCC_greater();
            return true;
        case Opcode::TIX:
            X = X + 1;
            if (X < value)
                setCC_less();
            else if (X == value)
                setCC_equal();
            else
                setCC_greater();
            return true;

        case Opcode::J:
            PC = addr;
            return true;
        case Opcode::JEQ:
            if (getCC() == 0x40) PC = addr;
            return true;
        case Opcode::JGT:
            if (getCC() == 0x80) PC = addr;
            return true;
        case Opcode::JLT:
            if (getCC() == 0x00) PC = addr;
            return true;
        case Opcode::JSUB:
            L = PC;
            PC = addr;
            return true;
        case Opcode::RSUB:
            PC = L;
            return true;

        case Opcode::TD: {
            Device* dev = getDevice(getByte(addr));
            if (dev != nullptr && dev->test()) {
                setCC_less();
            } else {
                setCC_equal();
            }
            return true;
        }
        case Opcode::RD: {
            Device* dev = getDevice(getByte(addr));
            if (dev != nullptr) {
                A = (A & 0xFFFF00) | dev->read();
            }
            return true;
        }
        case Opcode::WD: {
            Device* dev = getDevice(getByte(addr));
            if (dev != nullptr) {
                dev->write(A & 0xFF);
            }
            return true;
        }

        default:
            invalidOpcode(opcode);
            return false;
    }
}

void Machine::start() {
    if (running) return;
    
    running = true;
    timerThread = new std::thread([this]() {
        while (running) {
            int instructionsPerTick = speed / 1000;
            if (instructionsPerTick < 1) instructionsPerTick = 1;
            
            for (int i = 0; i < instructionsPerTick && running; i++) {
                execute();
            }
            
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    });
}

void Machine::stop() {
    running = false;
    if (timerThread != nullptr) {
        timerThread->join();
        delete timerThread;
        timerThread = nullptr;
    }
}

bool Machine::isRunning() const {
    return running;
}

int Machine::getSpeed() const {
    return speed;
}

void Machine::setSpeed(int kHz) {
    speed = kHz;
}