#include "DroneScrewAlgoStub.h"
#include "VrError.h"
#include "VrLog.h"

#include <QCoreApplication>
#include <QDir>
#include <QDomDocument>
#include <QFile>
#include <QFileInfo>
#include <QHash>
#include <QByteArray>
#include <QStringList>
#include <QTextStream>

#include <algorithm>
#include <cmath>
#include <cstring>
#include <exception>
#include <limits>
#include <sstream>

#ifdef DRONESCREW_STEREO_BOLT_DIRECT_LINK
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>

namespace
{
constexpr const char* kDefaultConfigPath = "config/config.rk3588.yaml";
constexpr const char* kDefaultCalibPath = "calib/stereo_calib.xml";
constexpr const char* kDefaultModelPath = "weights/best.rknn";
constexpr int kDefaultYoloImgSize = 960;
constexpr int kDefaultExpectedBoltCount = 8;
constexpr int kMaxRangeBolts = 64;

cv::Mat mat64(int rows, int cols, const double* values)
{
    cv::Mat m(rows, cols, CV_64F);
    std::memcpy(m.ptr<double>(0), values, sizeof(double) * rows * cols);
    return m;
}

int roiSampleCount(double span)
{
    if (span <= 1.0)
        return 2;
    return std::max(3, std::min(17, static_cast<int>(std::ceil(span / 64.0)) + 1));
}

bool sampleMapBilinear(const std::vector<float>& mapX,
                       const std::vector<float>& mapY,
                       int mapWidth,
                       int mapHeight,
                       double rectX,
                       double rectY,
                       double& rawX,
                       double& rawY)
{
    if (mapWidth <= 0 || mapHeight <= 0 ||
        mapX.size() != static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight) ||
        mapY.size() != mapX.size())
    {
        return false;
    }

    rectX = std::max(0.0, std::min(rectX, static_cast<double>(mapWidth - 1)));
    rectY = std::max(0.0, std::min(rectY, static_cast<double>(mapHeight - 1)));

    const int x0 = static_cast<int>(std::floor(rectX));
    const int y0 = static_cast<int>(std::floor(rectY));
    const int x1 = std::min(x0 + 1, mapWidth - 1);
    const int y1 = std::min(y0 + 1, mapHeight - 1);
    const double tx = rectX - x0;
    const double ty = rectY - y0;

    const size_t i00 = static_cast<size_t>(y0) * mapWidth + x0;
    const size_t i10 = static_cast<size_t>(y0) * mapWidth + x1;
    const size_t i01 = static_cast<size_t>(y1) * mapWidth + x0;
    const size_t i11 = static_cast<size_t>(y1) * mapWidth + x1;

    const double xTop = static_cast<double>(mapX[i00]) * (1.0 - tx) +
                        static_cast<double>(mapX[i10]) * tx;
    const double xBottom = static_cast<double>(mapX[i01]) * (1.0 - tx) +
                           static_cast<double>(mapX[i11]) * tx;
    const double yTop = static_cast<double>(mapY[i00]) * (1.0 - tx) +
                        static_cast<double>(mapY[i10]) * tx;
    const double yBottom = static_cast<double>(mapY[i01]) * (1.0 - tx) +
                           static_cast<double>(mapY[i11]) * tx;

    rawX = xTop * (1.0 - ty) + xBottom * ty;
    rawY = yTop * (1.0 - ty) + yBottom * ty;
    return std::isfinite(rawX) && std::isfinite(rawY);
}

void appendUniqueDir(QStringList& dirs, const QString& dir)
{
    if (dir.isEmpty())
        return;

    const QString absolute = QDir(dir).absolutePath();
    if (!dirs.contains(absolute))
        dirs.append(absolute);
}

QString resolvePath(const QString& path, const QStringList& baseDirs)
{
    const QString trimmed = path.trimmed();
    if (trimmed.isEmpty())
        return QString();

    const QFileInfo direct(trimmed);
    if (direct.isAbsolute())
        return direct.absoluteFilePath();

    QString fallback;
    for (const QString& baseDir : baseDirs)
    {
        const QFileInfo candidate(QDir(baseDir).filePath(trimmed));
        if (fallback.isEmpty())
            fallback = candidate.absoluteFilePath();
        if (candidate.exists())
            return candidate.absoluteFilePath();
    }
    return fallback.isEmpty() ? QFileInfo(trimmed).absoluteFilePath() : fallback;
}

QString yamlValueAfterColon(QString line)
{
    const int comment = line.indexOf('#');
    if (comment >= 0)
        line = line.left(comment);

    const int colon = line.indexOf(':');
    if (colon < 0)
        return QString();

    QString value = line.mid(colon + 1).trimmed();
    if (value.size() >= 2)
    {
        const QChar first = value.at(0);
        const QChar last = value.at(value.size() - 1);
        if ((first == '"' && last == '"') || (first == '\'' && last == '\''))
            value = value.mid(1, value.size() - 2);
    }
    return value;
}

QHash<QString, QString> readYamlValues(const QString& configPath)
{
    QHash<QString, QString> values;
    QFile file(configPath);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return values;

    QTextStream in(&file);
    QString section;
    int sectionIndent = -1;
    while (!in.atEnd())
    {
        const QString raw = in.readLine();
        int indent = 0;
        while (indent < raw.size() && raw.at(indent).isSpace())
            ++indent;

        QString line = raw.mid(indent);
        const int comment = line.indexOf('#');
        if (comment >= 0)
            line = line.left(comment);
        line = line.trimmed();
        if (line.isEmpty())
            continue;

        if (indent == 0 && line.endsWith(':'))
        {
            section = line.left(line.size() - 1).trimmed();
            sectionIndent = indent;
            continue;
        }
        if (!section.isEmpty() && indent <= sectionIndent)
            section.clear();

        const int colon = line.indexOf(':');
        if (colon < 0)
            continue;

        const QString key = line.left(colon).trimmed();
        const QString value = yamlValueAfterColon(line);
        const QString fullKey = section.isEmpty() ? key : (section + "." + key);
        values[fullKey] = value;
    }
    return values;
}

QString childText(const QDomElement& parent, const QString& name)
{
    const QDomElement child = parent.firstChildElement(name);
    return child.isNull() ? QString() : child.text().trimmed();
}

int childInt(const QDomElement& parent, const QString& name, int fallback = 0)
{
    bool ok = false;
    const int value = childText(parent, name).toInt(&ok);
    return ok ? value : fallback;
}

double childDouble(const QDomElement& parent, const QString& name, double fallback = 0.0)
{
    bool ok = false;
    const double value = childText(parent, name).toDouble(&ok);
    return ok ? value : fallback;
}

std::vector<double> matrixValues(const QDomElement& parent, const QString& name)
{
    std::vector<double> values;
    const QDomElement element = parent.firstChildElement(name);
    if (element.isNull())
        return values;

    const QString text = childText(element, "data").simplified();
    const QStringList tokens = text.split(' ', Qt::SkipEmptyParts);
    values.reserve(tokens.size());
    for (const QString& token : tokens)
    {
        bool ok = false;
        const double value = token.toDouble(&ok);
        if (ok)
            values.push_back(value);
    }
    return values;
}

bool copyDoubles(const std::vector<double>& src, double* dst, int count)
{
    if (static_cast<int>(src.size()) < count)
        return false;
    for (int i = 0; i < count; ++i)
        dst[i] = src[static_cast<size_t>(i)];
    return true;
}

bool loadCalibration(const QString& xmlPath, StereoBoltCalibC& calib, std::string& error)
{
    QFile file(xmlPath);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        error = "open calibration xml failed: " + file.errorString().toStdString();
        return false;
    }

    QDomDocument doc;
    QString parseError;
    int line = 0;
    int column = 0;
    if (!doc.setContent(&file, &parseError, &line, &column))
    {
        std::ostringstream oss;
        oss << "parse calibration xml failed at " << line << ":" << column
            << ": " << parseError.toStdString();
        error = oss.str();
        return false;
    }

    const QDomElement root = doc.documentElement();
    const QDomElement left = root.firstChildElement("LeftCamera");
    const QDomElement right = root.firstChildElement("RightCamera");
    const QDomElement stereo = root.firstChildElement("Stereo");
    if (left.isNull() || right.isNull() || stereo.isNull())
    {
        error = "calibration xml missing LeftCamera/RightCamera/Stereo";
        return false;
    }

    std::memset(&calib, 0, sizeof(calib));
    calib.image_width = childInt(left, "ImageWidth");
    calib.image_height = childInt(left, "ImageHeight");
    calib.baseline_mm = childDouble(stereo, "Baseline");

    if (calib.image_width <= 0 || calib.image_height <= 0)
    {
        error = "calibration xml has invalid image size";
        return false;
    }

    if (!copyDoubles(matrixValues(left, "CameraMatrix"), calib.left_K, 9) ||
        !copyDoubles(matrixValues(left, "DistCoeffs"), calib.left_D, 5) ||
        !copyDoubles(matrixValues(right, "CameraMatrix"), calib.right_K, 9) ||
        !copyDoubles(matrixValues(right, "DistCoeffs"), calib.right_D, 5) ||
        !copyDoubles(matrixValues(stereo, "R"), calib.R, 9) ||
        !copyDoubles(matrixValues(stereo, "T"), calib.T, 3))
    {
        error = "calibration xml has incomplete matrix data";
        return false;
    }
    return true;
}

bool readFileBytes(const QString& path, QByteArray& bytes, std::string& error)
{
    QFile file(path);
    if (!file.open(QIODevice::ReadOnly))
    {
        error = "open model failed: " + file.errorString().toStdString();
        return false;
    }

    bytes = file.readAll();
    if (bytes.isEmpty())
    {
        error = "model file is empty";
        return false;
    }
    return true;
}

bool yamlBool(const QHash<QString, QString>& values, const QString& key, bool fallback)
{
    const QString value = values.value(key).trimmed().toLower();
    if (value == "true" || value == "1" || value == "yes")
        return true;
    if (value == "false" || value == "0" || value == "no")
        return false;
    return fallback;
}

double yamlDouble(const QHash<QString, QString>& values, const QString& key, double fallback)
{
    bool ok = false;
    const double value = values.value(key).toDouble(&ok);
    return ok ? value : fallback;
}

int yamlInt(const QHash<QString, QString>& values, const QString& key, int fallback)
{
    bool ok = false;
    const int value = values.value(key).toInt(&ok);
    return ok ? value : fallback;
}

void fillBaseResult(const DroneScrewInputImage& leftImage, DroneScrewResult& result)
{
    result.frameId = leftImage.frameId;
    result.timestampUs = leftImage.timestampUs;
    result.imageWidth = leftImage.width;
    result.imageHeight = leftImage.height;
    result.boxes.clear();
    result.distances.clear();
}

bool validateInput(const DroneScrewInputImage& leftImage,
                   const DroneScrewInputImage& rightImage,
                   std::string& message)
{
    if (!leftImage.data || !rightImage.data)
    {
        message = "stereo_bolt input invalid: null image buffer";
        return false;
    }
    if (leftImage.width <= 0 || leftImage.height <= 0 ||
        rightImage.width <= 0 || rightImage.height <= 0)
    {
        message = "stereo_bolt input invalid: empty image size";
        return false;
    }
    if (leftImage.width != rightImage.width || leftImage.height != rightImage.height)
    {
        std::ostringstream oss;
        oss << "stereo_bolt input invalid: left/right size mismatch left="
            << leftImage.width << "x" << leftImage.height
            << " right=" << rightImage.width << "x" << rightImage.height;
        message = oss.str();
        return false;
    }
    if (leftImage.pixelFormat != 0 || rightImage.pixelFormat != 0)
    {
        std::ostringstream oss;
        oss << "stereo_bolt input invalid: expected Mono8 pixelFormat=0, got left="
            << leftImage.pixelFormat << " right=" << rightImage.pixelFormat;
        message = oss.str();
        return false;
    }
    if ((leftImage.stride > 0 && leftImage.stride < leftImage.width) ||
        (rightImage.stride > 0 && rightImage.stride < rightImage.width))
    {
        message = "stereo_bolt input invalid: stride is smaller than width";
        return false;
    }
    return true;
}

void appendDistance(const StereoBoltModuleDistanceC& distance, DroneScrewResult& result)
{
    DroneScrewDistance d;
    d.fromId = distance.bolt_id_i;
    d.toId = distance.bolt_id_j;
    d.distanceMm = static_cast<float>(distance.distance_mm);
    result.distances.push_back(d);
}

void appendRangeDistance(int index,
                         int confidence,
                         double distanceMm,
                         DroneScrewResult& result)
{
    DroneScrewDistance d;
    d.fromId = index;
    d.toId = confidence;
    d.distanceMm = static_cast<float>(distanceMm);
    result.distances.push_back(d);
}

void appendRangeBox(const StereoBoltRangeBoltC& bolt, DroneScrewResult& result)
{
    constexpr int kMarkerSize = 12;
    DroneScrewBox box;
    box.classId = bolt.confidence;
    box.score = static_cast<float>(bolt.ncc_score);
    box.x = bolt.left_x - kMarkerSize / 2;
    box.y = bolt.left_y - kMarkerSize / 2;
    box.width = kMarkerSize;
    box.height = kMarkerSize;
    result.boxes.push_back(box);
}

std::string buildSuccessMessage(const StereoBoltModuleResultC& out, int expectedCount)
{
    std::ostringstream oss;
    oss << "stereo_bolt ok: bolts=" << out.data.n_bolts
        << " expected=" << expectedCount
        << " plane_rms_mm=" << out.data.ground_plane.rms_mm;

    if (out.data.n_bolts > 0 && out.data.bolts)
    {
        oss << " heights_mm=[";
        const int n = std::min(out.data.n_bolts, 16);
        for (int i = 0; i < n; ++i)
        {
            if (i > 0) oss << ",";
            oss << out.data.bolts[i].height_mm;
        }
        if (out.data.n_bolts > n) oss << ",...";
        oss << "]";
    }

    if (out.data.n_adjacent_distances > 0 && out.data.adjacent_distances)
    {
        oss << " adjacent_mm=[";
        const int n = std::min(out.data.n_adjacent_distances, 16);
        for (int i = 0; i < n; ++i)
        {
            if (i > 0) oss << ",";
            const auto& d = out.data.adjacent_distances[i];
            oss << d.bolt_id_i << "-" << d.bolt_id_j << ":" << d.distance_mm;
        }
        if (out.data.n_adjacent_distances > n) oss << ",...";
        oss << "]";
    }

    return oss.str();
}

std::string buildFailureMessage(const StereoBoltModuleResultC& out)
{
    std::ostringstream oss;
    oss << "stereo_bolt rejected: reason=" << out.failure.reason
        << " left_rois=" << out.failure.n_left_rois
        << " right_rois=" << out.failure.n_right_rois
        << " pairs=" << out.failure.n_pairs;
    return oss.str();
}

std::string buildRangeMessage(const StereoBoltRangeSummaryC& summary,
                              const StereoBoltRangeBoltC* bolts,
                              int writtenBolts)
{
    std::ostringstream oss;
    oss << "stereo_bolt range: found=" << summary.found
        << " status=" << summary.status
        << " median_mm=" << summary.distance_mm
        << " bolt_count=" << summary.bolt_count;
    if (writtenBolts > 0 && bolts)
    {
        oss << " bolts_mm=[";
        const int n = std::min(writtenBolts, 16);
        for (int i = 0; i < n; ++i)
        {
            if (i > 0) oss << ",";
            oss << bolts[i].distance_mm
                << "/ncc=" << bolts[i].ncc_score
                << "/lrc=" << bolts[i].lrc_px;
        }
        if (writtenBolts > n) oss << ",...";
        oss << "]";
    }
    return oss.str();
}
}

DroneScrewAlgoStub::DroneScrewAlgoStub() = default;

DroneScrewAlgoStub::~DroneScrewAlgoStub()
{
    UnInit();
}

bool DroneScrewAlgoStub::buildRectifiedLeftToRawMap(const StereoBoltCalibC& calib,
                                                    double rectAlpha,
                                                    std::string& error)
{
    m_rectMapWidth = 0;
    m_rectMapHeight = 0;
    m_leftRectToRawX.clear();
    m_leftRectToRawY.clear();

    if (calib.image_width <= 0 || calib.image_height <= 0)
    {
        error = "invalid calibration image size for rectified-to-raw map";
        return false;
    }
    if (!std::isfinite(rectAlpha))
    {
        error = "invalid rectification alpha for rectified-to-raw map";
        return false;
    }

    try
    {
        const cv::Size imageSize(calib.image_width, calib.image_height);
        const cv::Mat K1 = mat64(3, 3, calib.left_K);
        const cv::Mat D1 = mat64(1, 5, calib.left_D);
        const cv::Mat K2 = mat64(3, 3, calib.right_K);
        const cv::Mat D2 = mat64(1, 5, calib.right_D);
        const cv::Mat R = mat64(3, 3, calib.R);
        const cv::Mat T = mat64(3, 1, calib.T);

        cv::Mat R1;
        cv::Mat R2;
        cv::Mat P1;
        cv::Mat P2;
        cv::Mat Q;
        cv::Rect validRoi1;
        cv::Rect validRoi2;
        cv::stereoRectify(K1, D1, K2, D2, imageSize, R, T,
                          R1, R2, P1, P2, Q,
                          cv::CALIB_ZERO_DISPARITY,
                          rectAlpha,
                          imageSize,
                          &validRoi1,
                          &validRoi2);

        cv::Mat mapX;
        cv::Mat mapY;
        cv::initUndistortRectifyMap(K1, D1, R1, P1, imageSize,
                                    CV_32FC1, mapX, mapY);

        if (mapX.empty() || mapY.empty() ||
            mapX.cols != imageSize.width || mapX.rows != imageSize.height ||
            mapY.cols != imageSize.width || mapY.rows != imageSize.height)
        {
            error = "OpenCV returned invalid rectified-to-raw map";
            return false;
        }

        const size_t total = static_cast<size_t>(imageSize.width) *
                             static_cast<size_t>(imageSize.height);
        m_leftRectToRawX.resize(total);
        m_leftRectToRawY.resize(total);
        for (int y = 0; y < imageSize.height; ++y)
        {
            const float* srcX = mapX.ptr<float>(y);
            const float* srcY = mapY.ptr<float>(y);
            const size_t offset = static_cast<size_t>(y) * imageSize.width;
            std::copy(srcX, srcX + imageSize.width, m_leftRectToRawX.begin() + offset);
            std::copy(srcY, srcY + imageSize.width, m_leftRectToRawY.begin() + offset);
        }

        m_calib = calib;
        m_rectAlpha = rectAlpha;
        m_rectMapWidth = imageSize.width;
        m_rectMapHeight = imageSize.height;
        return true;
    }
    catch (const cv::Exception& e)
    {
        error = std::string("OpenCV rectified-to-raw map failed: ") + e.what();
    }
    catch (const std::exception& e)
    {
        error = std::string("rectified-to-raw map failed: ") + e.what();
    }
    catch (...)
    {
        error = "rectified-to-raw map failed: unknown exception";
    }

    m_rectMapWidth = 0;
    m_rectMapHeight = 0;
    m_leftRectToRawX.clear();
    m_leftRectToRawY.clear();
    return false;
}

bool DroneScrewAlgoStub::mapRectifiedLeftRoi(const StereoBoltModuleRoiC& roi,
                                             int rawWidth,
                                             int rawHeight,
                                             DroneScrewBox& box) const
{
    if (roi.width <= 0 || roi.height <= 0)
        return false;
    if (m_rectMapWidth <= 0 || m_rectMapHeight <= 0 ||
        m_leftRectToRawX.empty() || m_leftRectToRawY.empty())
    {
        return false;
    }

    const int dstWidth = rawWidth > 0 ? rawWidth : m_rectMapWidth;
    const int dstHeight = rawHeight > 0 ? rawHeight : m_rectMapHeight;
    if (dstWidth <= 0 || dstHeight <= 0)
        return false;

    const double rectLeft = static_cast<double>(roi.x);
    const double rectTop = static_cast<double>(roi.y);
    const double rectRight = static_cast<double>(roi.x) + static_cast<double>(roi.width) - 1.0;
    const double rectBottom = static_cast<double>(roi.y) + static_cast<double>(roi.height) - 1.0;
    if (rectRight < 0.0 || rectBottom < 0.0 ||
        rectLeft > static_cast<double>(m_rectMapWidth - 1) ||
        rectTop > static_cast<double>(m_rectMapHeight - 1))
    {
        return false;
    }

    double minRawX = std::numeric_limits<double>::infinity();
    double minRawY = std::numeric_limits<double>::infinity();
    double maxRawX = -std::numeric_limits<double>::infinity();
    double maxRawY = -std::numeric_limits<double>::infinity();
    bool hasSample = false;

    const int samplesX = roiSampleCount(rectRight - rectLeft);
    const int samplesY = roiSampleCount(rectBottom - rectTop);
    for (int sy = 0; sy < samplesY; ++sy)
    {
        const double ty = (samplesY <= 1) ? 0.0 : static_cast<double>(sy) / (samplesY - 1);
        const double rectY = rectTop + (rectBottom - rectTop) * ty;
        for (int sx = 0; sx < samplesX; ++sx)
        {
            const double tx = (samplesX <= 1) ? 0.0 : static_cast<double>(sx) / (samplesX - 1);
            const double rectX = rectLeft + (rectRight - rectLeft) * tx;

            double rawX = 0.0;
            double rawY = 0.0;
            if (!sampleMapBilinear(m_leftRectToRawX, m_leftRectToRawY,
                                   m_rectMapWidth, m_rectMapHeight,
                                   rectX, rectY, rawX, rawY))
            {
                continue;
            }

            minRawX = std::min(minRawX, rawX);
            minRawY = std::min(minRawY, rawY);
            maxRawX = std::max(maxRawX, rawX);
            maxRawY = std::max(maxRawY, rawY);
            hasSample = true;
        }
    }

    if (!hasSample)
        return false;
    if (maxRawX < 0.0 || maxRawY < 0.0 ||
        minRawX > static_cast<double>(dstWidth - 1) ||
        minRawY > static_cast<double>(dstHeight - 1))
    {
        return false;
    }

    const int left = std::max(0, std::min(dstWidth - 1,
        static_cast<int>(std::floor(minRawX))));
    const int top = std::max(0, std::min(dstHeight - 1,
        static_cast<int>(std::floor(minRawY))));
    const int right = std::max(0, std::min(dstWidth - 1,
        static_cast<int>(std::ceil(maxRawX))));
    const int bottom = std::max(0, std::min(dstHeight - 1,
        static_cast<int>(std::ceil(maxRawY))));
    if (right < left || bottom < top)
        return false;

    box.classId = roi.class_id;
    box.score = static_cast<float>(roi.score);
    box.x = left;
    box.y = top;
    box.width = right - left + 1;
    box.height = bottom - top + 1;
    return true;
}

void DroneScrewAlgoStub::appendRoiBox(const StereoBoltModuleRoiC& roi,
                                      DroneScrewResult& result) const
{
    DroneScrewBox box;
    if (mapRectifiedLeftRoi(roi, result.imageWidth, result.imageHeight, box))
        result.boxes.push_back(box);
}

void DroneScrewAlgoStub::appendBoltBox(const StereoBoltModuleBoltC& bolt,
                                       DroneScrewResult& result) const
{
    DroneScrewBox box;
    if (mapRectifiedLeftRoi(bolt.left_roi, result.imageWidth, result.imageHeight, box))
    {
        box.hasPhysicalHeight = true;
        box.physicalHeightMm = static_cast<float>(bolt.height_mm);
        result.boxes.push_back(box);
    }
}

int DroneScrewAlgoStub::Init(const DroneScrewAlgoParams& params)
{
    std::lock_guard<std::mutex> lk(m_mutex);
    m_params = params;
    m_initError.clear();
    m_bInited = false;

    if (m_ctx)
    {
        sb_destroy(m_ctx);
        m_ctx = nullptr;
    }
    m_rectMapWidth = 0;
    m_rectMapHeight = 0;
    m_leftRectToRawX.clear();
    m_leftRectToRawY.clear();

    QStringList configBases;
    appendUniqueDir(configBases, QDir::currentPath());
    appendUniqueDir(configBases, QCoreApplication::applicationDirPath());

    const QString configInput = QString::fromStdString(
        m_params.modelPath.empty() ? std::string(kDefaultConfigPath) : m_params.modelPath);
    const QString configPath = resolvePath(configInput, configBases);
    const QHash<QString, QString> yaml = readYamlValues(configPath);

    QStringList assetBases = configBases;
    const QFileInfo configInfo(configPath);
    appendUniqueDir(assetBases, configInfo.absolutePath());
    appendUniqueDir(assetBases, QDir(configInfo.absolutePath()).absoluteFilePath(".."));

    const QString calibRel = yaml.value("calibration.xml_path", kDefaultCalibPath);
    const QString modelRel = yaml.value("yolo.model_path", kDefaultModelPath);
    const QString calibPath = resolvePath(calibRel, assetBases);
    const QString modelPath = resolvePath(modelRel, assetBases);

    StereoBoltCalibC calib{};
    std::string loadError;
    if (!loadCalibration(calibPath, calib, loadError))
    {
        m_initError = std::string("stereo_bolt load calibration failed xml=") +
                      calibPath.toStdString() + " err=" + loadError;
        LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
        return ERR_CODE(DEV_OPEN_ERR);
    }

    QByteArray modelBytes;
    if (!readFileBytes(modelPath, modelBytes, loadError))
    {
        m_initError = std::string("stereo_bolt load model failed model=") +
                      modelPath.toStdString() + " err=" + loadError;
        LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
        return ERR_CODE(DEV_OPEN_ERR);
    }

    StereoBoltModelC model{};
    model.data = reinterpret_cast<const uint8_t*>(modelBytes.constData());
    model.size = static_cast<size_t>(modelBytes.size());
    const QString backend = yaml.value("yolo.backend", "rknn").trimmed().toLower();
    model.backend = (backend == "opencv" || backend == "opencv_dnn")
                        ? SB_YOLO_BACKEND_OPENCV
                        : (backend == "auto" ? SB_YOLO_BACKEND_AUTO : SB_YOLO_BACKEND_RKNN);
    model.imgsz = yamlInt(yaml, "yolo.imgsz", kDefaultYoloImgSize);
    model.conf = m_params.scoreThreshold > 0.0f
                     ? m_params.scoreThreshold
                     : static_cast<float>(yamlDouble(yaml, "yolo.conf", 0.5));
    model.iou = m_params.nmsThreshold > 0.0f ? m_params.nmsThreshold : 0.45f;

    StereoBoltParamsC algoParams = sb_default_params();
    algoParams.interpolation =
        yaml.value("rectification.interpolation", "cubic").trimmed().toLower() == "linear" ? 0 : 1;
    algoParams.rect_alpha = yamlDouble(yaml, "rectification.alpha", algoParams.rect_alpha);
    algoParams.wd_z_min_mm = yamlDouble(yaml, "working_distance.z_min_mm",
                                        yamlDouble(yaml, "pointcloud.z_min_mm", 1200.0));
    algoParams.wd_z_max_mm = yamlDouble(yaml, "working_distance.z_max_mm",
                                        yamlDouble(yaml, "pointcloud.z_max_mm", 2800.0));
    algoParams.sgbm_min_disparity =
        yamlDouble(yaml, "sgbm.min_disparity", algoParams.sgbm_min_disparity);
    algoParams.sgbm_num_disparities =
        yamlDouble(yaml, "sgbm.num_disparities", algoParams.sgbm_num_disparities);
    algoParams.foot_rim_correction =
        yamlBool(yaml, "measurement.foot_rim_correction", false) ? 1 : 0;
    algoParams.rng_z_min_mm = yamlDouble(yaml, "ranging.z_min_mm", algoParams.rng_z_min_mm);
    algoParams.rng_z_max_mm = yamlDouble(yaml, "ranging.z_max_mm", algoParams.rng_z_max_mm);
    algoParams.rng_lrc_max_mm = yamlDouble(yaml, "ranging.lrc_max_mm", algoParams.rng_lrc_max_mm);
    algoParams.rng_lrc_trusted_mm =
        yamlDouble(yaml, "ranging.lrc_trusted_mm", algoParams.rng_lrc_trusted_mm);

    if (!buildRectifiedLeftToRawMap(calib, algoParams.rect_alpha, loadError))
    {
        m_initError = std::string("stereo_bolt build bbox rectified-to-raw map failed calib=") +
                      calibPath.toStdString() + " err=" + loadError;
        LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
        return ERR_CODE(DEV_OPEN_ERR);
    }

    m_ctx = sb_create_ex(&calib, &model, &algoParams);

    if (!m_ctx)
    {
        m_rectMapWidth = 0;
        m_rectMapHeight = 0;
        m_leftRectToRawX.clear();
        m_leftRectToRawY.clear();
        const char* err = sb_last_error(nullptr);
        m_initError = std::string("stereo_bolt create_ex failed config=") +
                      configPath.toStdString() +
                      " calib=" + calibPath.toStdString() +
                      " model=" + modelPath.toStdString() +
                      " err=" + (err ? err : "unknown");
        LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
        return ERR_CODE(DEV_OPEN_ERR);
    }

    m_configPath = configPath.toStdString();
    m_calibPath = calibPath.toStdString();
    m_modelPath = modelPath.toStdString();
    m_bInited = true;

    LOG_INFO("[ALGO] stereo_bolt init ok create_ex config=%s calib=%s model=%s imgsz=%d wd=%.0f..%.0f rng=%.0f..%.0f expectedBoltCount=%d\n",
             m_configPath.c_str(),
             m_calibPath.c_str(),
             m_modelPath.c_str(),
             model.imgsz,
             algoParams.wd_z_min_mm,
             algoParams.wd_z_max_mm,
             algoParams.rng_z_min_mm,
             algoParams.rng_z_max_mm,
             m_params.expectedBoltCount > 0 ? m_params.expectedBoltCount : kDefaultExpectedBoltCount);
    return 0;
}

int DroneScrewAlgoStub::UnInit()
{
    std::lock_guard<std::mutex> lk(m_mutex);
    m_bInited = false;

    if (m_ctx)
    {
        sb_destroy(m_ctx);
        m_ctx = nullptr;
    }
    m_rectMapWidth = 0;
    m_rectMapHeight = 0;
    m_leftRectToRawX.clear();
    m_leftRectToRawY.clear();

    return 0;
}

int DroneScrewAlgoStub::Detect(const DroneScrewInputImage& leftImage,
                               const DroneScrewInputImage& rightImage,
                               DroneScrewResult& result)
{
    fillBaseResult(leftImage, result);

    std::lock_guard<std::mutex> lk(m_mutex);
    if (!m_bInited.load() || !m_ctx)
    {
        result.success = false;
        result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
        result.message = m_initError.empty()
                             ? "stereo_bolt not initialized"
                             : ("stereo_bolt not initialized: " + m_initError);
        return result.errorCode;
    }

    std::string validationMessage;
    if (!validateInput(leftImage, rightImage, validationMessage))
    {
        result.success = false;
        result.errorCode = ERR_CODE(DATA_ERR_INVALID);
        result.message = validationMessage;
        return result.errorCode;
    }

    StereoBoltModuleResultC out{};
    const int expectedCount =
        m_params.expectedBoltCount > 0 ? m_params.expectedBoltCount : kDefaultExpectedBoltCount;
    const int leftStride =
        (leftImage.stride == leftImage.width) ? 0 : leftImage.stride;
    const int rightStride =
        (rightImage.stride == rightImage.width) ? 0 : rightImage.stride;

    const sb_status_t st = sb_process_bolt_module_buffers(
        m_ctx,
        leftImage.data, leftImage.width, leftImage.height, leftStride,
        rightImage.data, rightImage.width, rightImage.height, rightStride,
        1,
        expectedCount,
        &out);

    if (st == SB_OK && out.success)
    {
        result.success = true;
        result.errorCode = 0;
        if (out.data.n_bolts > 0 && out.data.bolts)
        {
            for (int i = 0; i < out.data.n_bolts; ++i)
                appendBoltBox(out.data.bolts[i], result);
        }
        if (out.data.n_adjacent_distances > 0 && out.data.adjacent_distances)
        {
            for (int i = 0; i < out.data.n_adjacent_distances; ++i)
                appendDistance(out.data.adjacent_distances[i], result);
        }
        result.message = buildSuccessMessage(out, expectedCount);
        sb_free_bolt_module_result(&out);
        return 0;
    }

    if (st == SB_OK || st == SB_ERR_ALGORITHM_REJECTED)
    {
        result.success = false;
        result.errorCode = static_cast<int>(st == SB_OK ? SB_ERR_ALGORITHM_REJECTED : st);
        if (out.failure.n_left_rois > 0 && out.failure.left_rois)
        {
            for (int i = 0; i < out.failure.n_left_rois; ++i)
                appendRoiBox(out.failure.left_rois[i], result);
        }
        result.message = buildFailureMessage(out);
        sb_free_bolt_module_result(&out);
        return 0;
    }

    const char* err = sb_last_error(m_ctx);
    result.success = false;
    result.errorCode = static_cast<int>(st);
    result.message = std::string("stereo_bolt process failed: status=") +
                     std::to_string(static_cast<int>(st)) +
                     " err=" + (err ? err : "unknown");
    sb_free_bolt_module_result(&out);
    return result.errorCode;
}

int DroneScrewAlgoStub::DetectDistance(const DroneScrewInputImage& leftImage,
                                       const DroneScrewInputImage& rightImage,
                                       DroneScrewResult& result)
{
    fillBaseResult(leftImage, result);

    std::lock_guard<std::mutex> lk(m_mutex);
    if (!m_bInited.load() || !m_ctx)
    {
        result.success = false;
        result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
        result.message = m_initError.empty()
                             ? "stereo_bolt not initialized"
                             : ("stereo_bolt not initialized: " + m_initError);
        return result.errorCode;
    }

    std::string validationMessage;
    if (!validateInput(leftImage, rightImage, validationMessage))
    {
        result.success = false;
        result.errorCode = ERR_CODE(DATA_ERR_INVALID);
        result.message = validationMessage;
        return result.errorCode;
    }

    StereoBoltRangeSummaryC summary{};
    StereoBoltRangeBoltC bolts[kMaxRangeBolts]{};
    const int leftStride =
        (leftImage.stride == leftImage.width) ? 0 : leftImage.stride;
    const int rightStride =
        (rightImage.stride == rightImage.width) ? 0 : rightImage.stride;

    const sb_status_t st = sb_range_bolt_binned(
        m_ctx,
        leftImage.data, leftImage.width, leftImage.height, leftStride,
        rightImage.data, rightImage.width, rightImage.height, rightStride,
        1,
        &summary,
        bolts,
        kMaxRangeBolts);

    if (st != SB_OK)
    {
        const char* err = sb_last_error(m_ctx);
        result.success = false;
        result.errorCode = static_cast<int>(st);
        result.message = std::string("stereo_bolt range failed: status=") +
                         std::to_string(static_cast<int>(st)) +
                         " err=" + (err ? err : "unknown");
        return result.errorCode;
    }

    result.success = true;
    result.errorCode = 0;
    result.message = buildRangeMessage(summary, bolts,
                                       std::min(summary.bolt_count, kMaxRangeBolts));

    if (summary.found)
        appendRangeDistance(-1, 0, summary.distance_mm, result);

    const int writtenBolts = std::min(summary.bolt_count, kMaxRangeBolts);
    for (int i = 0; i < writtenBolts; ++i)
    {
        appendRangeDistance(i, bolts[i].confidence, bolts[i].distance_mm, result);
        appendRangeBox(bolts[i], result);
    }

    return 0;
}

int DroneScrewAlgoStub::UpdateParams(const DroneScrewAlgoParams& params)
{
    {
        std::lock_guard<std::mutex> lk(m_mutex);
        const bool needReinit =
            params.modelPath != m_params.modelPath ||
            std::fabs(params.scoreThreshold - m_params.scoreThreshold) > 0.0001f ||
            std::fabs(params.nmsThreshold - m_params.nmsThreshold) > 0.0001f ||
            params.inputWidth != m_params.inputWidth ||
            params.inputHeight != m_params.inputHeight ||
            params.modelType != m_params.modelType;
        const bool wasInited = m_bInited.load();

        if (wasInited && !needReinit)
        {
            m_params = params;
            return 0;
        }
    }

    return Init(params);
}

std::string DroneScrewAlgoStub::GetVersion() const
{
    std::lock_guard<std::mutex> lk(m_mutex);
    if (m_bInited.load())
        return std::string("StereoBoltDelivery C API direct link, create_ex config=") +
               m_configPath + " calib=" + m_calibPath + " model=" + m_modelPath;

    if (!m_initError.empty())
        return std::string("StereoBoltDelivery C API direct link not initialized: ") + m_initError;

    return "StereoBoltDelivery C API direct link";
}

std::unique_ptr<IDroneScrewAlgo> IDroneScrewAlgo::CreateDefault()
{
    return std::unique_ptr<IDroneScrewAlgo>(new DroneScrewAlgoStub());
}

#else

DroneScrewAlgoStub::DroneScrewAlgoStub() = default;

DroneScrewAlgoStub::~DroneScrewAlgoStub()
{
    UnInit();
}

int DroneScrewAlgoStub::Init(const DroneScrewAlgoParams& params)
{
    std::lock_guard<std::mutex> lk(m_mutex);
    m_params = params;
    m_ctx = nullptr;
    m_bInited = false;
    m_initError = "stereo_bolt_delivery is only linked in Linux/ARM builds";
    LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
    return ERR_CODE(DEV_UNSUPPORT);
}

int DroneScrewAlgoStub::UnInit()
{
    std::lock_guard<std::mutex> lk(m_mutex);
    m_ctx = nullptr;
    m_bInited = false;
    return 0;
}

int DroneScrewAlgoStub::Detect(const DroneScrewInputImage& leftImage,
                               const DroneScrewInputImage&,
                               DroneScrewResult& result)
{
    result.frameId = leftImage.frameId;
    result.timestampUs = leftImage.timestampUs;
    result.imageWidth = leftImage.width;
    result.imageHeight = leftImage.height;
    result.boxes.clear();
    result.distances.clear();
    result.success = false;
    result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
    result.message = "stereo_bolt_delivery is not available in this build";
    return result.errorCode;
}

int DroneScrewAlgoStub::DetectDistance(const DroneScrewInputImage& leftImage,
                                       const DroneScrewInputImage&,
                                       DroneScrewResult& result)
{
    result.frameId = leftImage.frameId;
    result.timestampUs = leftImage.timestampUs;
    result.imageWidth = leftImage.width;
    result.imageHeight = leftImage.height;
    result.boxes.clear();
    result.distances.clear();
    result.success = false;
    result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
    result.message = "stereo_bolt_delivery is not available in this build";
    return result.errorCode;
}

int DroneScrewAlgoStub::UpdateParams(const DroneScrewAlgoParams& params)
{
    std::lock_guard<std::mutex> lk(m_mutex);
    m_params = params;
    return 0;
}

std::string DroneScrewAlgoStub::GetVersion() const
{
    return "StereoBoltDelivery unavailable: not a Linux/ARM direct-link build";
}

std::unique_ptr<IDroneScrewAlgo> IDroneScrewAlgo::CreateDefault()
{
    return std::unique_ptr<IDroneScrewAlgo>(new DroneScrewAlgoStub());
}

#endif