#include "pose_engine.hpp"
#include "common.hpp"
#include "gpu_preprocess.hpp"

#include <opencv2/imgproc.hpp>

#include <algorithm>
#include <chrono>
#include <cmath>
#include <numeric>
#include <set>
#include <tuple>

namespace fangjia {
namespace {

constexpr int kInputWidth = 288;
constexpr int kInputHeight = 384;
constexpr int kKeypoints = 133;
constexpr int kSimccX = 576;
constexpr int kSimccY = 768;

__global__ void poseNormalizeKernel(
    float* output, std::uint8_t const* input, int people, int inputStep) {
    int index = blockIdx.x * blockDim.x + threadIdx.x;
    int pixels = kInputWidth * kInputHeight;
    int total = people * pixels;
    if (index >= total) return;
    int person = index / pixels;
    int pixel = index - person * pixels;
    int y = pixel / kInputWidth;
    int x = pixel - y * kInputWidth;
    auto const* value = input + person * inputStep * kInputHeight + y * inputStep + x * 3;
    float* destination = output + static_cast<std::size_t>(person) * 3 * pixels;
    destination[pixel] = static_cast<float>((static_cast<double>(value[0]) - 123.675) / 58.395);
    destination[pixels + pixel] = static_cast<float>((static_cast<double>(value[1]) - 116.28) / 57.12);
    destination[2 * pixels + pixel] = static_cast<float>((static_cast<double>(value[2]) - 103.53) / 57.375);
}

float iou(Box const& a, Box const& b) {
    float x1 = std::max(a.x1, b.x1);
    float y1 = std::max(a.y1, b.y1);
    float x2 = std::min(a.x2, b.x2);
    float y2 = std::min(a.y2, b.y2);
    float intersection = std::max(x2 - x1, 0.0F) * std::max(y2 - y1, 0.0F);
    float areaA = std::max(a.x2 - a.x1, 0.0F) * std::max(a.y2 - a.y1, 0.0F);
    float areaB = std::max(b.x2 - b.x1, 0.0F) * std::max(b.y2 - b.y1, 0.0F);
    return intersection / std::max(areaA + areaB - intersection, 1.0F);
}

float distance(float x1, float y1, float x2, float y2) {
    return std::hypot(x1 - x2, y1 - y2);
}

} // namespace

std::vector<int> BoxTracker::update(
    std::vector<Box>& boxes, int frameIndex, int width, int height) {
    // Tracks older than Python's max_age can never be matched again.
    // Reclaim them to bound memory during continuous camera operation.
    for(auto it=tracks_.begin();it!=tracks_.end();) {
        if(frameIndex-it->second.lastFrame>30)it=tracks_.erase(it);
        else ++it;
    }
    struct Pair { float score; int detection; int track; };
    std::vector<Pair> pairs;
    float imageDiagonal = std::hypot(static_cast<float>(width), static_cast<float>(height));
    for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
        float cx = 0.5F * (boxes[d].x1 + boxes[d].x2);
        float cy = 0.5F * (boxes[d].y1 + boxes[d].y2);
        for (auto const& [id, track] : tracks_) {
            if (frameIndex - track.lastFrame > 30) continue;
            float pcx = 0.5F * (track.box.x1 + track.box.x2);
            float pcy = 0.5F * (track.box.y1 + track.box.y2);
            float overlap = iou(boxes[d], track.box);
            float centerDistance = distance(cx, cy, pcx, pcy) / imageDiagonal;
            if (overlap >= 0.05F || centerDistance <= 0.18F) {
                pairs.push_back({2.0F * overlap + std::max(0.0F, 1.0F - centerDistance / 0.18F), d, id});
            }
        }
    }
    std::sort(pairs.begin(), pairs.end(), [](Pair const& a, Pair const& b) {
        if (a.score != b.score) return a.score > b.score;
        if (a.detection != b.detection) return a.detection > b.detection;
        return a.track > b.track;
    });
    std::vector<int> result(boxes.size(), 0);
    std::unordered_map<int, bool> assigned;
    for (auto const& pair : pairs) {
        if (result[pair.detection] || assigned[pair.track]) continue;
        result[pair.detection] = pair.track;
        assigned[pair.track] = true;
    }
    for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
        if (!result[d]) result[d] = nextId_++;
        boxes[d].trackId = result[d];
        tracks_[result[d]] = {boxes[d], frameIndex};
    }
    return result;
}

RtmwEngine::RtmwEngine(
    std::filesystem::path const& enginePath, TrtLogger& logger,
    int maxSourceWidth, int maxSourceHeight, int maxPeople)
    : engine_(enginePath, logger), maxSourceWidth_(maxSourceWidth),
      maxSourceHeight_(maxSourceHeight), maxPeople_(maxPeople),
      centersHost_(maxPeople * 2), scalesHost_(maxPeople * 2),
      simccXHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccX),
      simccYHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccY) {
    engine_.setInputShape("input", nvinfer1::Dims4{1,3,kInputHeight,kInputWidth});
    engine_.prepare();
    auto xs=engine_.shape("simcc_x"),ys=engine_.shape("simcc_y");
    if(xs.nbDims!=3||ys.nbDims!=3||xs.d[0]!=1||ys.d[0]!=1||xs.d[1]!=kKeypoints||ys.d[1]!=kKeypoints||xs.d[2]!=kSimccX||ys.d[2]!=kSimccY)
        throw std::runtime_error("RTMW engine I/O shape does not match the 133-point model");
    for(auto name:{"input","simcc_x","simcc_y"})if(engine_.dataType(name)!=nvinfer1::DataType::kFLOAT)
        throw std::runtime_error("RTMW requires float32 I/O bindings");
    std::size_t const values = static_cast<std::size_t>(maxPeople) * 3 * kInputWidth * kInputHeight;
    CUDA_CHECK(cudaMalloc(
        &cropsDevice_, static_cast<std::size_t>(maxPeople_) * kInputWidth * kInputHeight * 3));
    CUDA_CHECK(cudaMalloc(&networkDevice_, values * sizeof(float)));
    CUDA_CHECK(cudaMalloc(&sourceDevice_, static_cast<size_t>(maxSourceWidth_)*maxSourceHeight_*3));
}

RtmwEngine::~RtmwEngine() {
    if (sourceDevice_) cudaFree(sourceDevice_);
    if (cropsDevice_) cudaFree(cropsDevice_);
    if (networkDevice_) cudaFree(networkDevice_);
}

void RtmwEngine::warmup(cudaStream_t stream) {
    std::vector<Box> boxes{{0, 0, static_cast<float>(maxSourceWidth_), static_cast<float>(maxSourceHeight_), 1, 1}};
    cv::Mat black(maxSourceHeight_, maxSourceWidth_, CV_8UC3, cv::Scalar(0, 0, 0));
    for (int i = 0; i < 2; ++i) infer(black, boxes, stream);
}

std::vector<PersonPose> RtmwEngine::infer(
    cv::Mat const& frame, std::vector<Box> const& boxes, cudaStream_t stream,
    float* preprocessMs, float* inferenceMs, float* postprocessMs) {
    int batch = std::min(static_cast<int>(boxes.size()), maxPeople_);
    if (batch <= 0) return {};
    auto const preprocessStarted = std::chrono::steady_clock::now();
    int const cropStep = kInputWidth * 3;
    int const pixels = kInputWidth * kInputHeight;
    if (frame.cols>maxSourceWidth_ || frame.rows>maxSourceHeight_) throw std::runtime_error("Pose input exceeds allocation");
    auto npp = cms::nppContext(stream);
    CUDA_CHECK(cudaMemcpy2DAsync(sourceDevice_, frame.cols*3, frame.data, frame.step, frame.cols*3, frame.rows, cudaMemcpyHostToDevice, stream));
    for (int i = 0; i < batch; ++i) {
        auto const& box = boxes[i];
        double width = (static_cast<double>(box.x2) - static_cast<double>(box.x1)) * 1.25;
        double height = (static_cast<double>(box.y2) - static_cast<double>(box.y1)) * 1.25;
        constexpr double aspect = static_cast<double>(kInputWidth) / kInputHeight;
        if (width > height * aspect) height = width / aspect;
        else width = height * aspect;
        centersHost_[i * 2] = 0.5 * (static_cast<double>(box.x1) + static_cast<double>(box.x2));
        centersHost_[i * 2 + 1] = 0.5 * (static_cast<double>(box.y1) + static_cast<double>(box.y2));
        scalesHost_[i * 2] = width;
        scalesHost_[i * 2 + 1] = height;

        cv::Point2f source[3]{
            {static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1])},
            {static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)},
            {static_cast<float>(centersHost_[i * 2] - width * 0.5), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)}};
        cv::Point2f destination[3]{
            {kInputWidth * 0.5F, kInputHeight * 0.5F},
            {kInputWidth * 0.5F, kInputHeight * 0.5F - kInputWidth * 0.5F},
            {0.0F, kInputHeight * 0.5F - kInputWidth * 0.5F}};
        cv::Mat affine = cv::getAffineTransform(source, destination);
            auto* cropDevice=cropsDevice_+static_cast<size_t>(i)*cropStep*kInputHeight;
            Npp8u fill[3]={0,0,0};
            cms::nppCheck(nppiSet_8u_C3R_Ctx(fill,cropDevice,cropStep,{kInputWidth,kInputHeight},npp));
            double coeff[2][3];
            for(int row=0;row<2;++row) for(int col=0;col<3;++col) coeff[row][col]=affine.at<double>(row,col);
            cms::nppCheck(nppiWarpAffine_8u_C3R_Ctx(sourceDevice_,{frame.cols,frame.rows},frame.cols*3,{0,0,frame.cols,frame.rows},cropDevice,cropStep,{0,0,kInputWidth,kInputHeight},coeff,NPPI_INTER_LINEAR,npp));
    }
    int const totalPixels = batch * pixels;
    poseNormalizeKernel<<<(totalPixels + 255) / 256, 256, 0, stream>>>(
        networkDevice_, cropsDevice_, batch, cropStep);
    CUDA_CHECK(cudaGetLastError());
    CUDA_CHECK(cudaStreamSynchronize(stream));
    if (preprocessMs) {
        *preprocessMs = static_cast<float>(std::chrono::duration<double, std::milli>(
            std::chrono::steady_clock::now() - preprocessStarted).count());
    }

    // RTMPose's Python API invokes the ONNX/TensorRT session once per person.
    // Preserve that execution contract instead of batching people together.
    engine_.setInputShape("input", nvinfer1::Dims4{1, 3, kInputHeight, kInputWidth});
    engine_.prepare();
    CudaEventTimer timer;
    float inferTime = 0.0F;
    std::size_t const inputValues = static_cast<std::size_t>(3) * pixels;
    std::size_t const xValues = static_cast<std::size_t>(kKeypoints) * kSimccX;
    std::size_t const yValues = static_cast<std::size_t>(kKeypoints) * kSimccY;
    for (int person = 0; person < batch; ++person) {
        CUDA_CHECK(cudaMemcpyAsync(
            engine_.device("input"), networkDevice_ + static_cast<std::size_t>(person) * inputValues,
            inputValues * sizeof(float), cudaMemcpyDeviceToDevice, stream));
        timer.start(stream);
        if (!engine_.enqueue(stream)) throw std::runtime_error("RTMW enqueue failed");
        inferTime += timer.stop(stream);
        CUDA_CHECK(cudaMemcpyAsync(
            simccXHost_.data() + static_cast<std::size_t>(person) * xValues,
            engine_.device("simcc_x"), xValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
        CUDA_CHECK(cudaMemcpyAsync(
            simccYHost_.data() + static_cast<std::size_t>(person) * yValues,
            engine_.device("simcc_y"), yValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
    }
    if (inferenceMs) *inferenceMs = inferTime;

    auto const postprocessStarted = std::chrono::steady_clock::now();
    CUDA_CHECK(cudaStreamSynchronize(stream));
    std::vector<PersonPose> result(batch);
    for (int person = 0; person < batch; ++person) {
        result[person].box = boxes[person];
        for (int keypoint = 0; keypoint < kKeypoints; ++keypoint) {
            auto xBegin = simccXHost_.begin() + (person * kKeypoints + keypoint) * kSimccX;
            auto yBegin = simccYHost_.begin() + (person * kKeypoints + keypoint) * kSimccY;
            auto xIt = std::max_element(xBegin, xBegin + kSimccX);
            auto yIt = std::max_element(yBegin, yBegin + kSimccY);
            float score = 0.5F * (*xIt + *yIt);
            if(!std::isfinite(score))throw std::runtime_error("Non-finite RTMW output");
            float localX = score > 0.0F ? static_cast<float>(xIt - xBegin) * 0.5F : -0.5F;
            float localY = score > 0.0F ? static_cast<float>(yIt - yBegin) * 0.5F : -0.5F;
            double scaleX = scalesHost_[person * 2];
            double scaleY = scalesHost_[person * 2 + 1];
            result[person].keypoints[keypoint] = {
                static_cast<float>(static_cast<double>(localX) / kInputWidth * scaleX + centersHost_[person * 2] - scaleX * 0.5),
                static_cast<float>(static_cast<double>(localY) / kInputHeight * scaleY + centersHost_[person * 2 + 1] - scaleY * 0.5),
                score};
        }
    }
    float post = static_cast<float>(std::chrono::duration<double, std::milli>(
        std::chrono::steady_clock::now() - postprocessStarted).count());
    if (postprocessMs) *postprocessMs = post;
    return result;
}

PoseFrameResult PoseFusionTracker::update(
    std::vector<PersonPose> const& rawPeople,
    std::vector<PersonPose> const& isolatedPeople,
    int frameIndex) {
    for(auto it=states_.begin();it!=states_.end();) {
        if(it->second.lastFrame>=0&&frameIndex-it->second.lastFrame>30)it=states_.erase(it);
        else ++it;
    }
    struct SideMeta {
        bool limbConsensus{};
        bool handValid{};
        bool distanceEligible{};
        bool alarmEligible{};
        double handScore{};
        double linkToSelectedLimb{};
        std::string handSource;
        std::string handState;
    };
    struct FusedPerson {
        PersonPose pose;
        std::array<SideMeta, 2> meta;
    };
    struct Measurement {
        std::array<Vec2, 21> hand{};
        bool precise{};
        bool provisional{};
        double quality{};
        std::string source{"model"};
    };
    auto vec = [](Keypoint const& point) { return Vec2{point.x, point.y}; };
    auto add = [](Vec2 a, Vec2 b) { return Vec2{a.x + b.x, a.y + b.y}; };
    auto sub = [](Vec2 a, Vec2 b) { return Vec2{a.x - b.x, a.y - b.y}; };
    auto mul = [](Vec2 a, double value) { return Vec2{a.x * value, a.y * value}; };
    auto norm = [](Vec2 a) { return std::hypot(a.x, a.y); };
    auto median = [](std::vector<double> values) {
        if (values.empty()) return 0.0;
        std::sort(values.begin(), values.end());
        auto middle = values.size() / 2;
        return values.size() % 2 ? values[middle] : 0.5 * (values[middle - 1] + values[middle]);
    };
    auto percentile90 = [](std::vector<double> values) {
        if (values.empty()) return 0.0;
        std::sort(values.begin(), values.end());
        double position = 0.9 * (values.size() - 1);
        auto lower = static_cast<std::size_t>(position);
        auto upper = std::min(lower + 1, values.size() - 1);
        return values[lower] * (upper - position) + values[upper] * (position - lower);
    };
    auto boxDiagonal = [](Box const& box) {
        return std::max(std::hypot(box.x2 - box.x1, box.y2 - box.y1), 1.0F);
    };
    auto normalized = [](Keypoint const& point, Box const& box) {
        return Vec2{
            (point.x - box.x1) / std::max(box.x2 - box.x1, 1.0F),
            (point.y - box.y1) / std::max(box.y2 - box.y1, 1.0F)};
    };
    auto denormalized = [](Vec2 point, Box const& box) {
        return Vec2{
            point.x * std::max(box.x2 - box.x1, 1.0F) + box.x1,
            point.y * std::max(box.y2 - box.y1, 1.0F) + box.y1};
    };
    auto solveTwoBoneElbow = [&](Vec2 shoulder, Vec2 wrist, double upperLength,
                                 double forearmLength, Vec2 reference) {
        Vec2 direction = sub(wrist, shoulder);
        double length = norm(direction);
        if (length < 1e-6) return reference;
        Vec2 unit = mul(direction, 1.0 / length);
        if (length >= upperLength + forearmLength) return add(shoulder, mul(unit, upperLength));
        if (length <= std::abs(upperLength - forearmLength)) {
            double sign = upperLength >= forearmLength ? 1.0 : -1.0;
            return add(shoulder, mul(unit, upperLength * sign));
        }
        double along = (upperLength * upperLength - forearmLength * forearmLength + length * length) /
            (2.0 * length);
        double height = std::sqrt(std::max(upperLength * upperLength - along * along, 0.0));
        Vec2 base = add(shoulder, mul(unit, along));
        Vec2 perpendicular{-unit.y, unit.x};
        Vec2 first = add(base, mul(perpendicular, height));
        Vec2 second = sub(base, mul(perpendicular, height));
        return norm(sub(first, reference)) <= norm(sub(second, reference)) ? first : second;
    };
    struct Candidate {
        PersonPose const* pose{};
        std::string source;
        std::array<Vec2, 3> limb{};
        std::array<Vec2, 21> hand{};
        double limbScore{};
        double handScore{};
        double handSpan{};
        double wristLink{};
        bool limbValid{};
        bool handValid{};
    };
    auto buildCandidate = [&](PersonPose const& pose, int side, std::string source) {
        int shoulder = side == 0 ? 5 : 6;
        int elbow = side == 0 ? 7 : 8;
        int wrist = side == 0 ? 9 : 10;
        int handStart = side == 0 ? 91 : 112;
        Candidate candidate;
        candidate.pose = &pose;
        candidate.source = std::move(source);
        candidate.limb = {normalized(pose.keypoints[shoulder], pose.box),
                          normalized(pose.keypoints[elbow], pose.box),
                          normalized(pose.keypoints[wrist], pose.box)};
        double minX = pose.keypoints[handStart].x, maxX = minX;
        double minY = pose.keypoints[handStart].y, maxY = minY;
        for (int index = 0; index < 21; ++index) {
            candidate.hand[index] = normalized(pose.keypoints[handStart + index], pose.box);
            minX = std::min<double>(minX, pose.keypoints[handStart + index].x);
            maxX = std::max<double>(maxX, pose.keypoints[handStart + index].x);
            minY = std::min<double>(minY, pose.keypoints[handStart + index].y);
            maxY = std::max<double>(maxY, pose.keypoints[handStart + index].y);
            candidate.handScore += pose.keypoints[handStart + index].score;
        }
        candidate.handScore /= 21.0;
        candidate.limbScore = (pose.keypoints[shoulder].score + pose.keypoints[elbow].score + pose.keypoints[wrist].score) / 3.0;
        double upper = norm(sub(candidate.limb[0], candidate.limb[1]));
        double forearm = norm(sub(candidate.limb[1], candidate.limb[2]));
        candidate.handSpan = std::hypot(maxX - minX, maxY - minY) / boxDiagonal(pose.box);
        candidate.wristLink = distance(
            pose.keypoints[wrist].x, pose.keypoints[wrist].y,
            pose.keypoints[handStart].x, pose.keypoints[handStart].y) / boxDiagonal(pose.box);
        candidate.limbValid = upper >= 0.025 && upper <= 0.8 && forearm >= 0.025 && forearm <= 0.8;
        candidate.handValid = candidate.handSpan >= 0.008 && candidate.handSpan <= 0.35 && candidate.wristLink <= 0.12;
        return candidate;
    };
    auto robustDistance = [&](auto const& points, auto const& target, bool available) {
        if (!available) return 0.0;
        std::vector<double> distances;
        for (std::size_t index = 0; index < points.size(); ++index) {
            distances.push_back(norm(sub(points[index], target[index])));
        }
        return median(std::move(distances));
    };

    std::vector<FusedPerson> fused;
    fused.reserve(rawPeople.size());
    for (std::size_t personIndex = 0; personIndex < rawPeople.size(); ++personIndex) {
        auto const& raw = rawPeople[personIndex];
        auto const& isolated = personIndex < isolatedPeople.size() ? isolatedPeople[personIndex] : raw;
        auto& state = states_[raw.box.trackId];
        FusedPerson output;
        output.pose = raw;
        for (int side = 0; side < 2; ++side) {
            int shoulder = side == 0 ? 5 : 6;
            int elbow = side == 0 ? 7 : 8;
            int wrist = side == 0 ? 9 : 10;
            int handStart = side == 0 ? 91 : 112;
            auto& track = state.fusion[side];
            std::array<Candidate, 2> candidates{
                buildCandidate(raw, side, "raw"), buildCandidate(isolated, side, "isolated")};
            int limbChoice = 0;
            double bestLimbScore = -1e30;
            for (int choice = 0; choice < 2; ++choice) {
                std::array<Vec2, 3> target{};
                if (track.hasLimbPrevious) {
                    for (int i = 0; i < 3; ++i) target[i] = add(track.limbPrevious[i], track.hasLimbVelocity ? mul(track.limbVelocity[i], 0.65) : Vec2{});
                }
                double temporal = robustDistance(candidates[choice].limb, target, track.hasLimbPrevious);
                double score = candidates[choice].limbScore + (candidates[choice].limbValid ? 1.2 : -5.0)
                    + (track.limbSource == candidates[choice].source ? 0.35 : 0.0) - 7.0 * temporal;
                if (score > bestLimbScore) { bestLimbScore = score; limbChoice = choice; }
            }
            int handChoice = 0;
            double bestHandScore = -1e30;
            double chosenTemporal = 0.0;
            double chosenLink = 1.0;
            for (int choice = 0; choice < 2; ++choice) {
                std::array<Vec2, 21> target{};
                if (track.hasHandPrevious) {
                    for (int i = 0; i < 21; ++i) target[i] = add(track.handPrevious[i], track.hasHandVelocity ? mul(track.handVelocity[i], 0.65) : Vec2{});
                }
                double temporal = robustDistance(candidates[choice].hand, target, track.hasHandPrevious);
                auto const& limbPose = *candidates[limbChoice].pose;
                double link = distance(
                    candidates[choice].pose->keypoints[handStart].x,
                    candidates[choice].pose->keypoints[handStart].y,
                    limbPose.keypoints[wrist].x, limbPose.keypoints[wrist].y) / boxDiagonal(raw.box);
                double linkPenalty = 1.5 * std::min(std::max(link - 0.12, 0.0), 0.2);
                double score = candidates[choice].handScore + (candidates[choice].handValid ? 3.0 : -7.0)
                    + (track.handSource == candidates[choice].source ? 0.45 : 0.0)
                    + (candidates[choice].source == candidates[limbChoice].source ? 0.15 : 0.0)
                    - linkPenalty - 8.0 * std::min(temporal, 0.15);
                if (score > bestHandScore) {
                    bestHandScore = score;
                    handChoice = choice;
                    chosenTemporal = temporal;
                    chosenLink = link;
                }
            }
            double endpointAdvantage = candidates[handChoice].handScore - candidates[1 - handChoice].handScore;
            if (handChoice != limbChoice && candidates[handChoice].handValid && endpointAdvantage >= 0.6) {
                limbChoice = handChoice;
                // fuse_person() rewrites this link after its visible-endpoint
                // override.  Temporal recovery consumes the rewritten value.
                chosenLink = candidates[handChoice].wristLink;
            }
            auto const& limb = candidates[limbChoice];
            auto const& hand = candidates[handChoice];
            for (int index : {shoulder, elbow, wrist}) output.pose.keypoints[index] = limb.pose->keypoints[index];
            for (int index = 0; index < 21; ++index) output.pose.keypoints[handStart + index] = hand.pose->keypoints[handStart + index];
            std::vector<double> limbDistances, handDistances;
            for (int index = 0; index < 3; ++index) limbDistances.push_back(norm(sub(candidates[0].limb[index], candidates[1].limb[index])));
            for (int index = 0; index < 21; ++index) handDistances.push_back(norm(sub(candidates[0].hand[index], candidates[1].hand[index])));
            double handDisagreement = percentile90(handDistances);
            double scoreAdvantage = candidates[0].handScore - candidates[1].handScore;
            auto& meta = output.meta[side];
            meta.limbConsensus = median(limbDistances) <= 0.05;
            meta.handValid = hand.handValid;
            meta.distanceEligible = hand.handValid && (
                handDisagreement <= 0.06 ||
                (hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08));
            meta.alarmEligible = meta.distanceEligible || (hand.handValid && hand.source == "raw" && scoreAdvantage >= 0.6);
            meta.handScore = std::nearbyint(hand.handScore * 1e4) / 1e4;
            meta.linkToSelectedLimb = std::nearbyint(chosenLink * 1e5) / 1e5;
            meta.handSource = hand.source;
            if (!hand.handValid) meta.handState = "rejected";
            else if (handDisagreement <= 0.06) meta.handState = "consensus";
            else if (hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08) meta.handState = "visible_endpoint";
            else if (hand.source == "isolated") meta.handState = "inferred";
            else meta.handState = "conflict";
            if (track.hasLimbPrevious) {
                for (int i = 0; i < 3; ++i) {
                    auto delta = sub(limb.limb[i], track.limbPrevious[i]);
                    track.limbVelocity[i] = track.hasLimbVelocity ? add(mul(track.limbVelocity[i], 0.65), mul(delta, 0.35)) : delta;
                }
                track.hasLimbVelocity = true;
            }
            if (hand.handValid) {
                if (track.hasHandPrevious) {
                    for (int i = 0; i < 21; ++i) {
                        auto delta = sub(hand.hand[i], track.handPrevious[i]);
                        track.handVelocity[i] = track.hasHandVelocity ? add(mul(track.handVelocity[i], 0.65), mul(delta, 0.35)) : delta;
                    }
                    track.hasHandVelocity = true;
                }
                track.handPrevious = hand.hand;
                track.hasHandPrevious = true;
                track.handSource = hand.source;
            }
            track.limbPrevious = limb.limb;
            track.hasLimbPrevious = true;
            track.limbSource = limb.source;
        }
        // Same-person duplicate suppression from the reference fusion.
        auto handCenter = [&](int side) {
            int start = side == 0 ? 91 : 112;
            Vec2 center{};
            for (int i = 0; i < 21; ++i) center = add(center, vec(output.pose.keypoints[start + i]));
            return mul(center, 1.0 / 21.0);
        };
        if (norm(sub(handCenter(0), handCenter(1))) / boxDiagonal(raw.box) < 0.08) {
            int weaker = output.meta[0].handScore <= output.meta[1].handScore ? 0 : 1;
            output.meta[weaker].handState = "duplicate";
            output.meta[weaker].distanceEligible = false;
            output.meta[weaker].alarmEligible = false;
        }
        // Motion-adaptive EMA in person-box coordinates.
        std::array<Vec2, 133> now{};
        for (int i = 0; i < 133; ++i) now[i] = normalized(output.pose.keypoints[i], raw.box);
        if (!state.hasSmoothed) { state.smoothed = now; state.hasSmoothed = true; }
        else {
            for (int i = 0; i < 133; ++i) {
                double displacement = norm(sub(now[i], state.smoothed[i]));
                double alpha = 0.32 + 0.58 * std::clamp(displacement / 0.08, 0.0, 1.0);
                state.smoothed[i] = add(mul(now[i], alpha), mul(state.smoothed[i], 1.0 - alpha));
            }
        }
        fused.push_back(std::move(output));
    }

    // Exact build_global_hands() evidence: selected fusion output, stable
    // score ordering, then the same cross-person duplicate collapse.
    std::vector<GlobalHand> globalCandidates;
    for (std::size_t person = 0; person < fused.size(); ++person) {
        for (int side = 0; side < 2; ++side) {
            auto const& meta = fused[person].meta[side];
            if (!meta.alarmEligible) continue;
            int const start = side == 0 ? 91 : 112;
            GlobalHand hand;
            hand.personId = static_cast<int>(person) + 1;
            hand.trackId = fused[person].pose.box.trackId;
            hand.left = side == 0;
            hand.source = meta.handSource;
            hand.state = meta.handState;
            hand.distanceEligible = meta.distanceEligible;
            hand.alarmEligible = meta.alarmEligible;
            hand.score = static_cast<float>(meta.handScore);
            float minX = fused[person].pose.keypoints[start].x;
            float maxX = minX;
            float minY = fused[person].pose.keypoints[start].y;
            float maxY = minY;
            for (int point = 0; point < 21; ++point) {
                hand.keypoints[point] = fused[person].pose.keypoints[start + point];
                minX = std::min(minX, hand.keypoints[point].x);
                maxX = std::max(maxX, hand.keypoints[point].x);
                minY = std::min(minY, hand.keypoints[point].y);
                maxY = std::max(maxY, hand.keypoints[point].y);
            }
            hand.spanPx = std::hypot(maxX - minX, maxY - minY);
            globalCandidates.push_back(std::move(hand));
        }
    }
    std::stable_sort(globalCandidates.begin(), globalCandidates.end(), [](auto const& first, auto const& second) {
        return first.score > second.score;
    });
    std::vector<GlobalHand> globalHands;
    for (auto& candidate : globalCandidates) {
        Vec2 candidateCenter{};
        for (auto const& point : candidate.keypoints) candidateCenter = add(candidateCenter, vec(point));
        candidateCenter = mul(candidateCenter, 1.0 / 21.0);
        auto duplicate = globalHands.end();
        for (auto iterator = globalHands.begin(); iterator != globalHands.end(); ++iterator) {
            Vec2 existingCenter{};
            for (auto const& point : iterator->keypoints) existingCenter = add(existingCenter, vec(point));
            existingCenter = mul(existingCenter, 1.0 / 21.0);
            double const threshold = std::clamp(
                0.45 * std::max<double>(candidate.spanPx, iterator->spanPx), 18.0, 70.0);
            if (norm(sub(candidateCenter, existingCenter)) <= threshold) {
                duplicate = iterator;
                break;
            }
        }
        if (duplicate == globalHands.end()) globalHands.push_back(std::move(candidate));
        else duplicate->aliases.emplace_back(candidate.personId, candidate.left);
    }

    // Build the model measurements, then collapse cross-person duplicates.
    std::vector<std::array<std::optional<Measurement>, 2>> measurements(fused.size());
    for (std::size_t person = 0; person < fused.size(); ++person) {
        for (int side = 0; side < 2; ++side) {
            if (!fused[person].meta[side].alarmEligible) continue;
            Measurement measurement;
            int start = side == 0 ? 91 : 112;
            for (int i = 0; i < 21; ++i) measurement.hand[i] = vec(fused[person].pose.keypoints[start + i]);
            measurement.precise = fused[person].meta[side].distanceEligible;
            measurement.provisional = !measurement.precise;
            measurement.quality = fused[person].meta[side].handScore;
            measurements[person][side] = measurement;
        }
    }
    auto handSpan = [&](Measurement const& measurement) {
        double minX = measurement.hand[0].x, maxX = minX, minY = measurement.hand[0].y, maxY = minY;
        for (auto point : measurement.hand) { minX = std::min(minX, point.x); maxX = std::max(maxX, point.x); minY = std::min(minY, point.y); maxY = std::max(maxY, point.y); }
        return std::hypot(maxX - minX, maxY - minY);
    };
    auto handCenter = [&](Measurement const& measurement) {
        Vec2 center{}; for (auto point : measurement.hand) center = add(center, point); return mul(center, 1.0 / 21.0);
    };
    for (std::size_t firstPerson = 0; firstPerson < fused.size(); ++firstPerson) {
        for (int firstSide = 0; firstSide < 2; ++firstSide) {
            if (!measurements[firstPerson][firstSide]) continue;
            for (std::size_t secondPerson = firstPerson + 1; secondPerson < fused.size(); ++secondPerson) {
                for (int secondSide = 0; secondSide < 2; ++secondSide) {
                    if (!measurements[firstPerson][firstSide] || !measurements[secondPerson][secondSide]) continue;
                    auto const& first = *measurements[firstPerson][firstSide];
                    auto const& second = *measurements[secondPerson][secondSide];
                    double firstSpan = handSpan(first), secondSpan = handSpan(second);
                    if (norm(sub(handCenter(first), handCenter(second))) > std::max(70.0, 0.5 * std::max(firstSpan, secondSpan))) continue;
                    double firstArea = std::max((fused[firstPerson].pose.box.x2 - fused[firstPerson].pose.box.x1) * (fused[firstPerson].pose.box.y2 - fused[firstPerson].pose.box.y1), 1.0F);
                    double secondArea = std::max((fused[secondPerson].pose.box.x2 - fused[secondPerson].pose.box.x1) * (fused[secondPerson].pose.box.y2 - fused[secondPerson].pose.box.y1), 1.0F);
                    if (firstArea >= 2.5 * secondArea &&
                        (firstSpan >= 1.45 * secondSpan || first.quality >= second.quality + 0.6)) {
                        measurements[secondPerson][secondSide].reset();
                        states_[fused[secondPerson].pose.box.trackId].endpoints[secondSide] = {};
                    } else if (secondArea >= 2.5 * firstArea &&
                               (secondSpan >= 1.45 * firstSpan || second.quality >= first.quality + 0.6)) {
                        measurements[firstPerson][firstSide].reset();
                        states_[fused[firstPerson].pose.box.trackId].endpoints[firstSide] = {};
                    }
                }
            }
        }
    }

    // Match resolve_global_hand_assignments(): a large foreground track may reclaim
    // a nearby hand that RTMW emitted only from a smaller background crop.
    struct Reassignment {
        double normalizedDistance{};
        std::size_t targetPerson{};
        int targetSide{};
        std::size_t sourcePerson{};
        int sourceSide{};
        Vec2 sourceCenter{};
    };
    std::vector<Reassignment> reassignments;
    for (std::size_t targetPerson = 0; targetPerson < fused.size(); ++targetPerson) {
        for (int targetSide = 0; targetSide < 2; ++targetSide) {
            if (measurements[targetPerson][targetSide]) continue;
            auto& targetEndpoint = states_[fused[targetPerson].pose.box.trackId].endpoints[targetSide];
            if (!targetEndpoint.hasWrist || !targetEndpoint.hasHandShape || targetEndpoint.missedFrames > 30) continue;
            double targetArea = std::max(
                (fused[targetPerson].pose.box.x2 - fused[targetPerson].pose.box.x1) *
                (fused[targetPerson].pose.box.y2 - fused[targetPerson].pose.box.y1), 1.0F);
            Vec2 shapeCenter{};
            for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
            shapeCenter = mul(shapeCenter, 1.0 / 21.0);
            Vec2 predictedCenter = add(targetEndpoint.wrist, shapeCenter);
            Measurement targetShape;
            for (int point = 0; point < 21; ++point) targetShape.hand[point] = targetEndpoint.handShape[point];
            double targetSpan = handSpan(targetShape);
            double gate = 0.20 * boxDiagonal(fused[targetPerson].pose.box);
            for (std::size_t sourcePerson = 0; sourcePerson < fused.size(); ++sourcePerson) {
                if (sourcePerson == targetPerson) continue;
                for (int sourceSide = 0; sourceSide < 2; ++sourceSide) {
                    auto const& source = measurements[sourcePerson][sourceSide];
                    if (!source || source->source != "model") continue;
                    double sourceArea = std::max(
                        (fused[sourcePerson].pose.box.x2 - fused[sourcePerson].pose.box.x1) *
                        (fused[sourcePerson].pose.box.y2 - fused[sourcePerson].pose.box.y1), 1.0F);
                    if (targetArea < 2.5 * sourceArea) continue;
                    Vec2 sourceCenter = handCenter(*source);
                    double sourceSpan = handSpan(*source);
                    double candidateDistance = norm(sub(sourceCenter, predictedCenter));
                    if (candidateDistance > gate || targetSpan < 1.45 * std::max(sourceSpan, 1.0)) continue;
                    reassignments.push_back({
                        candidateDistance / std::max(gate, 1e-6), targetPerson, targetSide,
                        sourcePerson, sourceSide, sourceCenter});
                }
            }
        }
    }
    std::sort(reassignments.begin(), reassignments.end(), [](auto const& first, auto const& second) {
        return std::tie(first.normalizedDistance, first.targetPerson, first.targetSide,
                        first.sourcePerson, first.sourceSide) <
               std::tie(second.normalizedDistance, second.targetPerson, second.targetSide,
                        second.sourcePerson, second.sourceSide);
    });
    std::set<std::pair<std::size_t, int>> usedTargets, usedSources;
    for (auto const& item : reassignments) {
        auto targetKey = std::make_pair(item.targetPerson, item.targetSide);
        auto sourceKey = std::make_pair(item.sourcePerson, item.sourceSide);
        if (usedTargets.count(targetKey) || usedSources.count(sourceKey) ||
            measurements[item.targetPerson][item.targetSide] ||
            !measurements[item.sourcePerson][item.sourceSide]) continue;
        auto& targetEndpoint = states_[fused[item.targetPerson].pose.box.trackId].endpoints[item.targetSide];
        Vec2 shapeCenter{};
        for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
        shapeCenter = mul(shapeCenter, 1.0 / 21.0);
        Vec2 reassignedWrist = sub(item.sourceCenter, shapeCenter);
        Measurement reassigned;
        for (int point = 0; point < 21; ++point) {
            reassigned.hand[point] = add(reassignedWrist, targetEndpoint.handShape[point]);
        }
        reassigned.provisional = true;
        reassigned.source = "global_reassignment";
        reassigned.quality = measurements[item.sourcePerson][item.sourceSide]->quality;
        measurements[item.targetPerson][item.targetSide] = reassigned;
        measurements[item.sourcePerson][item.sourceSide].reset();
        auto& sourceEndpoint = states_[fused[item.sourcePerson].pose.box.trackId].endpoints[item.sourceSide];
        sourceEndpoint = {};
        usedTargets.insert(targetKey);
        usedSources.insert(sourceKey);
    }

    PoseFrameResult result;
    result.globalHands = std::move(globalHands);
    result.selectedPeople.reserve(fused.size());
    result.smoothedPeople.reserve(fused.size());
    result.v2People.reserve(fused.size());
    for (auto const& person : fused) {
        std::array<FusionMeta,2> meta{};
        for (int side=0;side<2;++side) {
            auto const& m=person.meta[side];
            meta[side]={m.limbConsensus,m.handValid,m.distanceEligible,m.alarmEligible,m.handScore,m.handSource,m.handState};
        }
        result.metadata.push_back(meta);
        result.selectedPeople.push_back(person.pose);
        PersonPose smoothed = person.pose;
        auto const& state = states_[person.pose.box.trackId];
        for (int keypoint = 0; keypoint < 133; ++keypoint) {
            Vec2 point = denormalized(state.smoothed[keypoint], person.pose.box);
            smoothed.keypoints[keypoint].x = static_cast<float>(point.x);
            smoothed.keypoints[keypoint].y = static_cast<float>(point.y);
        }
        result.smoothedPeople.push_back(smoothed);
        result.v2People.push_back(std::move(smoothed));
    }
    for (std::size_t person = 0; person < fused.size(); ++person) {
        auto& state = states_[fused[person].pose.box.trackId];
        int frameGap = state.lastFrame < 0 ? 0 : std::max(frameIndex - state.lastFrame - 1, 0);
        for (auto& endpoint : state.endpoints) endpoint.missedFrames += frameGap;
        state.lastFrame = frameIndex;
        for (int side = 0; side < 2; ++side) {
            if (!fused[person].meta[side].limbConsensus || !fused[person].meta[side].distanceEligible) continue;
            int shoulderIndex = side == 0 ? 5 : 6;
            int elbowIndex = side == 0 ? 7 : 8;
            int wristIndex = side == 0 ? 9 : 10;
            Vec2 shoulder = vec(fused[person].pose.keypoints[shoulderIndex]);
            Vec2 elbow = vec(fused[person].pose.keypoints[elbowIndex]);
            Vec2 wrist = vec(fused[person].pose.keypoints[wristIndex]);
            double diagonal = boxDiagonal(fused[person].pose.box);
            double upper = norm(sub(shoulder, elbow)) / diagonal;
            double forearm = norm(sub(elbow, wrist)) / diagonal;
            if (upper >= 0.04 && upper <= 0.65 && forearm >= 0.04 && forearm <= 0.65) {
                auto& samples = state.boneSamples[side];
                samples.push_back({upper, forearm});
                while (samples.size() > 120) samples.pop_front();
            }
        }
        for (int side = 0; side < 2; ++side) {
            int shoulderIndex = side == 0 ? 5 : 6;
            int elbowIndex = side == 0 ? 7 : 8;
            int wristIndex = side == 0 ? 9 : 10;
            int handStart = side == 0 ? 91 : 112;
            auto& endpoint = state.endpoints[side];
            auto measurement = measurements[person][side];
            // Reference temporal recovery gate.
            bool const recoverableHandState = fused[person].meta[side].handState == "conflict" ||
                fused[person].meta[side].handState == "inferred";
            if (!measurement && endpoint.hasWrist && endpoint.hasHandShape && endpoint.missedFrames <= 30 &&
                recoverableHandState && fused[person].meta[side].handValid &&
                fused[person].meta[side].handScore >= 2.0 &&
                fused[person].meta[side].linkToSelectedLimb <= 0.08) {
                Vec2 measured = vec(fused[person].pose.keypoints[handStart]);
                Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
                double gate = std::min(0.20, 0.05 + 0.008 * std::max(endpoint.missedFrames, 1)) * boxDiagonal(fused[person].pose.box);
                if (norm(sub(measured, prediction)) <= gate) {
                    Measurement recovered;
                    for (int i = 0; i < 21; ++i) recovered.hand[i] = vec(fused[person].pose.keypoints[handStart + i]);
                    recovered.provisional = true; recovered.quality = fused[person].meta[side].handScore; recovered.source = "temporal_recovery";
                    measurement = recovered;
                }
            }
            LimbState outputState;
            std::array<Vec2, 21> outputHand{};
            bool hasOutput = false;
            bool returningAfterLong = measurement && endpoint.missedFrames > 12;
            if (measurement) {
                Vec2 measuredWrist = measurement->hand[0];
                std::array<Vec2, 21> measuredShape{};
                for (int i = 0; i < 21; ++i) measuredShape[i] = sub(measurement->hand[i], measuredWrist);
                if (!endpoint.hasWrist || endpoint.missedFrames > 12) {
                    endpoint.wrist = measuredWrist; endpoint.velocity = {}; endpoint.handShape = measuredShape;
                } else {
                    Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
                    Vec2 residual = sub(measuredWrist, prediction);
                    double alpha = measurement->precise ? 0.78 : 0.88;
                    double beta = measurement->precise ? 0.18 : 0.24;
                    endpoint.wrist = add(prediction, mul(residual, alpha));
                    endpoint.velocity = add(mul(endpoint.velocity, 0.82), mul(residual, beta));
                    double shapeAlpha = measurement->precise ? 0.55 : 0.70;
                    for (int i = 0; i < 21; ++i) endpoint.handShape[i] = add(mul(measuredShape[i], shapeAlpha), mul(endpoint.handShape[i], 1.0 - shapeAlpha));
                }
                endpoint.hasWrist = endpoint.hasHandShape = true;
                endpoint.lastObservedFrame = frameIndex;
                endpoint.missedFrames = 0;
                outputState = measurement->source == "global_reassignment" ? LimbState::Reassigned : measurement->source == "temporal_recovery" ? LimbState::Recovered : measurement->precise ? LimbState::Observed : measurement->provisional ? LimbState::Provisional : LimbState::Interpolated;
                for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
                hasOutput = true;
            } else {
                ++endpoint.missedFrames;
                if (!endpoint.hasWrist || endpoint.missedFrames > 30) outputState = LimbState::ReachableLong;
                else if (endpoint.missedFrames <= 12) {
                    endpoint.wrist = add(endpoint.wrist, endpoint.velocity);
                    endpoint.velocity = mul(endpoint.velocity, 0.86);
                    for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
                    hasOutput = true; outputState = LimbState::PredictedShort;
                } else outputState = LimbState::ReachableMedium;
            }
            Vec2 smoothedShoulder = denormalized(state.smoothed[shoulderIndex], fused[person].pose.box);
            Vec2 smoothedElbow = denormalized(state.smoothed[elbowIndex], fused[person].pose.box);
            Keypoint shoulder = fused[person].pose.keypoints[shoulderIndex];
            shoulder.x = static_cast<float>(smoothedShoulder.x);
            shoulder.y = static_cast<float>(smoothedShoulder.y);
            Keypoint elbow = fused[person].pose.keypoints[elbowIndex];
            elbow.x = static_cast<float>(smoothedElbow.x);
            elbow.y = static_cast<float>(smoothedElbow.y);
            Keypoint wrist = result.smoothedPeople[person].keypoints[wristIndex];
            std::array<Keypoint, 21> hand{};
            for (int i = 0; i < 21; ++i) {
                hand[i] = result.smoothedPeople[person].keypoints[handStart + i];
            }
            if (hasOutput) {
                auto& ownSamples = state.boneSamples[side];
                auto& oppositeSamples = state.boneSamples[1 - side];
                auto const& samples = ownSamples.size() < 10 && oppositeSamples.size() >= 10
                    ? oppositeSamples : ownSamples;
                double upperRatio = 0.24, forearmRatio = 0.22;
                if (!samples.empty()) {
                    std::vector<double> upperValues, forearmValues;
                    upperValues.reserve(samples.size());
                    forearmValues.reserve(samples.size());
                    for (auto const& sample : samples) {
                        upperValues.push_back(sample[0]);
                        forearmValues.push_back(sample[1]);
                    }
                    upperRatio = median(std::move(upperValues));
                    forearmRatio = median(std::move(forearmValues));
                }
                double diagonal = boxDiagonal(fused[person].pose.box);
                double upperLength = upperRatio * diagonal;
                double forearmLength = forearmRatio * diagonal;
                double totalLength = upperLength + forearmLength;
                if (totalLength > 0.95 * diagonal) {
                    double lengthScale = 0.95 * diagonal / totalLength;
                    upperLength *= lengthScale;
                    forearmLength *= lengthScale;
                }
                Vec2 referenceElbow = returningAfterLong || !endpoint.hasLastElbow
                    ? vec(fused[person].pose.keypoints[elbowIndex]) : endpoint.lastElbow;
                if (!fused[person].meta[side].limbConsensus || outputState != LimbState::Observed) {
                    Vec2 solved = solveTwoBoneElbow(
                        smoothedShoulder, outputHand[0], upperLength, forearmLength, referenceElbow);
                    elbow.x = static_cast<float>(solved.x);
                    elbow.y = static_cast<float>(solved.y);
                }
                endpoint.lastElbow = vec(elbow);
                endpoint.hasLastElbow = true;
                Vec2 armDirection = sub(outputHand[0], smoothedShoulder);
                if (norm(armDirection) > 1e-6) {
                    endpoint.lastDirection = mul(armDirection, 1.0 / norm(armDirection));
                    endpoint.hasLastDirection = true;
                }
                wrist.x = outputHand[0].x; wrist.y = outputHand[0].y;
                for (int i = 0; i < 21; ++i) {
                    hand[i].x = outputHand[i].x; hand[i].y = outputHand[i].y;
                }
            }
            auto& v2Pose = result.v2People[person];
            v2Pose.keypoints[shoulderIndex] = shoulder;
            v2Pose.keypoints[elbowIndex] = elbow;
            v2Pose.keypoints[wristIndex] = wrist;
            if (hasOutput) {
                for (int point = 0; point < 21; ++point) v2Pose.keypoints[handStart + point] = hand[point];
            }
            result.limbs.push_back({
                fused[person].pose.box.trackId, side == 0, outputState,
                elbow, wrist, hand, fused[person].pose.box,
                fused[person].meta[side].distanceEligible,
                fused[person].meta[side].alarmEligible,
                static_cast<float>(fused[person].meta[side].handScore)});
        }
    }
    return result;
}

} // namespace fangjia