#include "fgc/MtiProtocol.h"

#include <cctype>
#include <cstdio>
#include <cstring>
#include <string>
#include <utility>

namespace fgc {

namespace {

// Big-endian readers (MTi is big-endian; host x86 is little-endian).
float beFloat(const uint8_t* p) {
    uint32_t u = (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) |
                 (uint32_t(p[2]) << 8) | uint32_t(p[3]);
    float f;
    std::memcpy(&f, &u, sizeof(f));
    return f;
}

uint16_t beU16(const uint8_t* p) {
    return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
}

uint32_t beU32(const uint8_t* p) {
    return (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) | (uint32_t(p[2]) << 8) |
           uint32_t(p[3]);
}

// Append a big-endian value to a byte vector.
void putBE(std::vector<uint8_t>& v, uint16_t x) {
    v.push_back(static_cast<uint8_t>(x >> 8));
    v.push_back(static_cast<uint8_t>(x & 0xFF));
}
void putBE(std::vector<uint8_t>& v, uint32_t x) {
    v.push_back(static_cast<uint8_t>((x >> 24) & 0xFF));
    v.push_back(static_cast<uint8_t>((x >> 16) & 0xFF));
    v.push_back(static_cast<uint8_t>((x >> 8) & 0xFF));
    v.push_back(static_cast<uint8_t>(x & 0xFF));
}

}  // namespace

uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len) {
    unsigned sum = 0;
    for (std::size_t i = 0; i < len; ++i) sum += from_bid[i];
    return static_cast<uint8_t>(sum & 0xFF);
}

std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data) {
    std::vector<uint8_t> m;
    m.reserve(5 + data.size());
    m.push_back(kMtiPreamble);
    m.push_back(kMtiBid);
    m.push_back(mid);
    m.push_back(static_cast<uint8_t>(data.size()));
    m.insert(m.end(), data.begin(), data.end());
    // Checksum covers BID..DATA; the CS byte makes the total ≡ 0 (mod 256).
    uint8_t s = mtiChecksum(m.data() + 1, m.size() - 1);
    m.push_back(static_cast<uint8_t>((0x100 - s) & 0xFF));
    return m;
}

std::vector<uint8_t> msgGoToConfig() { return mtiMessage(kMidGoToConfig); }
std::vector<uint8_t> msgGoToMeasurement() { return mtiMessage(kMidGoToMeasurement); }

std::vector<uint8_t> msgSetOutputMode() {
    std::vector<uint8_t> d;
    putBE(d, kOutputMode);
    return mtiMessage(kMidSetOutputMode, d);
}

std::vector<uint8_t> msgSetOutputSettings() {
    std::vector<uint8_t> d;
    putBE(d, kOutputSettings);
    return mtiMessage(kMidSetOutputSettings, d);
}

std::vector<uint8_t> msgSetNoRotation(uint16_t seconds) {
    std::vector<uint8_t> d;
    putBE(d, seconds);
    return mtiMessage(kMidSetNoRotation, d);
}

std::vector<uint8_t> msgResetOrientation(uint16_t code) {
    std::vector<uint8_t> d;
    putBE(d, code);
    return mtiMessage(kMidResetOrientation, d);
}

std::vector<uint8_t> msgSetFilterProfile(uint16_t profile) {
    std::vector<uint8_t> d;
    putBE(d, profile);
    return mtiMessage(kMidReqFilterProfile, d);  // same MID; non-empty data => "set"
}

int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles) {
    auto lower = [](std::string s) {
        for (char& c : s) c = static_cast<char>(std::tolower((unsigned char)c));
        return s;
    };
    int vru = -1;
    for (const auto& p : profiles) {
        const std::string l = lower(p.label);
        if (l.find("nomag") != std::string::npos || l.find("no_mag") != std::string::npos ||
            l.find("no mag") != std::string::npos)
            return p.type;                                  // explicit no-magnetometer
        if (vru < 0 && l.find("vru") != std::string::npos)  // gyro-tracked heading
            vru = p.type;
    }
    return vru;
}

std::vector<uint8_t> msgReqProductCode()        { return mtiMessage(kMidReqProductCode); }
std::vector<uint8_t> msgReqDID()                { return mtiMessage(kMidReqDID); }
std::vector<uint8_t> msgReqFWRev()              { return mtiMessage(kMidReqFWRev); }
std::vector<uint8_t> msgReqPeriod()             { return mtiMessage(kMidReqPeriod); }
std::vector<uint8_t> msgReqOutputMode()         { return mtiMessage(kMidSetOutputMode); }
std::vector<uint8_t> msgReqOutputSettings()     { return mtiMessage(kMidSetOutputSettings); }
std::vector<uint8_t> msgReqFilterProfile()      { return mtiMessage(kMidReqFilterProfile); }
std::vector<uint8_t> msgReqAvailFilterProfiles(){ return mtiMessage(kMidReqAvailFilterProf); }

bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n) {
    switch (mid) {
        case kMidDeviceID:
            if (n < 4) return false;
            c.device_id = beU32(d);
            c.has_device_id = true;
            c.valid = true;
            return true;

        case kMidProductCode: {
            // ASCII string, possibly space-padded; trim trailing spaces/NULs.
            std::size_t end = n;
            while (end > 0 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
            c.product_code.assign(reinterpret_cast<const char*>(d), end);
            c.valid = true;
            return true;
        }

        case kMidFirmwareRev: {
            // MAJOR MINOR REV [BUILDNR(4) SCMREF(4)] — older firmware sends only 3.
            if (n < 3) return false;
            char buf[48];
            if (n >= 7) {
                uint32_t build = beU32(d + 3);
                std::snprintf(buf, sizeof(buf), "%u.%u.%u build %u", d[0], d[1], d[2], build);
            } else {
                std::snprintf(buf, sizeof(buf), "%u.%u.%u", d[0], d[1], d[2]);
            }
            c.firmware = buf;
            c.valid = true;
            return true;
        }

        case kMidReqPeriodAck:
            if (n < 2) return false;
            c.period = beU16(d);
            c.has_period = true;
            c.valid = true;
            return true;

        case kMidSetOutputModeAck: {  // 0xD1, ack to ReqOutputMode
            if (n < 2) return false;
            uint16_t m = beU16(d);
            c.output_mode = m;
            c.out_temperature = m & 0x0001;
            c.out_calibrated  = m & 0x0002;
            c.out_orientation = m & 0x0004;
            c.out_auxiliary   = m & 0x0008;
            c.out_status      = m & 0x0800;
            c.has_output_mode = true;
            c.valid = true;
            return true;
        }

        case kMidSetOutputSettingsAck: {  // 0xD3, ack to ReqOutputSettings
            if (n < 4) return false;
            uint32_t s = beU32(d);
            c.output_settings = s;
            switch (s & 0x0003) {
                case 0x1: c.timestamp_mode = "Sample counter"; break;
                default:  c.timestamp_mode = "None"; break;
            }
            switch ((s >> 2) & 0x0003) {
                case 0x0: c.orientation_mode = "Quaternion"; break;
                case 0x1: c.orientation_mode = "Euler"; break;
                case 0x2: c.orientation_mode = "Matrix"; break;
                default:  c.orientation_mode = "?"; break;
            }
            // Bits 4/5/6: 1 = output DISABLED.
            c.acc_enabled = !(s & 0x0010);
            c.gyr_enabled = !(s & 0x0020);
            c.mag_enabled = !(s & 0x0040);
            c.data_format = ((s >> 8) & 0x0003) == 0x1 ? "Fixed 12.20" : "Float";
            c.has_output_settings = true;
            c.valid = true;
            return true;
        }

        case kMidReqFilterProfileAck:  // VERSION, FILTERPROFILE(type)
            if (n < 2) return false;
            c.scenario_version = d[0];
            c.scenario_type    = d[1];
            c.has_scenario = true;
            c.valid = true;
            return true;

        case kMidAvailFilterProf: {
            // Repeating 22-byte records: TYPE(1) VERSION(1) LABEL(20, space-padded).
            c.available_profiles.clear();
            for (std::size_t o = 0; o + 22 <= n; o += 22) {
                ImuFilterProfile p;
                p.type    = d[o];
                p.version = d[o + 1];
                if (p.type == 0) continue;  // empty slot
                std::size_t end = o + 22;
                while (end > o + 2 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
                p.label.assign(reinterpret_cast<const char*>(d + o + 2), end - (o + 2));
                c.available_profiles.push_back(std::move(p));
            }
            c.valid = true;
            return true;
        }

        default:
            return false;
    }
}

void finalizeImuConfig(ImuDeviceConfig& c) {
    if (c.has_period && c.period > 0)
        c.sample_rate_hz = 115200.0f / static_cast<float>(c.period);
    if (c.has_scenario) {
        for (const auto& p : c.available_profiles) {
            if (p.type == c.scenario_type) { c.scenario_label = p.label; break; }
        }
    }
}

std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
    if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
    ImuSample s;
    std::size_t o = 0;
    s.temp_c = beFloat(data + o);  o += 4;
    for (int i = 0; i < 3; ++i) { s.acc[i] = beFloat(data + o); o += 4; }
    for (int i = 0; i < 3; ++i) { s.gyr[i] = beFloat(data + o); o += 4; }
    for (int i = 0; i < 3; ++i) { s.mag[i] = beFloat(data + o); o += 4; }
    s.roll_deg  = beFloat(data + o); o += 4;
    s.pitch_deg = beFloat(data + o); o += 4;
    s.yaw_deg   = beFloat(data + o); o += 4;
    // Report yaw as a 0..360 heading rather than the MTi's native -180..180.
    if (s.yaw_deg < 0.f) s.yaw_deg += 360.f;
    s.sample_counter = beU16(data + o); o += 2;
    s.valid = true;
    return s;
}

void MtiFramer::feed(const uint8_t* p, std::size_t n) {
    for (std::size_t i = 0; i < n; ++i) {
        uint8_t b = p[i];
        switch (state_) {
            case S::Pre:
                if (b == kMtiPreamble) state_ = S::Bid;
                break;
            case S::Bid:
                // After PRE we expect BID; otherwise resync (allow back-to-back PRE).
                if (b == kMtiBid) { sum_ = b; state_ = S::Mid; }
                else if (b == kMtiPreamble) { /* stay */ }
                else state_ = S::Pre;
                break;
            case S::Mid:
                mid_ = b; sum_ += b; state_ = S::Len;
                break;
            case S::Len:
                len_ = b; sum_ += b; data_.clear();
                state_ = (len_ == 0) ? S::Cs : S::Data;
                break;
            case S::Data:
                data_.push_back(b); sum_ += b;
                if (data_.size() == len_) state_ = S::Cs;
                break;
            case S::Cs:
                sum_ += b;
                if ((sum_ & 0xFF) == 0 && sink_)
                    sink_(mid_, data_.data(), data_.size());
                state_ = S::Pre;
                break;
        }
    }
}

}  // namespace fgc