#include <fmt/format.h>
#include <iostream>

#include "uarch/core.h"

fetch_stage::fetch_stage(core &c)
    : c(c)
    , pc(c.checker.pc)
{ }

decode_stage::decode_stage(core &c)
    : c(c)
    , pc(c.checker.pc)
    , icount(c.checker.icount)
{ }

indir_stage::indir_stage(core &c)
    : c(c)
{ }

exec_stage::exec_stage(core &c)
    : c(c)
    , acc(c.checker.acc)
    , link(c.checker.link)
    , mq(c.checker.mq)
    , pc(c.checker.pc)
    , ctlregs(c.checker.ctlregs)
{ }

void fetch_stage::clock() {
    if (c.restarto.has_value()) {
        auto &r = *c.restarto;
        pte(r.tr, "!", fmt::format("newpc={:05o}", r.new_pc));
        pc = r.new_pc;
        didrestart = true;
        outstandingfill = false;
    }

    if (c.fetch_mem_responsep.can_read()) {
        auto r = c.fetch_mem_responsep.read();
        c.icache.handle_response(r);
    }

    if (c.fetch_bundlep.can_write()) {
        fetch_bundle b;
        if (auto t = c.icache.fetchline(b.data, pc); t.has_value()) {
            b.tr = infra::pt::toplevel();
            b.gen = c.gen;
            b.pc = pc;
            if (didrestart)
                infra::pt::event(b.tr, ">", now-1, "");
            pte(b.tr, "F");
            c.fetch_bundlep.write(std::move(b));
            pc = (pc & 070000) | (((pc & ~memory::LINE_BYTE_OFFSET_MASK) + memory::LINE_BYTES) & 007777);
            didrestart = false;
            outstandingfill = false;
        }
    }

    if (!outstandingfill && c.fetch_mem_commandp.can_write() && !c.icache.probe(pc)) {
        memory::dram::command fr;
        fr.transaction = infra::pt::toplevel();
        pte(fr.transaction, "p", fmt::format("fpc={:05o}", pc));
        fr.line_address = pc >> memory::LINE_BYTES_LOG2;
        fr.responsep = &c.fetch_mem_responsep;
        c.fetch_mem_commandp.write(std::move(fr));
        outstandingfill = true;
    }
}

void decode_stage::clock() {
    if (c.restarto.has_value()) {
        auto &r = *c.restarto;
        pc = r.new_pc;
        interrupt |= r.interrupt;
        icount = c.icount;
        speculative_stores_sent = r.stores_sent;
    }

    if (c.fetch_bundlep.can_read() && c.indir_instp.can_write() && c.decode_to_exec_instp.can_write()) {
        auto b = c.fetch_bundlep.peek();

        if (b.gen != c.gen) {
            pte(b.tr, "-");
            c.fetch_bundlep.discard();
            return;
        }

        if ((b.pc >> memory::LINE_BYTES_LOG2) != (pc >> memory::LINE_BYTES_LOG2)) {
            if (!c.restarto.has_value()) {
                restart r;
                r.tr = b.tr;
                r.new_pc = pc;
                r.interrupt = false;
                ++c.gen;
                c.restarto = std::move(r);
            }
            pte(b.tr, "-");
            c.fetch_bundlep.discard();
            return;
        }

        inst_bundle i;

        i.inst = decode(c.exec.ctlregs[FLAGS],
                        pc,
                        b.data[pc & memory::LINE_BYTE_OFFSET_MASK],
                        interrupt);

        if ((i.inst.need_indirect_load || i.inst.need_exec_load) && speculative_stores_sent != c.stores_done) {
            pte(b.tr, "z");
            return;
        }

        // Commit point for decode

        interrupt = false;

        i.gen = c.gen;
        i.pc = pc;
        i.icount = icount++;
        i.tr = infra::pt::child(b.tr);

        pte(i.tr, "D", fmt::format("{:05o}: {}", pc, i.inst.disasm()));

        pc = i.inst.next_pc;

        speculative_stores_sent += i.inst.need_autoinc_store;
        speculative_stores_sent += i.inst.need_exec_store;

        if (i.inst.need_indirect_load) {
#if 0
            memory::dram::command fr;
            fr.transaction = i.tr;
            fr.line_address = *i.inst.init_address >> memory::LINE_BYTES_LOG2;
            fr.responsep = &c.indir_mem_responsep;
            pte(i.tr, "", fmt::format("iload={:05o}", *i.inst.init_address));
            c.decode_mem_commandp.write(std::move(fr));
#endif
            c.indir_instp.write(std::move(i));
        } else {
#if 0
            if (i.inst.need_exec_load) {
                memory::dram::command fr;
                fr.transaction = i.tr;
                fr.line_address = *i.inst.final_address >> memory::LINE_BYTES_LOG2;
                fr.responsep = &c.exec_mem_responsep;
                pte(i.tr, "", fmt::format("load={:05o}", *i.inst.final_address));
                c.decode_mem_commandp.write(std::move(fr));
            }
#endif
            c.decode_to_exec_instp.write(std::move(i));
        }

        if ((b.pc >> memory::LINE_BYTES_LOG2) != (pc >> memory::LINE_BYTES_LOG2)) {
            pte(b.tr, "-");
            c.fetch_bundlep.discard();
        }
    }
}

void indir_stage::clock() {
    if (c.restarto.has_value()) {
        gen = c.gen;
    }

    if (c.indir_mem_responsep.can_read()) {
        auto r = c.indir_mem_responsep.read();
        c.dcache.handle_response(r);
    }

    if (c.indir_instp.can_read() && c.indir_to_exec_instp.can_write()) {
        auto &i = c.indir_instp.peek();

        if (i.gen != gen && i.gen != c.gen) {
            pte(i.tr, "~");
            c.indir_instp.discard();
            return;
        } else if (i.gen != gen) {
            gen = c.gen;
            assert(i.gen == gen);
        }

        if (i.inst.need_indirect_load) {
            unsigned int addr;
            auto t = c.dcache.fetch(addr, i.inst.init_address.value());
            if (!t.has_value()) {
                if (c.indir_mem_load_commandp.can_write()) {
                    memory::dram::command fr;
                    fr.transaction = i.tr;
                    pte(fr.transaction, "1", fmt::format("indir load fill {:05o}", i.inst.init_address.value()));
                    fr.line_address = i.inst.init_address.value() >> memory::LINE_BYTES_LOG2;
                    fr.responsep = &c.indir_mem_responsep;
                    c.indir_mem_load_commandp.write(std::move(fr));
                }
                return;
            }
            if (i.inst.need_autoinc_store) {
                if (!c.indir_mem_store_commandp.can_write())
                    return;
                addr = (addr + 1) & 07777;
                pte(i.tr, "+", fmt::format("istore={:05o} istoredata={:04o}", *i.inst.init_address, addr));
                memory::dram::command sr;
                sr.transaction = i.tr;
                sr.line_address = *i.inst.init_address >> memory::LINE_BYTES_LOG2;
                sr.data[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = addr;
                sr.mask.fill(false);
                sr.mask[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = true;
                sr.write = true;
                c.indir_mem_store_commandp.write(std::move(sr));
                c.dcache.opportunistic_store(*i.inst.init_address, addr);
                c.icache.opportunistic_store(*i.inst.init_address, addr);
            } else {
                pte(i.tr, "I");
            }
            auto df = (c.exec.ctlregs[FLAGS] & FLAG_DF) >> FLAG_DF_SHIFT;
            i.inst.final_address = (unsigned int)((df << 12) | addr);
        }

#if 0
        if (i.inst.need_exec_load) {
            memory::dram::command fr;
            fr.transaction = i.tr;
            fr.line_address = *i.inst.final_address >> memory::LINE_BYTES_LOG2;
            fr.responsep = &c.exec_mem_responsep;
            pte(i.tr, "", fmt::format("load={:05o}", *i.inst.final_address));
            c.indir_mem_load_commandp.write(std::move(fr));
        }
#endif

        c.indir_to_exec_instp.write(std::move(i));

        c.indir_instp.discard();
    }
}

void exec_stage::clock() {
    c.restarto.reset();

    if (c.exec_mem_responsep.can_read()) {
        auto r = c.exec_mem_responsep.read();
        c.dcache.handle_response(r);
    }

    std::optional<infra::transaction> restarttr;
    std::optional<instruction_context> insto;

    bool progress = ctlregs[HALTED];

    if (!ctlregs[HALTED] && (c.decode_to_exec_instp.can_read() || c.indir_to_exec_instp.can_read())) {
        infra::port<inst_bundle> *instp = nullptr;
        if (c.decode_to_exec_instp.can_read()) {
            auto &i = c.decode_to_exec_instp.peek();
            if (i.gen != gen && i.gen != c.gen) {
                if (i.inst.need_autoinc_store) {
                    if (!c.exec_mem_store_commandp.can_write())
                        return;
                    auto addr = (*i.inst.final_address - 1) & 07777;
                    pte(i.tr, "U", fmt::format("unstore={:05o}, unstoredata={:04o}", *i.inst.init_address, addr));
                    memory::dram::command sr;
                    sr.transaction = i.tr;
                    sr.line_address = *i.inst.init_address >> memory::LINE_BYTES_LOG2;
                    sr.data[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = addr;
                    sr.mask.fill(false);
                    sr.mask[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = true;
                    sr.write = true;
                    c.exec_mem_store_commandp.write(std::move(sr));
                    stores_sent += 2; // Original store sent by Indir stage plus unstore here
                    c.stores_done += 2;
                    c.dcache.opportunistic_store(*i.inst.init_address, addr);
                    c.icache.opportunistic_store(*i.inst.init_address, addr);
                }
                pte(i.tr, "~");
                c.decode_to_exec_instp.discard();
            } else if (i.icount == c.icount) {
                instp = &c.decode_to_exec_instp;
            }
        }
        if (c.indir_to_exec_instp.can_read()) {
            auto &i = c.indir_to_exec_instp.peek();
            if (i.gen != gen && i.gen != c.gen) {
                if (i.inst.need_autoinc_store) {
                    if (!c.exec_mem_store_commandp.can_write())
                        return;
                    auto addr = (*i.inst.final_address - 1) & 07777;
                    pte(i.tr, "U", fmt::format("unstore={:05o}, unstoredata={:04o}", *i.inst.init_address, addr));
                    memory::dram::command sr;
                    sr.transaction = i.tr;
                    sr.line_address = *i.inst.init_address >> memory::LINE_BYTES_LOG2;
                    sr.data[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = addr;
                    sr.mask.fill(false);
                    sr.mask[*i.inst.init_address & memory::LINE_BYTE_OFFSET_MASK] = true;
                    sr.write = true;
                    c.exec_mem_store_commandp.write(std::move(sr));
                    stores_sent += 2; // Original store sent by Indir stage plus unstore here
                    c.stores_done += 2;
                    c.dcache.opportunistic_store(*i.inst.init_address, addr);
                    c.icache.opportunistic_store(*i.inst.init_address, addr);
                }
                pte(i.tr, "~");
                c.indir_to_exec_instp.discard();
            } else if (i.icount == c.icount) {
                instp = &c.indir_to_exec_instp;
            }
        }
        if (!instp)
            goto bail_out;
        auto &i = instp->peek();

        assert(i.gen == gen || i.gen == c.gen);
        if (i.gen != gen) {
            gen = c.gen;
            assert(i.gen == gen);
        }

        if (i.inst.need_exec_load) {
            unsigned int data;
            auto t = c.dcache.fetch(data, i.inst.final_address.value());
            if (t.has_value()) {
                i.inst.data = data;
            } else {
                if (c.exec_mem_load_commandp.can_write()) {
                    memory::dram::command fr;
                    fr.transaction = i.tr;
                    pte(fr.transaction, "2", fmt::format("exec load fill {:05o}", i.inst.final_address.value()));
                    fr.line_address = i.inst.final_address.value() >> memory::LINE_BYTES_LOG2;
                    fr.responsep = &c.exec_mem_responsep;
                    c.exec_mem_load_commandp.write(std::move(fr));
                }
                return;
            }
        }

        if (i.inst.need_exec_store && !c.exec_mem_store_commandp.can_write())
            return;

        // We are committed to complete execution of this instruction this cycle.

        insto = std::move(i.inst);
        auto &inst = *insto;

        pte(i.tr, "E");
        progress = true;

        assert(i.pc == pc);

        auto next_pc = inst.next_pc;

        if (inst.need_exec_load)
            pte(i.tr, "", fmt::format("loaddata={:04o}", *i.inst.data));

        if (inst.need_autoinc_store) {
            ++stores_sent; // It was sent by Indir stage
            ++c.stores_done;
        }
        if (inst.need_read_acc)
            inst.acc = acc;
        if (inst.need_read_link)
            inst.link = link;
        if (inst.need_read_mq)
            inst.mq = mq;
        if (inst.read_ctlreg.has_value())
            inst.ctlval = ctlregs[*inst.read_ctlreg];

        inst.execute();

        if (inst.need_write_acc)
            acc = inst.acc.value();
        if (inst.need_write_link)
            link = inst.link.value();
        if (inst.need_write_mq)
            mq = inst.mq.value();
        if (inst.write_ctlreg.has_value()) {
            ctlregs[*inst.write_ctlreg] = inst.ctlval.value();
            restarttr = i.tr;
        }

        if (inst.need_exec_store) {
            pte(i.tr, "", fmt::format("store={:05o} storedata={:04o}", *inst.final_address, *inst.data));
            memory::dram::command sr;
            sr.transaction = i.tr;
            sr.line_address = *inst.final_address >> memory::LINE_BYTES_LOG2;
            sr.data[*inst.final_address & memory::LINE_BYTE_OFFSET_MASK] = *inst.data;
            sr.mask.fill(false);
            sr.mask[*inst.final_address & memory::LINE_BYTE_OFFSET_MASK] = true;
            sr.write = true;
            c.exec_mem_store_commandp.write(std::move(sr));
            ++stores_sent;
            ++c.stores_done;
            c.dcache.opportunistic_store(*inst.final_address, *inst.data);
            c.icache.opportunistic_store(*inst.final_address, *inst.data);
        }

        assert(inst.next_pc == next_pc || inst.possibly_redirects);
        pc = inst.next_pc;

        if (pc != next_pc) {
            pte(i.tr, "", fmt::format("jump={:05o}", pc));
            restarttr = i.tr;
        }

        instp->discard();
    }

bail_out:
    bool interrupt = false;

    if (progress) {
        auto oldctlregs = ctlregs;
        interrupt = c.system.interact(c.icount++, ctlregs);
        if (interrupt || oldctlregs != ctlregs)
            if (!restarttr.has_value())
                restarttr = infra::pt::toplevel();
        cycles_since_progress = 0;
    } else {
        assert(++cycles_since_progress < 10);
    }

    if (restarttr.has_value()) {
        restart r;
        r.tr = *restarttr;
        r.new_pc = pc;
        r.interrupt = interrupt;
        r.stores_sent = stores_sent;
        gen = ++c.gen;
        c.restarto = std::move(r);
    }

    if (c.checker.icount != c.icount) {
        assert(c.checker.icount + 1 == c.icount);

        c.checker.execute();
        assert(c.checker.icount == c.icount);
//        std::cerr << fmt::format("icount={:} pc={:05o} checkerpc={:05o}\n", c.icount, pc, c.checker.pc);
        assert(pc == c.checker.pc);

        if (insto.has_value()) {
            auto &inst = *insto;
            assert(inst == c.checker.inst);
        }

        assert(acc == c.checker.acc);
        assert(link == c.checker.link);
        assert(mq == c.checker.mq);
        assert(ctlregs == c.checker.ctlregs);
    }

}