#include "fgc/TelemetryParser.h"

#include <vector>

namespace fgc {

namespace {

std::vector<std::string> split(const std::string& s, char delim) {
    std::vector<std::string> out;
    std::string cur;
    for (char c : s) {
        if (c == delim) {
            out.push_back(cur);
            cur.clear();
        } else if (c != '\r' && c != '\n') {
            cur += c;
        }
    }
    out.push_back(cur);
    return out;
}

}  // namespace

std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
    if (line.empty() || line[0] != '$') return std::nullopt;

    std::vector<std::string> f = split(line, ';');
    // Need indices 0..11: '$' marker + 11 data fields.
    if (f.size() < 12) return std::nullopt;

    try {
        MotorTelemetry t;
        t.encoder        = std::stoi(f[1]);
        t.encoder_err    = std::stoi(f[2]);
        t.sgt_val        = std::stoi(f[3]);
        t.sgt_stat       = std::stoi(f[4]);
        t.is_moving      = std::stoi(f[5]);
        t.control_status = std::stoi(f[6]);
        t.heading        = std::stof(f[7]);
        t.deviation_warn = std::stoi(f[8]);
        // NOTE: field order is humidity (index 9) BEFORE temperature (index 10),
        // matching the controller firmware. See docs/known-issues.md #6 - do not
        // swap without confirming the firmware output.
        t.humidity       = std::stoi(f[9]);
        t.temperature    = std::stoi(f[10]);
        t.fan_pwm        = std::stoi(f[11]);
        return t;
    } catch (const std::exception&) {
        return std::nullopt;
    }
}

}  // namespace fgc