#include <atomic>
#include <chrono>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <memory>
#include <thread>
#include <vector>

#include "behavior/behavior_event_format.h"
#include "face/face_result.h"
#include "node.h"
#include "utils/dma_alloc.h"
#include "utils/logger.h"

#if defined(RK3588_ENABLE_RGA)
#include "im2d.hpp"
#include "im2d_buffer.h"
#include "im2d_type.h"
#endif

namespace rk3588 {

namespace {

constexpr int kObjClassNum = 80;

const char* kCocoLabels[kObjClassNum] = {
    "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat",
    "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat",
    "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack",
    "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball",
    "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket",
    "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple",
    "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair",
    "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse",
    "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator",
    "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"
};

struct Color {
    uint8_t r, g, b;
};

const Color kClassColors[] = {
    {255, 0, 0},     {0, 255, 0},     {0, 0, 255},     {255, 255, 0},   {255, 0, 255},   // 0-4: red, green, blue, yellow, magenta
    {0, 255, 255},   {0, 255, 0},     {0, 128, 0},     {0, 0, 128},     {128, 128, 0},   // 5-9: cyan, person green, dark green, dark blue, olive
    {128, 0, 128},   {0, 128, 128},   {255, 128, 0},   {255, 0, 128},   {128, 255, 0},   // 10-14
    {0, 255, 128},   {128, 0, 255},   {0, 128, 255},   {255, 128, 128}, {128, 255, 128}  // 15-19
};

inline Color GetClassColor(int cls_id) {
    return kClassColors[cls_id % 20];
}

inline const char* GetClassName(int cls_id) {
    if (cls_id >= 0 && cls_id < kObjClassNum) {
        return kCocoLabels[cls_id];
    }
    return "unknown";
}

inline Color GetBehaviorEventColor(BehaviorEventType type) {
    switch (type) {
        case BehaviorEventType::Intrusion: return {255, 128, 0};
        case BehaviorEventType::Climb: return {255, 0, 255};
        case BehaviorEventType::Fall: return {255, 0, 0};
        case BehaviorEventType::Fight: return {255, 255, 0};
    }
    return {255, 255, 255};
}

inline int Clamp(int val, int min_val, int max_val) {
    return val < min_val ? min_val : (val > max_val ? max_val : val);
}

Rect MapRectToFrame(const Rect& in, int src_w, int src_h, int dst_w, int dst_h) {
    if (src_w <= 0 || src_h <= 0 || dst_w <= 0 || dst_h <= 0) return Rect{};
    const float sx = static_cast<float>(dst_w) / static_cast<float>(src_w);
    const float sy = static_cast<float>(dst_h) / static_cast<float>(src_h);
    Rect out{};
    out.x = std::max(0.0f, in.x * sx);
    out.y = std::max(0.0f, in.y * sy);
    out.w = std::max(0.0f, in.w * sx);
    out.h = std::max(0.0f, in.h * sy);
    if (out.x + out.w > static_cast<float>(dst_w)) out.w = std::max(0.0f, static_cast<float>(dst_w) - out.x);
    if (out.y + out.h > static_cast<float>(dst_h)) out.h = std::max(0.0f, static_cast<float>(dst_h) - out.y);
    return out;
}

#if defined(RK3588_ENABLE_RGA)
inline uint32_t PackColorArgb(const Color& c) {
    return (0xFFu << 24) | (static_cast<uint32_t>(c.r) << 16) |
           (static_cast<uint32_t>(c.g) << 8) | static_cast<uint32_t>(c.b);
}

inline bool WrapFrameAsRgaDst(const Frame& frame, rga_buffer_t& dst, int& rga_stride_bytes) {
    if (frame.DmaFd() < 0) return false;

    int rga_fmt = 0;
    int stride_bytes = 0;
    int wstride = 0;
    int hstride = frame.height;

    if (frame.format == PixelFormat::RGB) {
        rga_fmt = RK_FORMAT_RGB_888;
        stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width * 3);
        wstride = stride_bytes / 3;
        if (frame.planes[0].size > 0 && stride_bytes > 0) {
            const int hs = frame.planes[0].size / stride_bytes;
            if (hs >= frame.height) hstride = hs;
        }
    } else if (frame.format == PixelFormat::BGR) {
        rga_fmt = RK_FORMAT_BGR_888;
        stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width * 3);
        wstride = stride_bytes / 3;
        if (frame.planes[0].size > 0 && stride_bytes > 0) {
            const int hs = frame.planes[0].size / stride_bytes;
            if (hs >= frame.height) hstride = hs;
        }
    } else if (frame.format == PixelFormat::NV12) {
        rga_fmt = RK_FORMAT_YCbCr_420_SP;
        stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width);
        wstride = stride_bytes;
        if (frame.planes[0].size > 0 && stride_bytes > 0) {
            const int hs = frame.planes[0].size / stride_bytes;
            if (hs >= frame.height) hstride = hs;
        }
    } else if (frame.format == PixelFormat::YUV420) {
        rga_fmt = RK_FORMAT_YCbCr_420_P;
        stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width);
        wstride = stride_bytes;
        if (frame.planes[0].size > 0 && stride_bytes > 0) {
            const int hs = frame.planes[0].size / stride_bytes;
            if (hs >= frame.height) hstride = hs;
        }
    } else {
        return false;
    }

    rga_stride_bytes = stride_bytes;
    dst = wrapbuffer_fd_t(frame.DmaFd(), frame.width, frame.height, wstride, hstride, rga_fmt);
    return true;
}
#endif

void DrawHLine(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
               int x1, int x2, int y, int thickness, const Color& color) {
    x1 = Clamp(x1, 0, w - 1);
    x2 = Clamp(x2, 0, w - 1);
    if (x1 > x2) std::swap(x1, x2);

    for (int t = 0; t < thickness; ++t) {
        int cy = y + t;
        if (cy < 0 || cy >= h) continue;

        if (fmt == PixelFormat::RGB) {
            for (int x = x1; x <= x2; ++x) {
                int idx = (cy * stride) + x * 3;
                data[idx] = color.r;
                data[idx + 1] = color.g;
                data[idx + 2] = color.b;
            }
        } else if (fmt == PixelFormat::BGR) {
            for (int x = x1; x <= x2; ++x) {
                int idx = (cy * stride) + x * 3;
                data[idx] = color.b;
                data[idx + 1] = color.g;
                data[idx + 2] = color.r;
            }
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
            for (int x = x1; x <= x2; ++x) {
                data[cy * stride + x] = Y;
            }
        }
    }
}

void DrawVLine(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
               int x, int y1, int y2, int thickness, const Color& color) {
    y1 = Clamp(y1, 0, h - 1);
    y2 = Clamp(y2, 0, h - 1);
    if (y1 > y2) std::swap(y1, y2);

    for (int t = 0; t < thickness; ++t) {
        int cx = x + t;
        if (cx < 0 || cx >= w) continue;

        if (fmt == PixelFormat::RGB) {
            for (int y = y1; y <= y2; ++y) {
                int idx = (y * stride) + cx * 3;
                data[idx] = color.r;
                data[idx + 1] = color.g;
                data[idx + 2] = color.b;
            }
        } else if (fmt == PixelFormat::BGR) {
            for (int y = y1; y <= y2; ++y) {
                int idx = (y * stride) + cx * 3;
                data[idx] = color.b;
                data[idx + 1] = color.g;
                data[idx + 2] = color.r;
            }
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
            for (int y = y1; y <= y2; ++y) {
                data[y * stride + cx] = Y;
            }
        }
    }
}

void DrawRect(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
              int x1, int y1, int x2, int y2, float thickness, const Color& color) {
    int t = static_cast<int>(thickness);
    if (t < 1) t = 1;
    DrawHLine(data, w, h, stride, fmt, x1, x2, y1, t, color);
    DrawHLine(data, w, h, stride, fmt, x1, x2, y2 - t + 1, t, color);
    DrawVLine(data, w, h, stride, fmt, x1, y1, y2, t, color);
    DrawVLine(data, w, h, stride, fmt, x2 - t + 1, y1, y2, t, color);
}

const uint8_t kFont5x7[96][7] = {
    {0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // ' '
    {0x04,0x04,0x04,0x04,0x00,0x00,0x04}, // '!'
    {0x0A,0x0A,0x0A,0x00,0x00,0x00,0x00}, // '"'
    {0x0A,0x0A,0x1F,0x0A,0x1F,0x0A,0x0A}, // '#'
    {0x04,0x0F,0x14,0x0E,0x05,0x1E,0x04}, // '$'
    {0x18,0x19,0x02,0x04,0x08,0x13,0x03}, // '%'
    {0x0C,0x12,0x14,0x08,0x15,0x12,0x0D}, // '&'
    {0x0C,0x04,0x08,0x00,0x00,0x00,0x00}, // '''
    {0x02,0x04,0x08,0x08,0x08,0x04,0x02}, // '('
    {0x08,0x04,0x02,0x02,0x02,0x04,0x08}, // ')'
    {0x00,0x04,0x15,0x0E,0x15,0x04,0x00}, // '*'
    {0x00,0x04,0x04,0x1F,0x04,0x04,0x00}, // '+'
    {0x00,0x00,0x00,0x00,0x0C,0x04,0x08}, // ','
    {0x00,0x00,0x00,0x1F,0x00,0x00,0x00}, // '-'
    {0x00,0x00,0x00,0x00,0x00,0x0C,0x0C}, // '.'
    {0x00,0x01,0x02,0x04,0x08,0x10,0x00}, // '/'
    {0x0E,0x11,0x13,0x15,0x19,0x11,0x0E}, // '0'
    {0x04,0x0C,0x04,0x04,0x04,0x04,0x0E}, // '1'
    {0x0E,0x11,0x01,0x02,0x04,0x08,0x1F}, // '2'
    {0x1F,0x02,0x04,0x02,0x01,0x11,0x0E}, // '3'
    {0x02,0x06,0x0A,0x12,0x1F,0x02,0x02}, // '4'
    {0x1F,0x10,0x1E,0x01,0x01,0x11,0x0E}, // '5'
    {0x06,0x08,0x10,0x1E,0x11,0x11,0x0E}, // '6'
    {0x1F,0x01,0x02,0x04,0x08,0x08,0x08}, // '7'
    {0x0E,0x11,0x11,0x0E,0x11,0x11,0x0E}, // '8'
    {0x0E,0x11,0x11,0x0F,0x01,0x02,0x0C}, // '9'
    {0x00,0x0C,0x0C,0x00,0x0C,0x0C,0x00}, // ':'
    {0x00,0x0C,0x0C,0x00,0x0C,0x04,0x08}, // ';'
    {0x02,0x04,0x08,0x10,0x08,0x04,0x02}, // '<'
    {0x00,0x00,0x1F,0x00,0x1F,0x00,0x00}, // '='
    {0x08,0x04,0x02,0x01,0x02,0x04,0x08}, // '>'
    {0x0E,0x11,0x01,0x02,0x04,0x00,0x04}, // '?'
    {0x0E,0x11,0x17,0x15,0x17,0x10,0x0E}, // '@'
    {0x0E,0x11,0x11,0x1F,0x11,0x11,0x11}, // 'A'
    {0x1E,0x11,0x11,0x1E,0x11,0x11,0x1E}, // 'B'
    {0x0E,0x11,0x10,0x10,0x10,0x11,0x0E}, // 'C'
    {0x1C,0x12,0x11,0x11,0x11,0x12,0x1C}, // 'D'
    {0x1F,0x10,0x10,0x1E,0x10,0x10,0x1F}, // 'E'
    {0x1F,0x10,0x10,0x1E,0x10,0x10,0x10}, // 'F'
    {0x0E,0x11,0x10,0x17,0x11,0x11,0x0F}, // 'G'
    {0x11,0x11,0x11,0x1F,0x11,0x11,0x11}, // 'H'
    {0x0E,0x04,0x04,0x04,0x04,0x04,0x0E}, // 'I'
    {0x07,0x02,0x02,0x02,0x02,0x12,0x0C}, // 'J'
    {0x11,0x12,0x14,0x18,0x14,0x12,0x11}, // 'K'
    {0x10,0x10,0x10,0x10,0x10,0x10,0x1F}, // 'L'
    {0x11,0x1B,0x15,0x15,0x11,0x11,0x11}, // 'M'
    {0x11,0x11,0x19,0x15,0x13,0x11,0x11}, // 'N'
    {0x0E,0x11,0x11,0x11,0x11,0x11,0x0E}, // 'O'
    {0x1E,0x11,0x11,0x1E,0x10,0x10,0x10}, // 'P'
    {0x0E,0x11,0x11,0x11,0x15,0x12,0x0D}, // 'Q'
    {0x1E,0x11,0x11,0x1E,0x14,0x12,0x11}, // 'R'
    {0x0F,0x10,0x10,0x0E,0x01,0x01,0x1E}, // 'S'
    {0x1F,0x04,0x04,0x04,0x04,0x04,0x04}, // 'T'
    {0x11,0x11,0x11,0x11,0x11,0x11,0x0E}, // 'U'
    {0x11,0x11,0x11,0x11,0x11,0x0A,0x04}, // 'V'
    {0x11,0x11,0x11,0x15,0x15,0x15,0x0A}, // 'W'
    {0x11,0x11,0x0A,0x04,0x0A,0x11,0x11}, // 'X'
    {0x11,0x11,0x11,0x0A,0x04,0x04,0x04}, // 'Y'
    {0x1F,0x01,0x02,0x04,0x08,0x10,0x1F}, // 'Z'
    {0x0E,0x08,0x08,0x08,0x08,0x08,0x0E}, // '['
    {0x00,0x10,0x08,0x04,0x02,0x01,0x00}, // '\'
    {0x0E,0x02,0x02,0x02,0x02,0x02,0x0E}, // ']'
    {0x04,0x0A,0x11,0x00,0x00,0x00,0x00}, // '^'
    {0x00,0x00,0x00,0x00,0x00,0x00,0x1F}, // '_'
    {0x08,0x04,0x02,0x00,0x00,0x00,0x00}, // '`'
    {0x00,0x00,0x0E,0x01,0x0F,0x11,0x0F}, // 'a'
    {0x10,0x10,0x16,0x19,0x11,0x11,0x1E}, // 'b'
    {0x00,0x00,0x0E,0x10,0x10,0x11,0x0E}, // 'c'
    {0x01,0x01,0x0D,0x13,0x11,0x11,0x0F}, // 'd'
    {0x00,0x00,0x0E,0x11,0x1F,0x10,0x0E}, // 'e'
    {0x06,0x09,0x08,0x1C,0x08,0x08,0x08}, // 'f'
    {0x00,0x0F,0x11,0x11,0x0F,0x01,0x0E}, // 'g'
    {0x10,0x10,0x16,0x19,0x11,0x11,0x11}, // 'h'
    {0x04,0x00,0x0C,0x04,0x04,0x04,0x0E}, // 'i'
    {0x02,0x00,0x06,0x02,0x02,0x12,0x0C}, // 'j'
    {0x10,0x10,0x12,0x14,0x18,0x14,0x12}, // 'k'
    {0x0C,0x04,0x04,0x04,0x04,0x04,0x0E}, // 'l'
    {0x00,0x00,0x1A,0x15,0x15,0x11,0x11}, // 'm'
    {0x00,0x00,0x16,0x19,0x11,0x11,0x11}, // 'n'
    {0x00,0x00,0x0E,0x11,0x11,0x11,0x0E}, // 'o'
    {0x00,0x00,0x1E,0x11,0x1E,0x10,0x10}, // 'p'
    {0x00,0x00,0x0D,0x13,0x0F,0x01,0x01}, // 'q'
    {0x00,0x00,0x16,0x19,0x10,0x10,0x10}, // 'r'
    {0x00,0x00,0x0E,0x10,0x0E,0x01,0x1E}, // 's'
    {0x08,0x08,0x1C,0x08,0x08,0x09,0x06}, // 't'
    {0x00,0x00,0x11,0x11,0x11,0x13,0x0D}, // 'u'
    {0x00,0x00,0x11,0x11,0x11,0x0A,0x04}, // 'v'
    {0x00,0x00,0x11,0x11,0x15,0x15,0x0A}, // 'w'
    {0x00,0x00,0x11,0x0A,0x04,0x0A,0x11}, // 'x'
    {0x00,0x00,0x11,0x11,0x0F,0x01,0x0E}, // 'y'
    {0x00,0x00,0x1F,0x02,0x04,0x08,0x1F}, // 'z'
    {0x02,0x04,0x04,0x08,0x04,0x04,0x02}, // '{'
    {0x04,0x04,0x04,0x04,0x04,0x04,0x04}, // '|'
    {0x08,0x04,0x04,0x02,0x04,0x04,0x08}, // '}'
    {0x00,0x00,0x08,0x15,0x02,0x00,0x00}, // '~'
    {0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // DEL
};

void DrawChar(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
              int x, int y, char c, int scale, const Color& color) {
    if (c < 32 || c > 127) c = ' ';
    int idx = c - 32;
    const uint8_t* glyph = kFont5x7[idx];

    for (int row = 0; row < 7; ++row) {
        for (int col = 0; col < 5; ++col) {
            if (glyph[row] & (1 << (4 - col))) {
                for (int sy = 0; sy < scale; ++sy) {
                    for (int sx = 0; sx < scale; ++sx) {
                        int px = x + col * scale + sx;
                        int py = y + row * scale + sy;
                        if (px < 0 || px >= w || py < 0 || py >= h) continue;

                        if (fmt == PixelFormat::RGB) {
                            int i = py * stride + px * 3;
                            data[i] = color.r;
                            data[i + 1] = color.g;
                            data[i + 2] = color.b;
                        } else if (fmt == PixelFormat::BGR) {
                            int i = py * stride + px * 3;
                            data[i] = color.b;
                            data[i + 1] = color.g;
                            data[i + 2] = color.r;
                        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
                            uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
                            data[py * stride + px] = Y;
                        }
                    }
                }
            }
        }
    }
}

void DrawText(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
              int x, int y, const char* text, float scale, const Color& color) {
    int cx = x;
    int int_scale = static_cast<int>(scale);
    if (int_scale < 1) int_scale = 1;
    // Calculate char spacing based on scale
    int char_spacing = static_cast<int>(6 * scale);
    if (char_spacing < 4) char_spacing = 4;  // minimum spacing
    while (*text) {
        DrawChar(data, w, h, stride, fmt, cx, y, *text, int_scale, color);
        cx += char_spacing;
        ++text;
    }
}

}  // namespace

class OsdNode : public INode {
public:
    std::string Id() const override { return id_; }
    std::string Type() const override { return "osd"; }

    bool Init(const SimpleJson& config, const NodeContext& ctx) override {
        id_ = config.ValueOr<std::string>("id", "osd");
        draw_bbox_ = config.ValueOr<bool>("draw_bbox", true);
        draw_text_ = config.ValueOr<bool>("draw_text", true);
        draw_face_det_ = config.ValueOr<bool>("draw_face_det", draw_face_det_);
        draw_face_recog_ = config.ValueOr<bool>("draw_face_recog", draw_face_recog_);
        draw_face_bbox_ = config.ValueOr<bool>("draw_face_bbox", draw_face_bbox_);
        line_width_ = config.ValueOr<float>("line_width", 2.0f);
        font_scale_ = config.ValueOr<float>("font_scale", 1.0f);
        use_rga_bbox_ = config.ValueOr<bool>("use_rga_bbox", use_rga_bbox_);

        if (const SimpleJson* dbg = config.Find("debug"); dbg && dbg->IsObject()) {
            stats_log_ = dbg->ValueOr<bool>("stats", stats_log_);
            stats_interval_ = std::max<uint64_t>(
                1, static_cast<uint64_t>(dbg->ValueOr<int>("stats_interval", static_cast<int>(stats_interval_))));
        }

        // Load custom labels from config
        if (const SimpleJson* labels = config.Find("labels")) {
            for (const auto& item : labels->AsArray()) {
                labels_.push_back(item.AsString(""));
            }
            LogInfo("[osd] loaded " + std::to_string(labels_.size()) + " custom labels");
        }

        input_queue_ = ctx.input_queue;
        if (!input_queue_) {
            LogError("[osd] no input queue for node " + id_);
            return false;
        }
        if (ctx.output_queues.empty()) {
            LogError("[osd] no output queue for node " + id_);
            return false;
        }
        output_queues_ = ctx.output_queues;
        LogInfo("[osd] Init: id=" + id_ + " output_queues=" + std::to_string(output_queues_.size()));
        return true;
    }

    bool Start() override {
        LogInfo(std::string("[osd] start id=") + id_ + " draw_bbox=" + (draw_bbox_ ? "true" : "false") +
                " draw_text=" + (draw_text_ ? "true" : "false") +
                " draw_face_det=" + (draw_face_det_ ? "true" : "false") +
                " draw_face_recog=" + (draw_face_recog_ ? "true" : "false") +
                " draw_face_bbox=" + (draw_face_bbox_ ? "true" : "false") +
                " use_rga_bbox=" + (use_rga_bbox_ ? "true" : "false"));
#if !defined(RK3588_ENABLE_RGA)
        if (use_rga_bbox_ && draw_bbox_) {
            LogError("[osd] use_rga_bbox=true but RK3588_ENABLE_RGA is disabled at build; bbox will NOT be drawn id=" + id_);
        }
#endif
        return true;
    }

    void Stop() override {
        LogInfo("[osd] stop id=" + id_);
    }

    NodeStatus Process(FramePtr frame) override {
        if (!frame) return NodeStatus::DROP;

        FramePtr out = frame;
        if (out->data && (out->det || out->face_det || out->face_recog || out->behavior_events)) {
            if (out.use_count() > 1) {
                out = CloneFrameForWrite(out);
            }
            DrawDetections(out);
            DrawBehaviorEvents(out);
            if (draw_face_det_) DrawFaceDet(out);
            if (draw_face_recog_) DrawFaceRecog(out);
        }

        PushToDownstream(out);
        ++processed_;

        if (stats_log_ && stats_interval_ > 0 && (processed_ % stats_interval_) == 0) {
            LogInfo("[osd] processed=" + std::to_string(processed_) + " id=" + id_);
        }
        return NodeStatus::OK;
    }

private:
    static FramePtr CloneFrameForWrite(const FramePtr& src) {
        if (!src || !src->data || src->data_size == 0) return src;

        // Prefer DMA clone when src already lives in DMA-BUF.
        if (src->DmaFd() >= 0) {
            if (auto dma = DmaAlloc(src->data_size); dma && dma->valid()) {
#if defined(RK3588_ENABLE_RGA)
                // Fast path: RGA copy.
                rga_buffer_t rga_src{};
                rga_buffer_t rga_dst{};
                int dummy_stride = 0;
                Frame dst_frame = *src;
                dst_frame.buffer.reset();
                dst_frame.SetDmaFd(dma->fd);
                dst_frame.data = dma->data();
                dst_frame.data_size = dma->size;
                dst_frame.SetOwner(dma);
                if (WrapFrameAsRgaDst(*src, rga_src, dummy_stride) &&
                    WrapFrameAsRgaDst(dst_frame, rga_dst, dummy_stride)) {
                    if (imcopy(rga_src, rga_dst, 1, nullptr) == IM_STATUS_SUCCESS) {
                        auto out = std::make_shared<Frame>(*src);
                        out->buffer.reset();
                        out->SetDmaFd(dma->fd);
                        out->data = dma->data();
                        out->data_size = dma->size;
                        out->SetOwner(dma);

                        for (int i = 0; i < 3; ++i) {
                            FramePlane p = src->planes[i];
                            int offset = p.offset;
                            if (p.data && src->data) {
                                const ptrdiff_t d = p.data - src->data;
                                if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
                                    offset = static_cast<int>(d);
                                }
                            }
                            p.offset = offset;
                            if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
                                p.data = out->data + offset;
                            } else {
                                p.data = nullptr;
                            }
                            out->planes[i] = p;
                        }
                        out->SyncBufferFromFrame();
                        return out;
                    }
                }
#endif
                // Fallback: CPU memcpy (requires cache sync on linux).
                src->SyncStart();
                DmaSyncStart(dma.get());
                memcpy(dma->data(), src->data, src->data_size);
                DmaSyncEnd(dma.get());
                src->SyncEnd();

                auto out = std::make_shared<Frame>(*src);
                out->buffer.reset();
                out->SetDmaFd(dma->fd);
                out->data = dma->data();
                out->data_size = dma->size;
                out->SetOwner(dma);

                for (int i = 0; i < 3; ++i) {
                    FramePlane p = src->planes[i];
                    int offset = p.offset;
                    if (p.data && src->data) {
                        const ptrdiff_t d = p.data - src->data;
                        if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
                            offset = static_cast<int>(d);
                        }
                    }
                    p.offset = offset;
                    if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
                        p.data = out->data + offset;
                    } else {
                        p.data = nullptr;
                    }
                    out->planes[i] = p;
                }
                out->SyncBufferFromFrame();
                return out;
            }
        }

        auto buf = std::make_shared<std::vector<uint8_t>>(src->data_size);
        memcpy(buf->data(), src->data, src->data_size);

        auto out = std::make_shared<Frame>(*src);
        out->buffer.reset();
        out->SetDmaFd(-1);
        out->data = buf->data();
        out->data_size = buf->size();
        out->SetOwner(buf);

        for (int i = 0; i < 3; ++i) {
            FramePlane p = src->planes[i];
            int offset = p.offset;
            if (p.data && src->data) {
                const ptrdiff_t d = p.data - src->data;
                if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
                    offset = static_cast<int>(d);
                }
            }
            p.offset = offset;
            if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
                p.data = out->data + offset;
            } else {
                p.data = nullptr;
            }
            out->planes[i] = p;
        }
        out->SyncBufferFromFrame();

        return out;
    }

    void PushToDownstream(FramePtr frame) {
        for (auto& q : output_queues_) {
            q->Push(frame);
        }
    }

    const char* GetLabel(int cls_id) const {
        if (!labels_.empty()) {
            if (cls_id >= 0 && cls_id < static_cast<int>(labels_.size())) {
                return labels_[cls_id].c_str();
            }
            return "unknown";
        }
        return GetClassName(cls_id);
    }

    void DrawDetections(FramePtr frame) {
        if (!frame->det || frame->det->items.empty()) return;

        int w = frame->width;
        int h = frame->height;
        const int det_w = frame->det->img_w > 0 ? frame->det->img_w : w;
        const int det_h = frame->det->img_h > 0 ? frame->det->img_h : h;
        const bool map_det_to_frame = (det_w != w || det_h != h);
        uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
        PixelFormat fmt = frame->format;

        const bool want_rga_bbox = use_rga_bbox_ && draw_bbox_;
        bool rga_bbox_ok = false;

#if defined(RK3588_ENABLE_RGA)
        if (want_rga_bbox) {
            if (frame->DmaFd() < 0) {
                LogRgaBboxErrorThrottled("frame has no dma_fd");
            } else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
                         fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
                LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
            } else {
                rga_buffer_t dst{};
                int rga_stride_bytes = 0;
                if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
                    LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
                } else {
                    (void)rga_stride_bytes;
                    im_job_handle_t job = imbeginJob();
                    if (!job) {
                        LogRgaBboxErrorThrottled("imbeginJob failed");
                    } else {
                        bool ok = true;
                        for (const auto& det : frame->det->items) {
                            const Rect draw = map_det_to_frame ? MapRectToFrame(det.bbox, det_w, det_h, w, h) : det.bbox;
                            int x = Clamp(static_cast<int>(draw.x), 0, w - 1);
                            int y = Clamp(static_cast<int>(draw.y), 0, h - 1);
                            int rw = static_cast<int>(draw.w);
                            int rh = static_cast<int>(draw.h);
                            rw = Clamp(rw, 1, w - x);
                            rh = Clamp(rh, 1, h - y);
                            im_rect rect{x, y, rw, rh};
                            const uint32_t c = PackColorArgb(GetClassColor(det.cls_id));
                            if (imrectangleTask(job, dst, rect, c, line_width_) <= 0) {
                                ok = false;
                                break;
                            }
                        }
                        if (ok) {
                            ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
                        } else {
                            (void)imendJob(job, IM_SYNC, 0, nullptr);
                        }
                        rga_bbox_ok = ok;
                        if (!rga_bbox_ok) {
                            LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
                        }
                    }
                }
            }
        }
#else
        if (want_rga_bbox) {
            LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
        }
#endif

        int stride;
        if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
        } else {
            return;  // Unsupported format
        }

        const bool do_cpu_bbox = draw_bbox_ && !use_rga_bbox_;

        const bool do_cpu_text = draw_text_;
        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncStart();
        }

        for (const auto& det : frame->det->items) {
            const Rect draw = map_det_to_frame ? MapRectToFrame(det.bbox, det_w, det_h, w, h) : det.bbox;
            int x1 = static_cast<int>(draw.x);
            int y1 = static_cast<int>(draw.y);
            int x2 = static_cast<int>(draw.x + draw.w);
            int y2 = static_cast<int>(draw.y + draw.h);

            Color color = GetClassColor(det.cls_id);

            if (do_cpu_bbox) {
                DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
            }

            if (do_cpu_text) {
                char label[96];
                if (det.track_id >= 0) {
                    snprintf(label, sizeof(label), "%s %.0f%% id=%d", GetLabel(det.cls_id), det.score * 100, det.track_id);
                } else {
                    snprintf(label, sizeof(label), "%s %.0f%%", GetLabel(det.cls_id), det.score * 100);
                }
                int text_y = y1 - 8 * font_scale_;
                if (text_y < 0) text_y = y1 + 2;
                DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
            }
        }

        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncEnd();
        }
    }

    void DrawFaceRecog(FramePtr frame) {
        if (!frame->face_recog || frame->face_recog->items.empty()) return;

        int w = frame->width;
        int h = frame->height;
        uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
        PixelFormat fmt = frame->format;

        const bool want_rga_bbox = use_rga_bbox_ && draw_face_bbox_;
        bool rga_bbox_ok = false;

#if defined(RK3588_ENABLE_RGA)
        if (want_rga_bbox) {
            if (frame->DmaFd() < 0) {
                LogRgaBboxErrorThrottled("frame has no dma_fd");
            } else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
                         fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
                LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
            } else {
                rga_buffer_t dst{};
                int rga_stride_bytes = 0;
                if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
                    LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
                } else {
                    (void)rga_stride_bytes;
                    im_job_handle_t job = imbeginJob();
                    if (!job) {
                        LogRgaBboxErrorThrottled("imbeginJob failed");
                    } else {
                        bool ok = true;
                        for (const auto& it : frame->face_recog->items) {
                            int x = Clamp(static_cast<int>(it.bbox.x), 0, w - 1);
                            int y = Clamp(static_cast<int>(it.bbox.y), 0, h - 1);
                            int rw = static_cast<int>(it.bbox.w);
                            int rh = static_cast<int>(it.bbox.h);
                            rw = Clamp(rw, 1, w - x);
                            rh = Clamp(rh, 1, h - y);
                            im_rect rect{x, y, rw, rh};
                            const Color color = FaceRecogStateIsKnown(it.state) ? Color{0, 255, 0} : Color{255, 0, 0};
                            if (imrectangleTask(job, dst, rect, PackColorArgb(color), line_width_) <= 0) {
                                ok = false;
                                break;
                            }
                        }
                        if (ok) {
                            ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
                        } else {
                            (void)imendJob(job, IM_SYNC, 0, nullptr);
                        }
                        rga_bbox_ok = ok;
                        if (!rga_bbox_ok) {
                            LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
                        }
                    }
                }
            }
        }
#else
        if (want_rga_bbox) {
            LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
        }
#endif

        int stride;
        if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
        } else {
            return;
        }

        const bool do_cpu_bbox = draw_face_bbox_ && !use_rga_bbox_;
        const bool do_cpu_text = draw_text_;
        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncStart();
        }

        for (const auto& it : frame->face_recog->items) {
            int x1 = static_cast<int>(it.bbox.x);
            int y1 = static_cast<int>(it.bbox.y);
            int x2 = static_cast<int>(it.bbox.x + it.bbox.w);
            int y2 = static_cast<int>(it.bbox.y + it.bbox.h);

            const Color color = FaceRecogStateIsKnown(it.state) ? Color{0, 255, 0} : Color{255, 0, 0};

            if (do_cpu_bbox) {
                DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
            }

            if (do_cpu_text) {
                char label[96];
                const char* name = it.best_name.empty() ? FaceRecogStateName(it.state) : it.best_name.c_str();
                snprintf(label, sizeof(label), "%s %.2f", name, it.best_sim);
                int text_y = y1 - 8 * font_scale_;
                if (text_y < 0) text_y = y1 + 2;
                DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
            }
        }

        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncEnd();
        }
    }

    void DrawBehaviorEvents(FramePtr frame) {
        if (!frame->behavior_events || frame->behavior_events->items.empty()) return;

        int w = frame->width;
        int h = frame->height;
        uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
        PixelFormat fmt = frame->format;

        int stride;
        if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
        } else {
            return;
        }

        if (frame->DmaFd() >= 0) {
            frame->SyncStart();
        }

        for (const auto& event : frame->behavior_events->items) {
            if (event.status == BehaviorEventStatus::Pending) continue;

            const Rect& box = event.bbox;
            int x1 = static_cast<int>(box.x);
            int y1 = static_cast<int>(box.y);
            int x2 = static_cast<int>(box.x + box.w);
            int y2 = static_cast<int>(box.y + box.h);
            const Color color = GetBehaviorEventColor(event.type);

            DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
            if (draw_text_) {
                const std::string label = FormatBehaviorEventLabel(event);
                int text_y = y1 - static_cast<int>(8 * font_scale_);
                if (text_y < 0) text_y = y1 + 2;
                DrawText(data, w, h, stride, fmt, x1, text_y, label.c_str(), font_scale_, color);
            }
        }

        if (frame->DmaFd() >= 0) {
            frame->SyncEnd();
        }
    }

    void DrawFaceDet(FramePtr frame) {
        if (!frame->face_det || frame->face_det->faces.empty()) return;

        int w = frame->width;
        int h = frame->height;
        uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
        PixelFormat fmt = frame->format;

        const bool want_rga_bbox = use_rga_bbox_ && draw_face_bbox_;
        bool rga_bbox_ok = false;

#if defined(RK3588_ENABLE_RGA)
        if (want_rga_bbox) {
            if (frame->DmaFd() < 0) {
                LogRgaBboxErrorThrottled("frame has no dma_fd");
            } else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
                         fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
                LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
            } else {
                rga_buffer_t dst{};
                int rga_stride_bytes = 0;
                if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
                    LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
                } else {
                    (void)rga_stride_bytes;
                    im_job_handle_t job = imbeginJob();
                    if (!job) {
                        LogRgaBboxErrorThrottled("imbeginJob failed");
                    } else {
                        bool ok = true;
                        const Color color{0, 180, 180};  // Soft cyan for face overlays
                        for (const auto& it : frame->face_det->faces) {
                            int x = Clamp(static_cast<int>(it.bbox.x), 0, w - 1);
                            int y = Clamp(static_cast<int>(it.bbox.y), 0, h - 1);
                            int rw = static_cast<int>(it.bbox.w);
                            int rh = static_cast<int>(it.bbox.h);
                            rw = Clamp(rw, 1, w - x);
                            rh = Clamp(rh, 1, h - y);
                            im_rect rect{x, y, rw, rh};
                            if (imrectangleTask(job, dst, rect, PackColorArgb(color), line_width_) <= 0) {
                                ok = false;
                                break;
                            }
                        }
                        if (ok) {
                            ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
                        } else {
                            (void)imendJob(job, IM_SYNC, 0, nullptr);
                        }
                        rga_bbox_ok = ok;
                        if (!rga_bbox_ok) {
                            LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
                        }
                    }
                }
            }
        }
#else
        if (want_rga_bbox) {
            LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
        }
#endif

        int stride;
        if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
        } else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
            stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
        } else {
            return;
        }

        const bool do_cpu_bbox = draw_face_bbox_ && !use_rga_bbox_;
        const bool do_cpu_text = draw_text_;
        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncStart();
        }

        const Color color{0, 180, 180};  // Soft cyan for face overlays
        for (const auto& it : frame->face_det->faces) {
            int x1 = static_cast<int>(it.bbox.x);
            int y1 = static_cast<int>(it.bbox.y);
            int x2 = static_cast<int>(it.bbox.x + it.bbox.w);
            int y2 = static_cast<int>(it.bbox.y + it.bbox.h);

            if (do_cpu_bbox) {
                DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
            }
            if (do_cpu_text) {
                char label[64];
                snprintf(label, sizeof(label), "face %.0f%%", it.score * 100);
                int text_y = y1 - 8 * font_scale_;
                if (text_y < 0) text_y = y1 + 2;
                DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
            }
        }

        if ((do_cpu_bbox || do_cpu_text) && frame->DmaFd() >= 0) {
            frame->SyncEnd();
        }
    }

    std::string id_;
    bool draw_bbox_ = true;
    bool draw_text_ = true;
    bool draw_face_det_ = true;
    bool draw_face_recog_ = true;
    bool draw_face_bbox_ = true;
    bool use_rga_bbox_ = true;
    float line_width_ = 2.0f;
    float font_scale_ = 1.0f;
    std::vector<std::string> labels_;

    void LogRgaBboxErrorThrottled(const std::string& reason) {
        const auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                                std::chrono::steady_clock::now().time_since_epoch())
                                .count();
        if (last_rga_bbox_err_ms_ > 0 &&
            now_ms - static_cast<int64_t>(last_rga_bbox_err_ms_) < 1000) {
            return;
        }
        last_rga_bbox_err_ms_ = static_cast<uint64_t>(now_ms);
        LogError("[osd] RGA bbox failed: " + reason + " id=" + id_);
    }

    uint64_t last_rga_bbox_err_ms_ = 0;

    std::shared_ptr<SpscQueue<FramePtr>> input_queue_;
    std::vector<std::shared_ptr<SpscQueue<FramePtr>>> output_queues_;
    uint64_t processed_ = 0;

    bool stats_log_ = false;
    uint64_t stats_interval_ = 100;
};

REGISTER_NODE(OsdNode, "osd");

}  // namespace rk3588