#include "fgc/OperatingMode.h"

#include "fgc/Geometry.h"

#include <cctype>
#include <cmath>
#include <sstream>

namespace fgc {

namespace {

std::string lower(std::string s) {
    for (char& c : s) c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
    return s;
}

std::vector<std::string> words(const std::string& line) {
    std::istringstream       in(line);
    std::vector<std::string> out;
    for (std::string w; in >> w;) out.push_back(lower(w));
    return out;
}

// A whole-token number: "30" and "-4.5" yes, "30x" and "" no.
bool number(const std::string& s, double& out) {
    try {
        size_t used = 0;
        out = std::stod(s, &used);
        return used == s.size() && std::isfinite(out);
    } catch (...) {
        return false;
    }
}

}  // namespace

const char* modeName(Mode m) {
    switch (m) {
        case Mode::Auto:   return "auto";
        case Mode::Pan:    return "pan";
        case Mode::Manual: return "manual";
    }
    return "?";
}

std::vector<double> panHeadings(const PanSpec& pan, double step) {
    if (step <= 0.0 || pan.span < step) return {wrap360(pan.heading)};
    const int    intervals = static_cast<int>(std::ceil(pan.span / step - 1e-9));
    const double spacing   = pan.span / intervals;
    const double left      = pan.heading - pan.span / 2.0;
    std::vector<double> out;
    for (int i = 0; i <= intervals; ++i) out.push_back(wrap360(left + i * spacing));
    return out;
}

ModeRequest parseModeCommand(const std::vector<std::string>& tokens) {
    ModeRequest r;
    if (tokens.size() < 2) {
        r.valid = true;
        r.query = true;
        return r;
    }
    const std::string which = lower(tokens[1]);
    if (which == "auto" || which == "manual") {
        if (tokens.size() > 2) {
            r.error = "'mode " + which + "' takes no arguments";
            return r;
        }
        r.mode  = which == "auto" ? Mode::Auto : Mode::Manual;
        r.valid = true;
        return r;
    }
    if (which != "pan") {
        r.error = "unknown mode '" + tokens[1] + "' (auto, pan or manual)";
        return r;
    }
    if (tokens.size() < 4 || tokens.size() > 5) {
        r.error = "usage: mode pan <heading> <span> [<pitch>]";
        return r;
    }
    PanSpec p;
    if (!number(tokens[2], p.heading)) {
        r.error = "mode pan: bad heading '" + tokens[2] + "'";
        return r;
    }
    if (!number(tokens[3], p.span) || p.span < 0.0 || p.span > 360.0) {
        r.error = "mode pan: the span must be 0 to 360 degrees (got '" + tokens[3] + "')";
        return r;
    }
    if (tokens.size() == 5 && !number(tokens[4], p.pitch)) {
        r.error = "mode pan: bad pitch '" + tokens[4] + "'";
        return r;
    }
    p.heading = wrap360(p.heading);
    r.mode    = Mode::Pan;
    r.pan     = p;
    r.valid   = true;
    return r;
}

bool allowedIn(Mode mode, const std::string& line) {
    if (mode == Mode::Manual) return true;
    const auto w = words(line);
    if (w.empty()) return true;
    const std::string& verb = w[0];
    const std::string  sub  = w.size() > 1 ? w[1] : "";
    if (verb == "help" || verb == "mode" || verb == "cancel" || verb == "exit" ||
        verb == "debug" || verb == "trace" || verb == "thermal")
        return true;
    if (verb == "gimbal") return sub == "status" || sub == "dump";
    if (verb == "imu") return sub == "status" || sub == "config";
    return false;
}

}  // namespace fgc