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

namespace {

const char* orientationName(uint32_t s) {
    switch (s & mtsettings::kOrientationMask) {
        case mtsettings::kOrientationQuaternion: return "Quaternion";
        case mtsettings::kOrientationEuler:      return "Euler";
        case mtsettings::kOrientationMatrix:     return "Matrix";
        default:                                 return "reserved";
    }
}

const char* formatName(uint32_t s) {
    switch (s & mtsettings::kFormatMask) {
        case mtsettings::kFormatFloat:     return "Float";
        case mtsettings::kFormatFixed1220: return "Fixed 12.20";
        default:                           return "Fixed 16.32";
    }
}

const char* timestampName(uint32_t s) {
    return (s & mtsettings::kTimestampMask) == mtsettings::kTimestampSampleCounter
               ? "Sample counter"
               : "none";
}

std::string join(const std::vector<std::string>& parts) {
    std::string out;
    for (const auto& p : parts) {
        if (!out.empty()) out += "; ";
        out += p;
    }
    return out;
}

std::string modeDetail(uint16_t expected, uint16_t actual) {
    const std::pair<uint16_t, const char*> blocks[] = {
        {mtmode::kTemperature, "temperature"}, {mtmode::kCalibrated, "calibrated data"},
        {mtmode::kOrientation, "orientation"}, {mtmode::kAuxiliary, "auxiliary"},
        {mtmode::kStatus, "status"},
    };
    std::vector<std::string> parts;
    for (const auto& [bit, name] : blocks) {
        if ((expected & bit) == (actual & bit)) continue;
        parts.push_back(std::string(name) + ((actual & bit) ? " is on, was not asked for"
                                                            : " is off, but was asked for"));
    }
    return join(parts);
}

std::string settingsDetail(uint32_t expected, uint32_t actual) {
    std::vector<std::string> parts;
    if ((expected & mtsettings::kOrientationMask) != (actual & mtsettings::kOrientationMask))
        parts.push_back(std::string("orientation is ") + orientationName(actual) +
                        ", asked for " + orientationName(expected));
    if ((expected & mtsettings::kFormatMask) != (actual & mtsettings::kFormatMask))
        parts.push_back(std::string("data format is ") + formatName(actual) + ", asked for " +
                        formatName(expected));
    if ((expected & mtsettings::kTimestampMask) != (actual & mtsettings::kTimestampMask))
        parts.push_back(std::string("timestamp is ") + timestampName(actual) + ", asked for " +
                        timestampName(expected));

    const std::pair<uint32_t, const char*> channels[] = {
        {mtsettings::kAccDisabled, "acceleration"},
        {mtsettings::kGyrDisabled, "rate of turn"},
        {mtsettings::kMagDisabled, "magnetic field"},
    };
    for (const auto& [bit, name] : channels) {
        if ((expected & bit) == (actual & bit)) continue;
        parts.push_back(std::string(name) + ((actual & bit) ? " is disabled on the device"
                                                            : " is enabled but was not asked for"));
    }

    // Anything outside the fields decoded above. The coordinate system lives in
    // here, so while mtsettings::kCoordSysNed is still 0 this is the branch that
    // reports a device whose earth reference is not what the gimbal frame assumes.
    const uint32_t known = mtsettings::kOrientationMask | mtsettings::kFormatMask |
                           mtsettings::kTimestampMask | mtsettings::kAccDisabled |
                           mtsettings::kGyrDisabled | mtsettings::kMagDisabled;
    if ((expected ^ actual) & ~known) {
        char buf[96];
        std::snprintf(buf, sizeof(buf),
                      "other bits differ (0x%08X); the coordinate system is one of these",
                      (expected ^ actual) & ~known);
        parts.push_back(buf);
    }
    return join(parts);
}

}  // namespace

std::string ImuConfigMismatch::describe() const {
    if (!answered) return field + ": the device did not report it, so it could not be verified";
    char buf[80];
    std::snprintf(buf, sizeof(buf), ": asked for 0x%08X, device reports 0x%08X", expected, actual);
    std::string out = field + buf;
    if (!detail.empty()) out += " (" + detail + ")";
    return out;
}

std::vector<ImuConfigMismatch> checkImuDeviceConfig(const ImuDeviceConfig& c) {
    std::vector<ImuConfigMismatch> out;

    if (!c.has_output_mode) {
        out.push_back({"output mode", kOutputMode, 0, false, ""});
    } else if (c.output_mode != kOutputMode) {
        out.push_back({"output mode", kOutputMode, c.output_mode, true,
                       modeDetail(kOutputMode, c.output_mode)});
    }

    if (!c.has_output_settings) {
        out.push_back({"output settings", kOutputSettings, 0, false, ""});
    } else if (c.output_settings != kOutputSettings) {
        out.push_back({"output settings", kOutputSettings, c.output_settings, true,
                       settingsDetail(kOutputSettings, c.output_settings)});
    }

    return out;
}

std::string mtiErrorText(uint8_t code) {
    switch (code) {
        case 0x03: return "invalid period";
        case 0x04: return "invalid message";
        case 0x1E: return "timer overflow";
        case 0x20: return "invalid baud rate";
        case 0x21: return "invalid parameter";
        case 0x28: return "device error";
        default: {
            char buf[32];
            std::snprintf(buf, sizeof(buf), "unknown error 0x%02X", code);
            return buf;
        }
    }
}

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;
    // Reported exactly as the device sends it. Roll, pitch and yaw are all raw:
    // the only transformation applied anywhere is the [IMU] x/y/z mount rotation,
    // so what reaches a consumer is the sensor's own numbers in the frame the
    // operator declared - nothing else.
    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