fwt_software/tests/test_mtiprotocol.cpp

#include <doctest/doctest.h>

#include "fgc/MtiProtocol.h"

#include <cstring>
#include <vector>

using namespace fgc;

namespace {
void putBEFloat(std::vector<uint8_t>& v, float f) {
    uint32_t u;
    std::memcpy(&u, &f, 4);
    v.push_back(static_cast<uint8_t>(u >> 24));
    v.push_back(static_cast<uint8_t>(u >> 16));
    v.push_back(static_cast<uint8_t>(u >> 8));
    v.push_back(static_cast<uint8_t>(u));
}

// Sum of all bytes from BID through CS must be ≡ 0 (mod 256) for a valid frame.
bool frameChecksumOk(const std::vector<uint8_t>& m) {
    unsigned s = 0;
    for (size_t i = 1; i < m.size(); ++i) s += m[i];
    return (s & 0xFF) == 0;
}
}  // namespace

TEST_CASE("config messages are well-formed with correct payloads") {
    auto cfg  = msgGoToConfig();
    auto mode = msgSetOutputMode();
    auto set  = msgSetOutputSettings();
    auto meas = msgGoToMeasurement();

    for (const auto& m : {cfg, mode, set, meas}) {
        CHECK(m[0] == kMtiPreamble);
        CHECK(m[1] == kMtiBid);
        CHECK(frameChecksumOk(m));
    }
    // GoToConfig / GoToMeasurement: no data.
    CHECK(cfg[2] == kMidGoToConfig);
    CHECK(cfg[3] == 0);
    CHECK(meas[2] == kMidGoToMeasurement);
    CHECK(meas[3] == 0);
    // SetOutputMode = 0x0007 (Temp|Calibrated|Orientation), 2-byte big-endian.
    CHECK(mode[2] == kMidSetOutputMode);
    CHECK(mode[3] == 2);
    CHECK(mode[4] == 0x00);
    CHECK(mode[5] == 0x07);
    // SetOutputSettings = 0x00000005 (Euler + sample counter), 4-byte big-endian.
    CHECK(set[2] == kMidSetOutputSettings);
    CHECK(set[3] == 4);
    CHECK(set[4] == 0x00);
    CHECK(set[5] == 0x00);
    CHECK(set[6] == 0x00);
    CHECK(set[7] == 0x05);
}

TEST_CASE("framer decodes a combined MTData frame into a full sample") {
    std::vector<uint8_t> d;
    putBEFloat(d, 24.5f);                                       // temp
    putBEFloat(d, 0.10f); putBEFloat(d, -0.20f); putBEFloat(d, 9.81f);   // acc
    putBEFloat(d, 0.01f); putBEFloat(d, 0.02f); putBEFloat(d, -0.03f);   // gyr
    putBEFloat(d, 0.45f); putBEFloat(d, -0.88f); putBEFloat(d, 0.21f);   // mag
    putBEFloat(d, -1.5f); putBEFloat(d, 3.25f); putBEFloat(d, 187.0f);   // roll/pitch/yaw
    d.push_back(0x12); d.push_back(0x34);                       // sample counter
    REQUIRE(d.size() == kMTDataLen);

    auto frame = mtiMessage(kMidMTData, d);

    ImuSample got;
    bool fired = false;
    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
        if (auto s = parseMTData(mid, p, n)) { got = *s; fired = true; }
    });

    // Leading noise must not break resync.
    const uint8_t noise[] = {0x00, 0xAB, 0xFA, 0x01};
    fr.feed(noise, sizeof(noise));
    fr.feed(frame.data(), frame.size());

    REQUIRE(fired);
    CHECK(got.valid);
    CHECK(got.temp_c == doctest::Approx(24.5f));
    CHECK(got.acc[2] == doctest::Approx(9.81f));
    CHECK(got.gyr[0] == doctest::Approx(0.01f));
    CHECK(got.mag[1] == doctest::Approx(-0.88f));
    CHECK(got.roll_deg == doctest::Approx(-1.5f));
    CHECK(got.pitch_deg == doctest::Approx(3.25f));
    CHECK(got.yaw_deg == doctest::Approx(187.0f));
    CHECK(got.sample_counter == 0x1234);
}

TEST_CASE("framer reassembles frames split across feeds and back-to-back frames") {
    // Real serial reads arrive in arbitrary chunks; the framer must not depend
    // on frame boundaries aligning with feed() calls.
    std::vector<uint8_t> d;
    putBEFloat(d, 7.5f);                  // temp
    d.resize(kMTDataLen, 0);              // rest of the 54-byte payload = 0
    auto frame = mtiMessage(kMidMTData, d);

    int    count = 0;
    float  last_temp = 0;
    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
        if (auto s = parseMTData(mid, p, n)) { ++count; last_temp = s->temp_c; }
    });

    // Two frames fed one byte at a time (worst-case fragmentation).
    for (uint8_t b : frame) fr.feed(&b, 1);
    for (uint8_t b : frame) fr.feed(&b, 1);
    CHECK(count == 2);
    CHECK(last_temp == doctest::Approx(7.5f));

    // Two frames concatenated in a single feed.
    std::vector<uint8_t> two = frame;
    two.insert(two.end(), frame.begin(), frame.end());
    count = 0;
    MtiFramer fr2([&](uint8_t mid, const uint8_t* p, size_t n) {
        if (parseMTData(mid, p, n)) ++count;
    });
    fr2.feed(two.data(), two.size());
    CHECK(count == 2);
}

TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
    auto frameWithYaw = [](float yaw) {
        std::vector<uint8_t> d;
        for (int i = 0; i < 10; ++i) putBEFloat(d, 0.f);  // temp + acc3 + gyr3 + mag3
        putBEFloat(d, 0.f);                                // roll
        putBEFloat(d, 0.f);                                // pitch
        putBEFloat(d, yaw);                                // yaw
        d.push_back(0); d.push_back(0);                    // counter
        return mtiMessage(kMidMTData, d);
    };
    ImuSample got;
    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
        if (auto s = parseMTData(mid, p, n)) got = *s;
    });

    auto fn = frameWithYaw(-90.f);   fr.feed(fn.data(), fn.size());
    CHECK(got.yaw_deg == doctest::Approx(270.0f));
    auto fp = frameWithYaw(45.f);    fr.feed(fp.data(), fp.size());
    CHECK(got.yaw_deg == doctest::Approx(45.0f));
    auto fb = frameWithYaw(-179.f);  fr.feed(fb.data(), fb.size());
    CHECK(got.yaw_deg == doctest::Approx(181.0f));
}

TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
    std::vector<uint8_t> d(kMTDataLen, 0);
    auto frame = mtiMessage(kMidMTData, d);

    SUBCASE("corrupt checksum") {
        auto bad = frame;
        bad.back() ^= 0xFF;
        bool fired = false;
        MtiFramer fr([&](uint8_t, const uint8_t*, size_t) { fired = true; });
        fr.feed(bad.data(), bad.size());
        CHECK_FALSE(fired);
    }
    SUBCASE("wrong-length MTData parses to nullopt") {
        std::vector<uint8_t> shortData(10, 0);
        CHECK_FALSE(parseMTData(kMidMTData, shortData.data(), shortData.size()).has_value());
    }
}