#pragma once

#include <cstdint>
#include <functional>
#include <optional>
#include <vector>

namespace fgc {

// Xsens MTi (legacy MT0100P) binary protocol helpers. The MTi streams MTData
// messages over RS-232/UART; this module frames the byte stream, validates
// checksums, builds the config messages, and decodes a combined MTData payload
// into an ImuSample. Kept pure (no I/O) so it is unit-testable and lives in
// fgc_core (mirrors TelemetryParser / DumpParser).
//
// Frame layout:  PRE(0xFA) BID(0xFF) MID LEN DATA[LEN] CS
// Checksum: (BID + MID + LEN + ΣDATA + CS) & 0xFF == 0. All multi-byte values
// are big-endian.

// Protocol constants.
inline constexpr uint8_t kMtiPreamble = 0xFA;
inline constexpr uint8_t kMtiBid      = 0xFF;
inline constexpr uint8_t kMidGoToConfig       = 0x30;
inline constexpr uint8_t kMidGoToConfigAck    = 0x31;
inline constexpr uint8_t kMidGoToMeasurement  = 0x10;
inline constexpr uint8_t kMidGoToMeasAck      = 0x11;
inline constexpr uint8_t kMidSetOutputMode    = 0xD0;
inline constexpr uint8_t kMidSetOutputModeAck = 0xD1;
inline constexpr uint8_t kMidSetOutputSettings    = 0xD2;
inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3;
inline constexpr uint8_t kMidMTData = 0x32;
inline constexpr uint8_t kMidError  = 0x42;

// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
inline constexpr uint16_t kOutputMode = 0x0007;
// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
// SampleCounter (bits1:0=01 => 0x01), float, all calibrated channels enabled.
inline constexpr uint32_t kOutputSettings = 0x00000005;
// Expected MTData payload with the above config: Temp(4) + Acc(12) + Gyr(12) +
// Mag(12) + Euler(12) + SampleCounter(2).
inline constexpr uint8_t kMTDataLen = 54;

// One fully decoded IMU reading.
struct ImuSample {
    bool     valid = false;
    float    temp_c = 0.f;          // °C
    float    acc[3] = {0, 0, 0};    // m/s^2 (incl. gravity), sensor frame
    float    gyr[3] = {0, 0, 0};    // rad/s
    float    mag[3] = {0, 0, 0};    // a.u. (normalized to earth field)
    float    roll_deg = 0.f;          // -180..180
    float    pitch_deg = 0.f;         // -90..90
    float    yaw_deg = 0.f;           // 0..360 heading (MTi native -180..180 is shifted)
    uint16_t sample_counter = 0;
};

// Lower byte of the sum of all bytes from BID through the end of DATA. The CS
// byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF.
uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len);

// Build a complete message (PRE BID MID LEN DATA CS) ready to write.
std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data = {});

// The four config messages for the orientation+calibrated Euler stream.
std::vector<uint8_t> msgGoToConfig();
std::vector<uint8_t> msgSetOutputMode();      // kOutputMode
std::vector<uint8_t> msgSetOutputSettings();  // kOutputSettings
std::vector<uint8_t> msgGoToMeasurement();

// Decode an MTData payload (the DATA bytes, big-endian) into an ImuSample.
// Returns nullopt if mid != MTData or len != kMTDataLen.
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len);

// Incremental framer: feed raw bytes; for each complete, checksum-valid frame it
// invokes the sink with (mid, data, len). Tolerates noise/resync by re-scanning
// for the preamble.
class MtiFramer {
public:
    using FrameSink = std::function<void(uint8_t mid, const uint8_t* data, std::size_t len)>;

    explicit MtiFramer(FrameSink sink) : sink_(std::move(sink)) {}
    void feed(const uint8_t* p, std::size_t n);

private:
    enum class S { Pre, Bid, Mid, Len, Data, Cs };
    FrameSink            sink_;
    S                    state_ = S::Pre;
    uint8_t              mid_ = 0;
    uint8_t              len_ = 0;
    std::vector<uint8_t> data_;
    unsigned             sum_ = 0;  // running checksum sum (BID..DATA)
};

}  // namespace fgc