#include "fgc/TelemetryParser.h"

#include <sstream>
#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;
}

AxisState stateFromChar(char c) {
    switch (c) {
        case 'B': case 'b': return AxisState::Boot;
        case 'R': case 'r': return AxisState::Reset;
        case 'H': case 'h': return AxisState::Homing;
        case 'A': case 'a': return AxisState::Ready;
        case 'E': case 'e': return AxisState::Error;
        default:            return AxisState::Unknown;
    }
}

}  // namespace

std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg) {
    // seg looks like "A,982,969,80084000,0,8,8,Se" (the part after "Y:").
    std::vector<std::string> f = split(seg, ',');
    if (f.size() < 7 || f[0].empty()) return std::nullopt;  // flags field (index 7) is optional
    try {
        AxisTelemetry a;
        a.state      = stateFromChar(f[0][0]);
        a.xactual    = std::stol(f[1]);
        a.xenc       = std::stol(f[2]);
        a.drv_status = static_cast<unsigned>(std::stoul(f[3], nullptr, 16));
        a.sg         = std::stoi(f[4]);
        a.cs         = std::stoi(f[5]);
        a.pwm        = std::stoi(f[6]);
        if (f.size() > 7) {
            for (char c : f[7]) {
                switch (c) {
                    case 'S': a.standstill = true; break;
                    case 's': a.stall      = true; break;
                    case 'o': case 'O': a.overtemp = true; break;
                    case 'L': a.endstop_l  = true; break;
                    case 'R': a.endstop_r  = true; break;
                    default: break;  // 'e' (eeprom) and unknown letters ignored
                }
            }
        }
        return a;
    } catch (const std::exception&) {
        return std::nullopt;
    }
}

std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
    // Expect: ST Y:<...> [P:<...>]
    std::istringstream in(line);
    std::string tok;
    if (!(in >> tok) || tok != "ST") return std::nullopt;

    MotorTelemetry t;
    bool saw_yaw = false;
    while (in >> tok) {
        // tok is "Y:..." or "P:..."
        auto colon = tok.find(':');
        if (colon == std::string::npos) return std::nullopt;
        char axis = tok[0];
        auto seg  = parseAxisSegment(tok.substr(colon + 1));
        if (!seg) return std::nullopt;
        if (axis == 'Y' || axis == 'y') {
            t.yaw   = *seg;
            saw_yaw = true;
        } else if (axis == 'P' || axis == 'p') {
            t.pitch         = *seg;
            t.pitch_present = true;
        }
    }
    if (!saw_yaw) return std::nullopt;
    return t;
}

}  // namespace fgc