#include "machine.h"

#include <memory>

#include "opcode.h"
#include "instructions.h"
#include <cmath>
#include <thread>

using std::make_shared;

string prefix = "Machine error: ";


Machine::Machine()
{
    devices.resize(NUM_DEVICES);
    // device 0: standard input
    devices[0] = make_shared<InputDevice>(std::cin);
    // device 1: standard output
    devices[1] = make_shared<OutputDevice>(std::cout);
    // device 2: standard error
    devices[2] = make_shared<OutputDevice>(std::cerr);
}

Machine::~Machine()
{
    for (auto& device : devices) {  
        device.reset();
    }
}

int Machine::getSpeed() const
{
    return speedkHz.load();
}

void Machine::setSpeed(int kHz)
{
    speedkHz.store(kHz);
}

// TODO: implement errors
void Machine::notImplemented(string mnemonic)
{
    cout << prefix << "Not implemented: " << mnemonic << endl;
}

void Machine::invalidOpcode(int opcode)
{
    cout << prefix << "Invalid opcode: " << opcode << endl;
}


void Machine::invalidAddressing()
{
    cout << prefix << "Invalid addressing mode" << endl;
}

void Machine::divisionByZero(int opcode)
{
    cout << prefix << "Division by zero error in opcode: " << opcode << endl;
}

void Machine::undefinedHandler(int opcode)
{
    cout << prefix << "Undefined handler for opcode: " << opcode << endl;
}

void Machine::tick()
{
    const int speed = speedkHz.load();
    if (speed <= 0) throw std::runtime_error("Invalid speed setting in Machine::tick"); 
    
    const auto delay = std::chrono::microseconds(1000 / speed);
    std::this_thread::sleep_for(delay);
}

int Machine::getReg(int regNum) const
{
    switch (regNum) {
        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 F;
        case 8: return PC;
        case 9: return SW;
        default:
            cerr << prefix << "Invalid register number: " << regNum << endl;
            return -1;
    }
}

// TODO: handle double for F register
void Machine::setReg(int regNum, int value)
{
    value = toSIC24(value);
    switch (regNum) {
        case 0: A = value; break;
        case 1: X = value; break;
        case 2: L = value; break;
        case 3: B = value; break;
        case 4: S = value; break;
        case 5: T = value; break;
        case 6: F = value; break;
        case 8: PC = value; break;
        case 9: SW = value; break;
        default:
            cerr << prefix << "Invalid register number: " << regNum << endl;
            break;
    }
}

int Machine::getByte(int address)
{
    if (address < 0 || address >= MEMORY_SIZE) {
        cerr << prefix << "Invalid memory address: " << address << endl;
        return -1;
    }
    return static_cast<int>(memory[address]);
}

void Machine::setByte(int address, int value)
{
    if(address < 0 || address >= MEMORY_SIZE) {
        cerr << prefix << "Invalid memory address: " << address << endl;
        return;
    }

    memory[address] = static_cast<unsigned char>(value);
}

// Assuming word is 3 bytes

int Machine::getWord(int address)
{
    if (address < 0 || address + 2 >= MEMORY_SIZE) {
        cerr << prefix << "Invalid memory address: " << address << endl;
        return -1;
    }
    return static_cast<int>(memory[address]) | (static_cast<int>(memory[address + 1]) << 8) | (static_cast<int>(memory[address + 2]) << 16);
}

// Assuming word is 3 bytes
void Machine::setWord(int address, int value)
{
    if(address < 0 || address + 2 >= MEMORY_SIZE) {
        cerr << prefix << "Invalid memory address: " << address << endl;
        return;
    }
    value &= 0xFFFFFF;

    memory[address] = static_cast<unsigned char>(value & 0xFF);
    memory[address + 1] = static_cast<unsigned char>((value >> 8) & 0xFF);
    memory[address + 2] = static_cast<unsigned char>((value >> 16) & 0xFF);
}

double Machine::getFloat(int address)
{
    if (address < 0 || address + 5 >= MEMORY_SIZE) {
        cerr << prefix << "Invalid float address: " << address << endl;
        return 0.0;
    }

    // load 6 bytes, little-endian → 48-bit word
    unsigned long long raw =
        (unsigned long long)memory[address]       |
        ((unsigned long long)memory[address+1] << 8)  |
        ((unsigned long long)memory[address+2] << 16) |
        ((unsigned long long)memory[address+3] << 24) |
        ((unsigned long long)memory[address+4] << 32) |
        ((unsigned long long)memory[address+5] << 40);

    int sign     = (raw >> 47) & 0x1;
    int exponent = (raw >> 40) & 0x7F;
    unsigned long long frac = raw & SICF_FRAC_MASK;   // 40 bits

    if (raw == 0) return 0.0;

    // value = (1 + frac/2^40) * 2^(exp - 64)
    double mant = 1.0 + (double)frac / (double)(1ULL << SICF_FRAC_BITS);
    int e = exponent - SICF_EXP_BIAS;
    double val = std::ldexp(mant, e);   // ldexp is fast enough here
    return sign ? -val : val;
}

void Machine::setFloat(int address, double value)
{
    if (address < 0 || address + 5 >= MEMORY_SIZE) {
        cerr << prefix << "Invalid float address: " << address << endl;
        return;
    }

    if (value == 0.0) {
        memory[address]   = 0;
        memory[address+1] = 0;
        memory[address+2] = 0;
        memory[address+3] = 0;
        memory[address+4] = 0;
        memory[address+5] = 0;
        return;
    }

    int sign = value < 0;
    double x = sign ? -value : value;

    // normalize x to [1, 2)
    int exp2 = 0;
    x = std::frexp(x, &exp2);     
    x *= 2.0;                     
    exp2 -= 1;                    

    int exp_field = exp2 + SICF_EXP_BIAS;
    if (exp_field < 0)   exp_field = 0;
    if (exp_field > 127) exp_field = 127;

    // mantissa = (x - 1) * 2^40
    double frac_d = (x - 1.0) * (double)(1ULL << SICF_FRAC_BITS);
    unsigned long long frac = (unsigned long long)(frac_d + 0.5); // round
    frac &= SICF_FRAC_MASK;

    unsigned long long raw =
        ((unsigned long long)sign     << 47) |
        ((unsigned long long)exp_field << 40) |
        frac;

    // store 6 bytes little-endian
    memory[address]   = (unsigned char)( raw        & 0xFF);
    memory[address+1] = (unsigned char)((raw >> 8)  & 0xFF);
    memory[address+2] = (unsigned char)((raw >> 16) & 0xFF);
    memory[address+3] = (unsigned char)((raw >> 24) & 0xFF);
    memory[address+4] = (unsigned char)((raw >> 32) & 0xFF);
    memory[address+5] = (unsigned char)((raw >> 40) & 0xFF);
}



Device &Machine::getDevice(int num)
{
    if(num < 0 || num >= static_cast<int>(devices.size()) || !devices[num]) {
        cerr << prefix << "Invalid device number: " << num << endl;
        return fallbackDevice;
    }
    return *devices[num];
}

void Machine::setDevice(int num, std::shared_ptr<Device> device)
{
    if(num < 0 || num >= NUM_DEVICES) {
        cerr << prefix << "Invalid device number: " << num << endl;
        return;
    }
    if(static_cast<int>(devices.size()) != NUM_DEVICES) {
        devices.resize(NUM_DEVICES);
    }
    // Enforce: devices with index >= 2 must be FileDevice instances
    if (num >= 2) {
        // try dynamic cast
        if (std::dynamic_pointer_cast<FileDevice>(device) == nullptr) {
            cerr << prefix << "Device at index " << num << " must be a FileDevice." << endl;
            return;
        }
    }
    devices[num] = device;
}

void Machine::setFileDevice(int num, const std::string &filename)
{
    if(num < 0 || num >= NUM_DEVICES) {
        cerr << prefix << "Invalid device number: " << num << endl;
        return;
    }
    if(static_cast<int>(devices.size()) != NUM_DEVICES) {
        devices.resize(NUM_DEVICES);
    }
    try {
        devices[num] = std::make_shared<FileDevice>(filename);
    } catch (const std::exception &e) {
        cerr << prefix << "Failed to create FileDevice for index " << num << ": " << e.what() << endl;
    }
}

int Machine::fetch()
{
    return getByte(PC++);
}

void Machine::execute() {
    int b1 = fetch();
    InstructionInfo &info = instructions[b1];

    if (info.type == InstructionType::TYPE1) { execF1(b1); return; }
    if (info.type == InstructionType::TYPE2) { execF2(b1, fetch()); return; }

    int opcode = b1 & TYPE3_4_SIC_MASK;
    InstructionInfo &info34 = instructions[opcode];
    int ni = b1 & NI_MASK;

    if (info34.type == InstructionType::TYPE3_4) {
        int b2 = fetch(), b3 = fetch();
        int x = (b2 & 0x80) ? 1 : 0;
        int b = (b2 & 0x40) ? 1 : 0;
        int p = (b2 & 0x20) ? 1 : 0;
        int e = (b2 & 0x10) ? 1 : 0;

        int operand;
        if (ni == NI_SIC) {
            // PURE SIC
            operand = ((b2 & 0x7F) << 8) | b3;
        } else {
            // SIC/XE
            operand = e
                ? (((b2 & 0x0F) << 16) | (b3 << 8) | fetch())   // F4: 20-bit
                : (((b2 & 0x0F) << 8)  |  b3);                  // F3: 12-bit
        }

        execSICF3F4(opcode, ni, x, b, p, e, operand);
        return;
    }

    invalidOpcode(b1);
}
  



bool Machine::execF1(int opcode)
{
    if (instructions[opcode].handler) {
        auto handler = reinterpret_cast<void(*)(Machine&)>(instructions[opcode].handler);
        handler(*this);
        return true;
    }
    undefinedHandler(opcode);
    return false;
}

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

    if (instructions[opcode].handler) {
        auto handler = reinterpret_cast<void(*)(Machine&, int, int)>(instructions[opcode].handler);
        handler(*this, r1, r2);
        return true;
    }
    undefinedHandler(opcode);
    return false;
}


bool Machine::execSICF3F4(int opcode, int ni, int x, int b, int p, int e, int operand)
{
    int ea_part = operand;
    int base    = 0;
    AddressingMode mode = getAddressingMode(ni);

    // --- PURE SIC ---
    if (mode == AddressingMode::SIC_DIRECT) {
        int ea = ea_part + (x ? getX() : 0);
        if (instructions[opcode].handler) {
            auto h = reinterpret_cast<void(*)(Machine&, int, AddressingMode)>(instructions[opcode].handler);
            h(*this, ea, mode);
            return true;
        }
        undefinedHandler(opcode);
        return false;
    }

    // --- SIC/XE EA calc ---

    if (!e) {                               // format 3
        if (b && !p) {
            base = getB();                  // base-relative, unsigned 12-bit
        } else if (p && !b) {
            // PC-relative, signed 12-bit
            if (ea_part & 0x800)            // bit 11 set?
                ea_part |= 0xFFFFF000;      // sign-extend
            base = getPC();
        }
    }
    // format 4 (e=1): b/p ignored, ea_part is 20-bit absolute
    int ea = base + ea_part + (x ? getX() : 0);

    if (instructions[opcode].handler) {
        auto h = reinterpret_cast<void(*)(Machine&, int, AddressingMode)>(instructions[opcode].handler);
        h(*this, ea, mode);
        return true;
    }

    undefinedHandler(opcode);
    return false;
}

void Machine::start()
{
    running.store(true);

    // Main execution loop
    // TODO: consider running in separate thread
    while (running.load()) {
        execute();
        tick();
    }
}

void Machine::stop()
{
    running.store(false);
}