#include "fgc/FirmwareConfig.h"

#include <sstream>

namespace fgc {
namespace {

// One answerable key: where its value comes from, and whether the host has it.
struct Answer {
    bool        have = false;
    long long   value = 0;
};

Answer answerFor(const std::string& key, const AxisConfigSource& s) {
    // Motor current. All three come from the same conversion, so they are
    // available together or not at all.
    if (key == "global_scaler") return {s.have_current, s.global_scaler};
    if (key == "run_irun")      return {s.have_current, s.run_irun};
    if (key == "homing_irun")   return {s.have_current, s.homing_irun};

    // Position anchors. `center` / `margin` are the names the existing CENTER
    // and MARGIN commands use.
    if (key == "center_count" || key == "center") return {true, s.center_count};
    if (key == "soft_margin"  || key == "margin") return {true, s.soft_margin};

    if (key == "steps_per_rev") return {s.have_travel, s.steps_per_rev};
    if (key == "enc_deviation") return {s.have_deviation, s.enc_deviation};

    // A VMAX of 0 is not a slow axis, it is a stopped one, so an unconverted
    // speed is reported as missing rather than sent.
    if (key == "normal_speed") return {s.normal_vmax > 0, s.normal_vmax};
    if (key == "homing_speed") return {s.homing_vmax > 0, s.homing_vmax};
    if (key == "center_speed") return {s.center_vmax > 0, s.center_vmax};

    if (key == "sw_mode")     return {s.have_wiring, static_cast<long long>(s.sw_mode)};
    if (key == "has_encoder") return {s.have_wiring, s.has_encoder ? 1 : 0};

    return {};  // not in the vocabulary
}

bool known(const std::string& key) {
    static const char* kKeys[] = {
        "global_scaler", "run_irun",     "homing_irun", "center_count", "center",
        "soft_margin",   "margin",       "steps_per_rev", "enc_deviation",
        "normal_speed",  "homing_speed", "center_speed",  "sw_mode",    "has_encoder",
    };
    for (const char* k : kKeys)
        if (key == k) return true;
    return false;
}

}  // namespace

std::optional<NeedRequest> parseNeedLine(const std::string& line) {
    std::istringstream is(line);
    std::string tok;
    if (!(is >> tok) || tok != "NEED") return std::nullopt;

    std::string axis;
    if (!(is >> axis)) return std::nullopt;
    if (axis.size() != 1) return std::nullopt;
    const char a = static_cast<char>(std::toupper(static_cast<unsigned char>(axis[0])));
    if (a != 'Y' && a != 'P') return std::nullopt;

    NeedRequest r;
    r.axis = a;
    while (is >> tok) r.keys.push_back(tok);
    return r;
}

FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src) {
    FirmwareConfigPlan plan;
    const std::string axis(1, need.axis);

    for (const auto& key : need.keys) {
        if (!known(key)) {
            // The firmware wants something this host has never heard of, which
            // means it is newer. Naming it is the whole diagnostic.
            plan.unknown.push_back(key);
            continue;
        }
        const Answer a = answerFor(key, src);
        if (!a.have) {
            plan.unanswered.push_back(key);
            continue;
        }
        plan.sets.push_back("SET " + axis + " " + key + " " + std::to_string(a.value));
    }
    return plan;
}

}  // namespace fgc