#include "fgc/DumpParser.h"

#include <array>
#include <sstream>
#include <utility>

namespace fgc {

namespace {

// Bit tables, copied verbatim from firmware/tools/decode_dump.py (keep in sync).
const char* stateName(int s) {
    switch (s) {
        case 0: return "BOOT";
        case 1: return "RESET";
        case 2: return "HOMING";
        case 3: return "READY";
        case 4: return "ERROR";
        default: return "?";
    }
}

struct Bit { int shift; const char* name; };

const std::array<Bit, 5> kMcusr = {{
    {0, "PORF (power-on)"}, {1, "EXTRF (external)"}, {2, "BORF (brown-out)"},
    {3, "WDRF (watchdog)"}, {4, "JTRF (jtag)"},
}};
const std::array<Bit, 12> kDrv = {{
    {31, "stst"}, {30, "olb"}, {29, "ola"}, {28, "s2gb"}, {27, "s2ga"},
    {26, "otpw"}, {25, "ot"}, {24, "stallguard"}, {15, "fsactive"},
    {14, "stealth"}, {12, "s2vsb"}, {11, "s2vsa"},
}};
const std::array<Bit, 3> kGstat = {{
    {0, "reset"}, {1, "drv_err"}, {2, "uv_cp"},
}};
const std::array<Bit, 10> kRamp = {{
    {0, "stop_l"}, {1, "stop_r"}, {4, "event_stop_l"}, {5, "event_stop_r"},
    {6, "event_stop_sg"}, {7, "event_pos_reached"}, {8, "velocity_reached"},
    {9, "position_reached"}, {10, "vzero"}, {13, "status_sg"},
}};

template <size_t N>
std::vector<std::string> bitsSet(unsigned val, const std::array<Bit, N>& table) {
    std::vector<std::string> out;
    for (const auto& b : table)
        if (val & (1u << b.shift)) out.emplace_back(b.name);
    if (out.empty()) out.emplace_back("-");
    return out;
}

// Parse "key=value key=value ..." into a map.
std::map<std::string, std::string> kvPairs(const std::string& s) {
    std::map<std::string, std::string> m;
    std::istringstream iss(s);
    std::string tok;
    while (iss >> tok) {
        auto eq = tok.find('=');
        if (eq != std::string::npos) m[tok.substr(0, eq)] = tok.substr(eq + 1);
    }
    return m;
}

// Integer from a decimal or "0x"-prefixed hex string; `fallback` on failure.
long asInt(const std::string& s, long fallback = 0) {
    if (s.empty()) return fallback;
    try {
        size_t pos = 0;
        bool hex = s.size() > 1 && s[0] == '0' && (s[1] == 'x' || s[1] == 'X');
        long v = std::stol(s, &pos, hex ? 16 : 10);
        return v;
    } catch (const std::exception&) {
        return fallback;
    }
}

std::string get(const std::map<std::string, std::string>& m, const std::string& k,
                const std::string& fallback = "") {
    auto it = m.find(k);
    return it == m.end() ? fallback : it->second;
}

std::string join(const std::vector<std::string>& v) {
    std::string out;
    for (size_t i = 0; i < v.size(); ++i) out += (i ? " " : "") + v[i];
    return out;
}

}  // namespace

DumpData parseDump(const std::string& block) {
    DumpData d;
    if (block.empty()) return d;

    // Split into lines, locate the BEGIN..END envelope.
    std::vector<std::string> lines;
    {
        std::istringstream iss(block);
        std::string l;
        while (std::getline(iss, l)) {
            if (!l.empty() && l.back() == '\r') l.pop_back();
            lines.push_back(l);
        }
    }
    size_t begin = lines.size(), end = lines.size();
    for (size_t i = 0; i < lines.size(); ++i) {
        if (begin == lines.size() && lines[i].rfind("DUMP BEGIN", 0) == 0) begin = i;
        if (lines[i].rfind("DUMP END", 0) == 0) { end = i; break; }
    }
    if (begin == lines.size() || end <= begin) return d;

    // Header.
    auto hdr     = kvPairs(lines[begin].substr(std::string("DUMP BEGIN").size()));
    d.build      = get(hdr, "build");
    d.uptime_ms  = asInt(get(hdr, "uptime", "0"));
    d.mcusr      = static_cast<unsigned>(asInt(get(hdr, "mcusr", "0")));
    d.free_ram   = asInt(get(hdr, "free_ram", "0"));
    d.reset_flags = bitsSet(d.mcusr, kMcusr);

    // Per-axis lines: "DUMP <Y|P> state=.." and "DUMP <Y|P> TMC ..".
    auto axisFor = [&d](char a) -> DumpAxis& {
        for (auto& ax : d.axes)
            if (ax.axis == a) return ax;
        d.axes.push_back(DumpAxis{});
        d.axes.back().axis = a;
        return d.axes.back();
    };

    for (size_t i = begin + 1; i < end; ++i) {
        const std::string& l = lines[i];
        if (l.rfind("DUMP ", 0) != 0 || l.size() < 7) continue;
        char a = l[5];
        if (a != 'Y' && a != 'P') continue;
        // Skip past "DUMP X" and any whitespace; the payload is either
        // "state=.." or "TMC GCONF=..". (substr(6) alone leaves a leading space,
        // which would make the "TMC" prefix check below miss.)
        std::string rest = l.substr(6);
        size_t nb = rest.find_first_not_of(" \t");
        rest = (nb == std::string::npos) ? "" : rest.substr(nb);
        DumpAxis& ax = axisFor(a);

        if (rest.rfind("TMC", 0) == 0) {
            auto m = kvPairs(rest.substr(3));
            ax.regs = m;
            ax.drv_status = static_cast<unsigned>(asInt(get(m, "DRV_STATUS", "0")));
            ax.gstat      = static_cast<unsigned>(asInt(get(m, "GSTAT", "0")));
            ax.ramp_stat  = static_cast<unsigned>(asInt(get(m, "RAMP_STAT", "0")));
            ax.cs_actual  = static_cast<int>((ax.drv_status >> 16) & 0x1F);
            ax.sg_result  = static_cast<int>(ax.drv_status & 0x3FF);
            ax.drv_flags   = bitsSet(ax.drv_status, kDrv);
            ax.gstat_flags = bitsSet(ax.gstat, kGstat);
            ax.ramp_flags  = bitsSet(ax.ramp_stat, kRamp);
        } else {
            auto m = kvPairs(rest);
            ax.state_name      = stateName(static_cast<int>(asInt(get(m, "state", "-1"))));
            ax.hsub            = static_cast<int>(asInt(get(m, "hsub", "0")));
            ax.enabled         = asInt(get(m, "enabled", "0")) != 0;
            ax.eeprom_restored = asInt(get(m, "eeprom_restored", "0")) != 0;
            ax.has_encoder     = asInt(get(m, "has_encoder", "0")) != 0;
            ax.lim_neg         = asInt(get(m, "lim_neg", "0"));
            ax.lim_pos         = asInt(get(m, "lim_pos", "0"));
            ax.hold_target     = asInt(get(m, "hold_target", "0"));
            ax.speed           = asInt(get(m, "speed", "0"));
        }
    }

    d.valid = true;
    return d;
}

std::vector<std::string> formatDump(const DumpData& d) {
    std::vector<std::string> out;
    if (!d.valid) {
        out.emplace_back("(no firmware dump captured)");
        return out;
    }
    {
        std::ostringstream os;
        os << "build=" << d.build << "  uptime=" << d.uptime_ms << " ms  free_ram="
           << d.free_ram << " bytes";
        out.push_back(os.str());
    }
    {
        std::ostringstream os;
        os << "reset cause (mcusr=0x" << std::hex << d.mcusr << std::dec << "): "
           << join(d.reset_flags);
        out.push_back(os.str());
    }
    for (const auto& ax : d.axes) {
        out.emplace_back("");
        {
            std::ostringstream os;
            os << "[" << ax.axis << "] state=" << ax.state_name
               << "  enabled=" << (ax.enabled ? 1 : 0)
               << "  limits=[" << ax.lim_neg << ".." << ax.lim_pos << "]"
               << "  hold_target=" << ax.hold_target
               << "  eeprom_restored=" << (ax.eeprom_restored ? 1 : 0)
               << "  has_encoder=" << (ax.has_encoder ? 1 : 0);
            out.push_back(os.str());
        }
        for (const char* k : {"GCONF", "CHOPCONF", "XACTUAL", "X_ENC", "XTARGET", "VACTUAL"}) {
            auto it = ax.regs.find(k);
            if (it != ax.regs.end()) out.push_back(std::string("    ") + k + " = " + it->second);
        }
        out.push_back("    DRV_STATUS = " + get(ax.regs, "DRV_STATUS", "?") + "  [" +
                      join(ax.drv_flags) + "]  CS_ACTUAL=" + std::to_string(ax.cs_actual) +
                      " SG_RESULT=" + std::to_string(ax.sg_result));
        out.push_back("    GSTAT      = " + get(ax.regs, "GSTAT", "?") + "  [" +
                      join(ax.gstat_flags) + "]");
        out.push_back("    RAMP_STAT  = " + get(ax.regs, "RAMP_STAT", "?") + "  [" +
                      join(ax.ramp_flags) + "]");
    }
    return out;
}

}  // namespace fgc