#include <algorithm>
#include <cmath>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include "color_analyzer.h"
#include "frame/frame.h"
#include "node.h"
#include "utils/logger.h"
#include "utils/simple_json.h"

namespace rk3588 {

struct PersonShoeCheckConfig {
    int person_class = 0;
    int shoe_class = 1;
    int violation_class = 2;
    float min_person_score = 0.0f;
    float min_shoe_score = 0.0f;
    bool require_person_track_id = false;
    bool use_person_bbox_for_violation = true;
    bool attach_person_track_to_shoe = true;
    bool emit_missing_violation = true;
    bool enable_size_preferred = false;
    float min_shoe_height_ratio = 0.08f;
    float min_shoe_area_ratio = 0.012f;
    float max_shoe_height_ratio = 0.24f;
    float max_shoe_width_ratio = 0.60f;
    float max_shoe_area_ratio = 0.15f;
    float max_shoe_roi_width_ratio = 0.65f;
    float max_shoe_roi_height_ratio = 0.60f;
    float max_shoe_roi_area_ratio = 0.25f;
    float max_shoe_aspect_ratio = 2.0f;
    float match_iou = 0.02f;
    float x_offset = -0.20f;
    float y_offset = 0.64f;
    float width_scale = 1.48f;
    float height_scale = 0.58f;
};

struct LogicGateConfig {
    std::string mode = "ppe_boots_check";
    int anchor_class = 6;
    int boots_class = 3;
    int violation_class = 10;
    bool enable_color_check = true;
    bool pass_through = true;
    bool debug = false;
    std::string violation_key = "ppe_violation";
    ColorConfig color;
    PersonShoeCheckConfig person_shoe;
};

struct ShoeGeomMetrics {
    float person_width_ratio = 0.0f;
    float person_height_ratio = 0.0f;
    float person_area_ratio = 0.0f;
    float roi_width_ratio = 0.0f;
    float roi_height_ratio = 0.0f;
    float roi_area_ratio = 0.0f;
    float aspect_ratio = 0.0f;
    bool person_gate = false;
    bool roi_gate = false;
    bool shape_gate = false;
};

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

    bool Init(const SimpleJson& config, const NodeContext& ctx) override {
        id_ = config.ValueOr<std::string>("id", "logic_gate");
        config_ = ParseConfig(config);
        output_queues_ = ctx.output_queues;

        if (config_.enable_color_check) {
            color_analyzer_ = std::make_unique<ColorAnalyzer>(config_.color);
        }

        LogInfo("[logic_gate] initialized id=" + id_ + " mode=" + config_.mode);
        return true;
    }

    bool Start() override {
        LogInfo("[logic_gate] started id=" + id_);
        return true;
    }

    void Stop() override {}

    NodeStatus Process(FramePtr frame) override {
        if (!frame || !frame->det) {
            PushToDownstream(frame);
            return NodeStatus::OK;
        }

        if (config_.mode == "person_shoe_check") {
            ProcessPersonShoeCheck(frame);
        } else if (config_.mode == "ppe_boots_check") {
            ProcessPpeBootsCheck(frame);
        }

        PushToDownstream(frame);
        return NodeStatus::OK;
    }

private:
    static Rect ClipRect(const Rect& r, int img_w, int img_h) {
        Rect out = r;
        out.x = std::max(0.0f, out.x);
        out.y = std::max(0.0f, out.y);
        if (img_w > 0) out.w = std::min(out.w, static_cast<float>(img_w) - out.x);
        if (img_h > 0) out.h = std::min(out.h, static_cast<float>(img_h) - out.y);
        out.w = std::max(0.0f, out.w);
        out.h = std::max(0.0f, out.h);
        return out;
    }

    static float IoU(const Rect& a, const Rect& b) {
        const float x1 = std::max(a.x, b.x);
        const float y1 = std::max(a.y, b.y);
        const float x2 = std::min(a.x + a.w, b.x + b.w);
        const float y2 = std::min(a.y + a.h, b.y + b.h);
        if (x2 <= x1 || y2 <= y1) return 0.0f;
        const float inter = (x2 - x1) * (y2 - y1);
        const float area_a = a.w * a.h;
        const float area_b = b.w * b.h;
        const float uni = area_a + area_b - inter;
        return uni > 0.0f ? (inter / uni) : 0.0f;
    }

    static bool CenterInside(const Rect& outer, const Rect& inner) {
        const float cx = inner.x + inner.w * 0.5f;
        const float cy = inner.y + inner.h * 0.5f;
        return cx >= outer.x && cx <= (outer.x + outer.w) &&
               cy >= outer.y && cy <= (outer.y + outer.h);
    }

    static ShoeGeomMetrics ComputeShoeGeomMetrics(const Rect& shoe_bbox,
                                                  const Rect& person_bbox,
                                                  const Rect& foot_region,
                                                  float min_shoe_height_ratio,
                                                  float min_shoe_area_ratio,
                                                  float max_shoe_width_ratio,
                                                  float max_shoe_height_ratio,
                                                  float max_shoe_area_ratio,
                                                  float max_shoe_roi_width_ratio,
                                                  float max_shoe_roi_height_ratio,
                                                  float max_shoe_roi_area_ratio,
                                                  float max_shoe_aspect_ratio) {
        ShoeGeomMetrics m;
        const float person_w = std::max(1.0f, person_bbox.w);
        const float person_h = std::max(1.0f, person_bbox.h);
        const float person_area = std::max(1.0f, person_w * person_h);
        const float roi_w = std::max(1.0f, foot_region.w);
        const float roi_h = std::max(1.0f, foot_region.h);
        const float roi_area = std::max(1.0f, roi_w * roi_h);
        const float shoe_w = std::max(1.0f, shoe_bbox.w);
        const float shoe_h = std::max(1.0f, shoe_bbox.h);
        const float shoe_area = std::max(1.0f, shoe_w * shoe_h);

        m.person_width_ratio = shoe_w / person_w;
        m.person_height_ratio = shoe_h / person_h;
        m.person_area_ratio = shoe_area / person_area;
        m.roi_width_ratio = shoe_w / roi_w;
        m.roi_height_ratio = shoe_h / roi_h;
        m.roi_area_ratio = shoe_area / roi_area;
        m.aspect_ratio = shoe_h / shoe_w;

        m.person_gate =
            m.person_height_ratio >= min_shoe_height_ratio &&
            m.person_area_ratio >= min_shoe_area_ratio &&
            m.person_width_ratio <= max_shoe_width_ratio &&
            m.person_height_ratio <= max_shoe_height_ratio &&
            m.person_area_ratio <= max_shoe_area_ratio;
        m.roi_gate =
            m.roi_width_ratio <= max_shoe_roi_width_ratio &&
            m.roi_height_ratio <= max_shoe_roi_height_ratio &&
            m.roi_area_ratio <= max_shoe_roi_area_ratio;
        m.shape_gate = m.aspect_ratio <= max_shoe_aspect_ratio;
        return m;
    }

    LogicGateConfig ParseConfig(const SimpleJson& cfg) {
        LogicGateConfig config;
        config.mode = cfg.ValueOr<std::string>("mode", config.mode);
        config.anchor_class = cfg.ValueOr<int>("anchor_class", config.anchor_class);
        config.boots_class = cfg.ValueOr<int>("boots_class", config.boots_class);
        config.violation_class = cfg.ValueOr<int>("violation_class", config.violation_class);
        config.violation_key = cfg.ValueOr<std::string>("violation_key", config.violation_key);
        config.pass_through = cfg.ValueOr<bool>("pass_through", config.pass_through);
        config.debug = cfg.ValueOr<bool>("debug", config.debug);

        if (const SimpleJson* color = cfg.Find("color_check")) {
            config.enable_color_check = color->ValueOr<bool>("enable", true);
            const std::string method = color->ValueOr<std::string>("method", "hsv");
            if (method == "hsv") config.color.method = ColorMethod::HSV;
            else if (method == "rgb") config.color.method = ColorMethod::RGB;
            else if (method == "brightness") config.color.method = ColorMethod::BRIGHTNESS;
            config.color.dark_threshold = color->ValueOr<int>("dark_threshold", 80);
            config.color.roi_expand = color->ValueOr<float>("roi_expand", 1.0f);
            config.color.center_w_scale = color->ValueOr<float>("center_w_scale", 0.6f);
            config.color.center_h_scale = color->ValueOr<float>("center_h_scale", 0.6f);
            config.color.debug_output = config.debug;
        }

        if (const SimpleJson* ps = cfg.Find("person_shoe_check"); ps && ps->IsObject()) {
            config.person_shoe.person_class = ps->ValueOr<int>("person_class", config.person_shoe.person_class);
            config.person_shoe.shoe_class = ps->ValueOr<int>("shoe_class", config.person_shoe.shoe_class);
            config.person_shoe.violation_class = ps->ValueOr<int>("violation_class", config.person_shoe.violation_class);
            config.person_shoe.min_person_score = ps->ValueOr<float>("min_person_score", config.person_shoe.min_person_score);
            config.person_shoe.min_shoe_score = ps->ValueOr<float>("min_shoe_score", config.person_shoe.min_shoe_score);
            config.person_shoe.require_person_track_id = ps->ValueOr<bool>("require_person_track_id", config.person_shoe.require_person_track_id);
            config.person_shoe.use_person_bbox_for_violation = ps->ValueOr<bool>(
                "use_person_bbox_for_violation", config.person_shoe.use_person_bbox_for_violation);
            config.person_shoe.attach_person_track_to_shoe = ps->ValueOr<bool>(
                "attach_person_track_to_shoe", config.person_shoe.attach_person_track_to_shoe);
            config.person_shoe.emit_missing_violation = ps->ValueOr<bool>(
                "emit_missing_violation", config.person_shoe.emit_missing_violation);
            config.person_shoe.match_iou = ps->ValueOr<float>("match_iou", config.person_shoe.match_iou);
            config.person_shoe.enable_size_preferred = ps->ValueOr<bool>(
                "enable_size_preferred", config.person_shoe.enable_size_preferred);
            config.person_shoe.min_shoe_height_ratio = ps->ValueOr<float>(
                "min_shoe_height_ratio", config.person_shoe.min_shoe_height_ratio);
            config.person_shoe.min_shoe_area_ratio = ps->ValueOr<float>(
                "min_shoe_area_ratio", config.person_shoe.min_shoe_area_ratio);
            config.person_shoe.max_shoe_height_ratio = ps->ValueOr<float>(
                "max_shoe_height_ratio", config.person_shoe.max_shoe_height_ratio);
            config.person_shoe.max_shoe_width_ratio = ps->ValueOr<float>(
                "max_shoe_width_ratio", config.person_shoe.max_shoe_width_ratio);
            config.person_shoe.max_shoe_area_ratio = ps->ValueOr<float>(
                "max_shoe_area_ratio", config.person_shoe.max_shoe_area_ratio);
            config.person_shoe.max_shoe_roi_width_ratio = ps->ValueOr<float>(
                "max_shoe_roi_width_ratio", config.person_shoe.max_shoe_roi_width_ratio);
            config.person_shoe.max_shoe_roi_height_ratio = ps->ValueOr<float>(
                "max_shoe_roi_height_ratio", config.person_shoe.max_shoe_roi_height_ratio);
            config.person_shoe.max_shoe_roi_area_ratio = ps->ValueOr<float>(
                "max_shoe_roi_area_ratio", config.person_shoe.max_shoe_roi_area_ratio);
            config.person_shoe.max_shoe_aspect_ratio = ps->ValueOr<float>(
                "max_shoe_aspect_ratio", config.person_shoe.max_shoe_aspect_ratio);
            if (const SimpleJson* fr = ps->Find("foot_region"); fr && fr->IsObject()) {
                config.person_shoe.x_offset = fr->ValueOr<float>("x_offset", config.person_shoe.x_offset);
                config.person_shoe.y_offset = fr->ValueOr<float>("y_offset", config.person_shoe.y_offset);
                config.person_shoe.width_scale = fr->ValueOr<float>("width_scale", config.person_shoe.width_scale);
                config.person_shoe.height_scale = fr->ValueOr<float>("height_scale", config.person_shoe.height_scale);
            }
        }

        return config;
    }

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

    Rect MapDetCoordToFrame(const Rect& det_bbox, const FramePtr& frame) const {
        if (!frame->transform_meta || !frame->transform_meta->valid) return det_bbox;
        const auto& meta = *frame->transform_meta;
        if (meta.src_w <= 0 || meta.src_h <= 0 || frame->width <= 0 || frame->height <= 0) return det_bbox;

        const float scale_x = static_cast<float>(frame->width) / static_cast<float>(meta.src_w);
        const float scale_y = static_cast<float>(frame->height) / static_cast<float>(meta.src_h);
        Rect mapped;
        mapped.x = det_bbox.x * scale_x;
        mapped.y = det_bbox.y * scale_y;
        mapped.w = det_bbox.w * scale_x;
        mapped.h = det_bbox.h * scale_y;
        return mapped;
    }

    Rect BuildFootRegion(const Rect& person_bbox, int img_w, int img_h) const {
        Rect roi;
        roi.x = person_bbox.x + person_bbox.w * config_.person_shoe.x_offset;
        roi.y = person_bbox.y + person_bbox.h * config_.person_shoe.y_offset;
        roi.w = person_bbox.w * config_.person_shoe.width_scale;
        roi.h = person_bbox.h * config_.person_shoe.height_scale;
        return ClipRect(roi, img_w, img_h);
    }

    void ProcessPersonShoeCheck(const FramePtr& frame) {
        auto& items = frame->det->items;
        const int img_w = frame->det->img_w > 0 ? frame->det->img_w : frame->width;
        const int img_h = frame->det->img_h > 0 ? frame->det->img_h : frame->height;

        std::vector<size_t> person_indices;
        std::vector<size_t> shoe_indices;
        person_indices.reserve(items.size());
        shoe_indices.reserve(items.size());
        for (size_t i = 0; i < items.size(); ++i) {
            if (items[i].cls_id == config_.person_shoe.person_class) {
                person_indices.push_back(i);
            } else if (items[i].cls_id == config_.person_shoe.shoe_class) {
                shoe_indices.push_back(i);
            }
        }

        if (config_.debug) {
            LogInfo("[logic_gate] person_shoe_check persons=" + std::to_string(person_indices.size()) +
                    " shoes=" + std::to_string(shoe_indices.size()));
        }

        std::vector<bool> shoe_used(shoe_indices.size(), false);
        std::vector<Detection> appended;

        for (size_t pi = 0; pi < person_indices.size(); ++pi) {
            Detection& person = items[person_indices[pi]];
            if (person.score < config_.person_shoe.min_person_score) continue;
            if (config_.person_shoe.require_person_track_id && person.track_id < 0) continue;

            const Rect foot_region = BuildFootRegion(person.bbox, img_w, img_h);
            if (config_.debug) {
                LogInfo("[logic_gate] person track_id=" + std::to_string(person.track_id) +
                        " bbox=(" + std::to_string(static_cast<int>(person.bbox.x)) + "," +
                        std::to_string(static_cast<int>(person.bbox.y)) + "," +
                        std::to_string(static_cast<int>(person.bbox.w)) + "," +
                        std::to_string(static_cast<int>(person.bbox.h)) + ")" +
                        " foot_region=(" + std::to_string(static_cast<int>(foot_region.x)) + "," +
                        std::to_string(static_cast<int>(foot_region.y)) + "," +
                        std::to_string(static_cast<int>(foot_region.w)) + "," +
                        std::to_string(static_cast<int>(foot_region.h)) + ")");
            }
            int best_shoe_local = -1;
            float best_match_score = 0.0f;
            float best_width_ratio = 0.0f;
            float best_height_ratio = 0.0f;
            float best_area_ratio = 0.0f;
            bool best_size_preferred = false;
            bool best_center_inside = false;
            int candidate_count = 0;

            for (size_t si = 0; si < shoe_indices.size(); ++si) {
                if (shoe_used[si]) continue;
                Detection& shoe = items[shoe_indices[si]];
                if (shoe.score < config_.person_shoe.min_shoe_score) continue;

                const float iou = IoU(foot_region, shoe.bbox);
                const bool center_inside = CenterInside(foot_region, shoe.bbox);
                const bool pass_gate = center_inside || iou >= config_.person_shoe.match_iou;
                const ShoeGeomMetrics metrics = ComputeShoeGeomMetrics(
                    shoe.bbox,
                    person.bbox,
                    foot_region,
                    config_.person_shoe.min_shoe_height_ratio,
                    config_.person_shoe.min_shoe_area_ratio,
                    config_.person_shoe.max_shoe_width_ratio,
                    config_.person_shoe.max_shoe_height_ratio,
                    config_.person_shoe.max_shoe_area_ratio,
                    config_.person_shoe.max_shoe_roi_width_ratio,
                    config_.person_shoe.max_shoe_roi_height_ratio,
                    config_.person_shoe.max_shoe_roi_area_ratio,
                    config_.person_shoe.max_shoe_aspect_ratio);
                const bool size_preferred = !config_.person_shoe.enable_size_preferred ||
                    (metrics.person_gate && metrics.roi_gate && metrics.shape_gate);
                const float match_score = shoe.score * 10.0f + (center_inside ? 1.0f : 0.0f) + iou;
                if (config_.debug) {
                    LogInfo("[logic_gate] shoe candidate person_track=" + std::to_string(person.track_id) +
                            " local_idx=" + std::to_string(static_cast<int>(si)) +
                            " det_idx=" + std::to_string(static_cast<int>(shoe_indices[si])) +
                            " used=" + std::string(shoe_used[si] ? "true" : "false") +
                            " bbox=(" + std::to_string(static_cast<int>(shoe.bbox.x)) + "," +
                            std::to_string(static_cast<int>(shoe.bbox.y)) + "," +
                            std::to_string(static_cast<int>(shoe.bbox.w)) + "," +
                            std::to_string(static_cast<int>(shoe.bbox.h)) + ")" +
                            " score=" + std::to_string(shoe.score) +
                            " iou=" + std::to_string(iou) +
                            " center_inside=" + std::string(center_inside ? "true" : "false") +
                            " pass_gate=" + std::string(pass_gate ? "true" : "false") +
                            " size_preferred=" + std::string(size_preferred ? "true" : "false") +
                            " person_ratios=(w:" + std::to_string(metrics.person_width_ratio) +
                            ",h:" + std::to_string(metrics.person_height_ratio) +
                            ",a:" + std::to_string(metrics.person_area_ratio) + ")" +
                            " roi_ratios=(w:" + std::to_string(metrics.roi_width_ratio) +
                            ",h:" + std::to_string(metrics.roi_height_ratio) +
                            ",a:" + std::to_string(metrics.roi_area_ratio) + ")" +
                            " aspect=" + std::to_string(metrics.aspect_ratio) +
                            " gates=(person:" + std::string(metrics.person_gate ? "true" : "false") +
                            ",roi:" + std::string(metrics.roi_gate ? "true" : "false") +
                            ",shape:" + std::string(metrics.shape_gate ? "true" : "false") + ")" +
                            " match_score=" + std::to_string(match_score));
                }
                if (!pass_gate) continue;

                ++candidate_count;
                const bool should_replace =
                    best_shoe_local < 0 ||
                    (size_preferred && !best_size_preferred) ||
                    (size_preferred == best_size_preferred && match_score > best_match_score);
                if (should_replace) {
                    best_shoe_local = static_cast<int>(si);
                    best_match_score = match_score;
                    best_width_ratio = metrics.person_width_ratio;
                    best_height_ratio = metrics.person_height_ratio;
                    best_area_ratio = metrics.person_area_ratio;
                    best_size_preferred = size_preferred;
                    best_center_inside = center_inside;
                }
            }

            if (best_shoe_local >= 0) {
                shoe_used[static_cast<size_t>(best_shoe_local)] = true;
                Detection& matched_shoe = items[shoe_indices[static_cast<size_t>(best_shoe_local)]];
                if (config_.debug) {
                    LogInfo("[logic_gate] shoe selected person_track=" + std::to_string(person.track_id) +
                            " candidate_count=" + std::to_string(candidate_count) +
                            " local_idx=" + std::to_string(best_shoe_local) +
                            " det_idx=" + std::to_string(static_cast<int>(shoe_indices[static_cast<size_t>(best_shoe_local)])) +
                            " bbox=(" + std::to_string(static_cast<int>(matched_shoe.bbox.x)) + "," +
                            std::to_string(static_cast<int>(matched_shoe.bbox.y)) + "," +
                            std::to_string(static_cast<int>(matched_shoe.bbox.w)) + "," +
                            std::to_string(static_cast<int>(matched_shoe.bbox.h)) + ")" +
                            " score=" + std::to_string(matched_shoe.score) +
                            " center_inside=" + std::string(best_center_inside ? "true" : "false") +
                            " size_preferred=" + std::string(best_size_preferred ? "true" : "false") +
                            " person_ratios=(w:" + std::to_string(best_width_ratio) +
                            ",h:" + std::to_string(best_height_ratio) +
                            ",a:" + std::to_string(best_area_ratio) + ")" +
                            " match_score=" + std::to_string(best_match_score));
                }
                if (config_.person_shoe.attach_person_track_to_shoe &&
                    matched_shoe.track_id < 0 && person.track_id >= 0) {
                    matched_shoe.track_id = person.track_id;
                }
                continue;
            }

            if (config_.debug) {
                LogInfo("[logic_gate] no shoe selected person_track=" + std::to_string(person.track_id) +
                        " candidate_count=" + std::to_string(candidate_count));
            }

            if (config_.person_shoe.emit_missing_violation) {
                Detection no_shoe;
                no_shoe.cls_id = config_.person_shoe.violation_class;
                no_shoe.track_id = person.track_id;
                no_shoe.score = std::max(0.5f, person.score);
                no_shoe.bbox = config_.person_shoe.use_person_bbox_for_violation ? person.bbox : foot_region;
                appended.push_back(no_shoe);

                if (config_.debug) {
                    LogInfo("[logic_gate] no_shoe track_id=" + std::to_string(no_shoe.track_id) +
                            " bbox=(" + std::to_string(static_cast<int>(no_shoe.bbox.x)) + "," +
                            std::to_string(static_cast<int>(no_shoe.bbox.y)) + "," +
                            std::to_string(static_cast<int>(no_shoe.bbox.w)) + "," +
                            std::to_string(static_cast<int>(no_shoe.bbox.h)) + ")");
                }
            }
        }

        if (!appended.empty()) {
            items.insert(items.end(), appended.begin(), appended.end());
        }
    }

    void ProcessPpeBootsCheck(const FramePtr& frame) {
        auto& detections = frame->det->items;
        std::vector<Detection> boots;
        boots.reserve(detections.size());
        for (const auto& det : detections) {
            if (det.cls_id == config_.boots_class) boots.push_back(det);
        }

        for (const auto& boot : boots) {
            if (!config_.enable_color_check || !color_analyzer_) continue;
            if (boot.track_id < 0) {
                if (config_.debug) {
                    LogInfo("[logic_gate] skip unmatched shoe bbox=(" +
                            std::to_string(static_cast<int>(boot.bbox.x)) + "," +
                            std::to_string(static_cast<int>(boot.bbox.y)) + "," +
                            std::to_string(static_cast<int>(boot.bbox.w)) + "," +
                            std::to_string(static_cast<int>(boot.bbox.h)) + ")");
                }
                continue;
            }

            const Rect mapped_bbox = MapDetCoordToFrame(boot.bbox, frame);
            const auto color_result = color_analyzer_->Analyze(*frame, mapped_bbox);
            if (color_result.is_dark) continue;

            Detection no_boots_det;
            no_boots_det.cls_id = config_.violation_class;
            no_boots_det.track_id = boot.track_id;
            no_boots_det.score = std::max(0.5f, color_result.confidence);
            no_boots_det.bbox = boot.bbox;
            detections.push_back(no_boots_det);

            if (config_.debug) {
                LogInfo("[logic_gate] color violation track_id=" + std::to_string(no_boots_det.track_id));
            }
        }
    }

    std::string id_;
    LogicGateConfig config_;
    std::unique_ptr<ColorAnalyzer> color_analyzer_;
    std::vector<std::shared_ptr<SpscQueue<FramePtr>>> output_queues_;
};

REGISTER_NODE(LogicGateNode, "logic_gate");

}  // namespace rk3588