#pragma once

#include <cstdint>
#include <functional>
#include <optional>
#include <string>
#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;

// Config-readback queries (sent with an empty data field in Config State; the
// device replies with the matching ack MID = request MID + 1). ReqOutputMode /
// ReqOutputSettings reuse the Set MIDs above (len 0 => request, not set).
inline constexpr uint8_t kMidReqDID                = 0x00;
inline constexpr uint8_t kMidDeviceID              = 0x01;
inline constexpr uint8_t kMidReqPeriod             = 0x04;
inline constexpr uint8_t kMidReqPeriodAck          = 0x05;
inline constexpr uint8_t kMidReqFWRev              = 0x12;
inline constexpr uint8_t kMidFirmwareRev           = 0x13;
inline constexpr uint8_t kMidReqProductCode        = 0x1C;
inline constexpr uint8_t kMidProductCode           = 0x1D;
inline constexpr uint8_t kMidReqAvailFilterProf    = 0x62;
inline constexpr uint8_t kMidAvailFilterProf       = 0x63;
inline constexpr uint8_t kMidReqFilterProfile      = 0x64;  // SetScenario shares this MID
inline constexpr uint8_t kMidReqFilterProfileAck   = 0x65;

// Orientation-control commands (valid in Measurement State).
inline constexpr uint8_t kMidSetNoRotation    = 0x22;  // 2-byte duration (seconds)
inline constexpr uint8_t kMidSetNoRotationAck = 0x23;
inline constexpr uint8_t kMidResetOrientation    = 0xA4;  // 2-byte CODE (Table 33)
inline constexpr uint8_t kMidResetOrientationAck = 0xA5;
// ResetOrientation CODE values we use.
inline constexpr uint16_t kResetHeading = 0x0001;  // current heading becomes yaw 0
inline constexpr uint16_t kResetStore   = 0x0000;  // persist current reset (Config state)

// 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;
};

// One available filter profile (a.k.a. XKF "scenario") as reported by the device
// in the AvailableFilterProfiles message.
struct ImuFilterProfile {
    uint8_t     type    = 0;
    uint8_t     version = 0;
    std::string label;   // human name, e.g. "General" / "VRU_general"
};

// Device configuration read back during the Config-state handshake. Each `has_*`
// flag marks whether the corresponding ack was actually received and decoded, so
// the UI can show "—" for anything the device did not answer.
struct ImuDeviceConfig {
    bool        valid = false;  // at least one field was populated

    // Identity.
    std::string product_code;             // e.g. "MTi-28A33G85"
    bool        has_device_id = false;
    uint32_t    device_id = 0;            // serial / device ID
    std::string firmware;                 // "2.3.1 build 25" ("" if unknown)

    // Output mode (which data the device streams).
    bool        has_output_mode = false;
    uint16_t    output_mode = 0;
    bool        out_temperature = false, out_calibrated = false;
    bool        out_orientation = false, out_auxiliary = false, out_status = false;

    // Output settings (how the data is formatted).
    bool        has_output_settings = false;
    uint32_t    output_settings = 0;
    std::string orientation_mode;         // "Quaternion" / "Euler" / "Matrix"
    std::string timestamp_mode;           // "Sample counter" / "None"
    bool        acc_enabled = true, gyr_enabled = true, mag_enabled = true;
    std::string data_format;              // "Float" / "Fixed 12.20"

    // Sample rate.
    bool        has_period = false;
    uint16_t    period = 0;               // raw, resolution 1/115200 s
    float       sample_rate_hz = 0.f;

    // Filter profile / XKF scenario.
    bool        has_scenario = false;
    uint8_t     scenario_type = 0, scenario_version = 0;
    std::string scenario_label;           // resolved from available_profiles, else ""
    std::vector<ImuFilterProfile> available_profiles;
};

// Orientation-control command builders.
// SetNoRotation: run the "no rotation" gyro-bias update for `seconds` (the device
// must be held still during it) to cut heading drift, esp. in no-magnetometer use.
std::vector<uint8_t> msgSetNoRotation(uint16_t seconds);
// ResetOrientation: CODE 0x0001 redefines the *current* heading as yaw 0
// (bore-sighting); CODE 0x0000 (in Config state) stores it across power cycles.
std::vector<uint8_t> msgResetOrientation(uint16_t code);

// SetFilterProfile (classic): select the XKF profile/scenario by type number.
// Valid in Config state; settings changed in Config state persist to the device's
// non-volatile memory automatically.
std::vector<uint8_t> msgSetFilterProfile(uint16_t profile);

// Pick the no-magnetometer XKF profile from the device's reported list: prefers a
// label containing "nomag"/"no_mag", else a "VRU" (gyro-tracked heading) profile.
// Returns the profile `type` number, or -1 if none looks magnetometer-free.
int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles);

// Config-readback query builders (empty data field => "request", not "set").
std::vector<uint8_t> msgReqProductCode();
std::vector<uint8_t> msgReqDID();
std::vector<uint8_t> msgReqFWRev();
std::vector<uint8_t> msgReqPeriod();
std::vector<uint8_t> msgReqOutputMode();          // kMidSetOutputMode, len 0
std::vector<uint8_t> msgReqOutputSettings();      // kMidSetOutputSettings, len 0
std::vector<uint8_t> msgReqFilterProfile();
std::vector<uint8_t> msgReqAvailFilterProfiles();

// Decode a single config-ack frame into `c`. Recognizes DeviceID(0x01),
// ProductCode(0x1D), FirmwareRev(0x13), ReqPeriodAck(0x05), output-mode ack
// (0xD1), output-settings ack (0xD3), filter-profile ack (0x65), and the
// available-profiles list (0x63). Unknown MIDs are ignored. Returns true if the
// frame was recognized and applied.
bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n);

// Resolve derived fields once all acks are applied: the scenario label (matched
// against available_profiles by type) and sample_rate_hz from the period.
void finalizeImuConfig(ImuDeviceConfig& c);

// 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
