81 lines
2.1 KiB
C++
81 lines
2.1 KiB
C++
#include "fgc/CommandParser.h"
|
|
|
|
#include <sstream>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
bool parseNumber(const std::string& s, double& out) {
|
|
if (s.empty()) return false;
|
|
try {
|
|
size_t pos = 0;
|
|
out = std::stod(s, &pos);
|
|
return pos == s.size(); // entire token must be numeric
|
|
} catch (const std::exception&) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Command parseCommand(const std::string& line) {
|
|
std::vector<std::string> tok;
|
|
std::istringstream iss(line);
|
|
std::string t;
|
|
while (iss >> t) tok.push_back(t);
|
|
|
|
Command cmd;
|
|
if (tok.empty()) return cmd;
|
|
|
|
cmd.verb = tok[0];
|
|
size_t idx = 1;
|
|
if (idx < tok.size()) cmd.device = tok[idx++];
|
|
|
|
if (idx < tok.size()) {
|
|
// If the last token is numeric, treat it as the value.
|
|
double v = 0.0;
|
|
size_t end = tok.size();
|
|
if (parseNumber(tok.back(), v)) {
|
|
cmd.value = v;
|
|
cmd.has_value = true;
|
|
end = tok.size() - 1;
|
|
}
|
|
// Anything between device and the value becomes the option (joined).
|
|
std::string option;
|
|
for (size_t i = idx; i < end; ++i) {
|
|
if (!option.empty()) option += " ";
|
|
option += tok[i];
|
|
}
|
|
cmd.option = option;
|
|
}
|
|
return cmd;
|
|
}
|
|
|
|
std::optional<Aim> parseAim(const std::vector<std::string>& tokens, size_t from) {
|
|
std::vector<std::string> nums;
|
|
for (size_t i = from; i < tokens.size(); ++i) {
|
|
std::istringstream parts(tokens[i]);
|
|
for (std::string n; std::getline(parts, n, ',');)
|
|
if (!n.empty()) nums.push_back(n);
|
|
}
|
|
if (nums.empty() || nums.size() > 2) return std::nullopt;
|
|
Aim a;
|
|
if (!parseNumber(nums[0], a.yaw)) return std::nullopt;
|
|
if (nums.size() == 2) {
|
|
double p = 0.0;
|
|
if (!parseNumber(nums[1], p)) return std::nullopt;
|
|
a.pitch = p;
|
|
}
|
|
return a;
|
|
}
|
|
|
|
std::string moveCommand(long yaw_counts, std::optional<long> pitch_counts) {
|
|
if (pitch_counts)
|
|
return "MOVE " + std::to_string(yaw_counts) + "," + std::to_string(*pitch_counts);
|
|
return "MOVE Y " + std::to_string(yaw_counts);
|
|
}
|
|
|
|
} // namespace fgc
|