#include "vp_ba_wrong_direction_node.h"

namespace vp_nodes
{

    vp_ba_wrong_direction_node::vp_ba_wrong_direction_node(std::string node_name,
                                                           std::map<int, std::vector<vp_objects::vp_point>> left_lines,
                                                           std::map<int, std::vector<vp_objects::vp_point>> right_lines,
                                                           int half_width,
                                                           bool need_record_image,
                                                           bool need_record_video) : vp_node(node_name), all_left_lines(left_lines), all_right_lines(right_lines), half_width(half_width),need_record_image(need_record_image), need_record_video(need_record_video)
    {
        VP_INFO(vp_utils::string_format("[%s] %s", node_name.c_str(), to_string().c_str()));
        this->initialized();
    }

    vp_ba_wrong_direction_node::~vp_ba_wrong_direction_node()
    {
        deinitialized();
    }

    std::string vp_ba_wrong_direction_node::to_string()
    {
        /*
         * return vertexs of all jam regions
         * [channel0: x1,y1 x2,y2 ...][channel1: x1,y1 x2,y2 ...]...
         */
        std::stringstream ss;
        for (auto &r : all_left_lines)
        {
            ss << "[" << r.first << ":";
            for (auto &p : r.second)
            {
                ss << " " << p.x << "," << p.y;
            }
            ss << "]";
        }
        return ss.str();
    }

    bool vp_ba_wrong_direction_node::point_in_poly(vp_objects::vp_point p, std::vector<vp_objects::vp_point> region)
    {
        int i, j, c = 0;
        int nvert = region.size();

        for (i = 0, j = nvert - 1; i < nvert; j = i++)
        {
            if (((region[i].y > p.y) != (region[j].y > p.y)) &&
                (p.x < (region[j].x - region[i].x) * (p.y - region[i].y) / (region[j].y - region[i].y) + region[i].x))
                c = !c;
        }
        return c;
    }

    bool vp_ba_wrong_direction_node::check_wrong_direction(std::vector<vp_objects::vp_point> test_line, std::vector<vp_objects::vp_point> right_line, double angle_tolerance = 30.0)
    {
        if (test_line.size() != 2 || right_line.size() != 2)
        {
            // 线段必须由两个点组成
            return false;
        }

        // 计算 test_line 的方向向量
        double dx_test = test_line[1].x - test_line[0].x;
        double dy_test = test_line[1].y - test_line[0].y;

        // 计算 right_line 的方向向量
        double dx_right = right_line[1].x - right_line[0].x;
        double dy_right = right_line[1].y - right_line[0].y;

        // 计算两个向量的模
        double magnitude_test = std::sqrt(dx_test * dx_test + dy_test * dy_test);
        double magnitude_right = std::sqrt(dx_right * dx_right + dy_right * dy_right);

        if (magnitude_test == 0 || magnitude_right == 0)
        {
            // 如果某条线段的长度为零，则无法比较方向
            return false;
        }

        // 归一化方向向量
        dx_test /= magnitude_test;
        dy_test /= magnitude_test;

        dx_right /= magnitude_right;
        dy_right /= magnitude_right;

        // 计算两个向量的夹角余弦值
        double dot_product = dx_test * dx_right + dy_test * dy_right;

        // 将余弦值限制在 -1 到 1 的范围内（防止计算误差）
        dot_product = std::max(-1.0, std::min(1.0, dot_product));

        // 计算夹角（弧度制）
        double angle_rad = std::acos(dot_product);

        // 将夹角转换为角度制
        double angle_deg = angle_rad * 180.0 / 3.1415926;

        // 判断夹角是否在允许的范围内
        return angle_deg > angle_tolerance;
    }

    std::shared_ptr<vp_objects::vp_meta> vp_ba_wrong_direction_node::handle_frame_meta(std::shared_ptr<vp_objects::vp_frame_meta> meta)
    {
        // if need applied on current channel or not
        if (all_left_lines.count(meta->channel_index) == 0 || all_right_lines.count(meta->channel_index) == 0)
        {
            return meta;
        }

        // for current channel
        // auto &jam_region = all_jam_regions[meta->channel_index];
        auto &left_line = all_left_lines[meta->channel_index];
        auto &right_line = all_right_lines[meta->channel_index];

        auto &wrong_direction_checking_status = all_wrong_direction_checking_status[meta->channel_index];
        auto &wrong_direction_result = all_wrong_direction_results[meta->channel_index];
        auto &last_notify = all_last_notifys[meta->channel_index];

        // for vp_frame_target only
        // for vp_frame_target only
        std::vector<int> hit_traget_ids;
        for (auto &target : meta->targets)
        {
            if (target->primary_label != target->class_label_of_person)
            {
                auto len = target->tracks.size();

                // target has been tracked AND tracked enough frames
                if (len < check_interval_frames || target->track_id < 0)
                {
                    continue;
                }
                // if target inside of stop region or not
                // if (!point_in_poly(loc, jam_region))
                //{
                //    continue;
                //}
                if (len > 2 && target->track_id >= 0)
                {
                    // check the last 2 points in tracks
                    auto p1 = target->tracks[len - 1].track_point();
                    auto p2 = target->tracks[len - 2].track_point();

                    // 使用 auto 自动推导类型
                    auto test_line = {p2, p1};

                    auto is_wrong_direction = (p1.x < half_width) ? check_wrong_direction(test_line, left_line) : check_wrong_direction(test_line, right_line);

                    if (is_wrong_direction)
                    {
                        wrong_direction_checking_status[target->track_id]++;
                        hit_traget_ids.push_back(target->track_id);
                    }
                }
            }
        }
        std::vector<int> involve_targets;
        for (auto i = wrong_direction_checking_status.begin(); i != wrong_direction_checking_status.end();)
        {
            if (std::find(hit_traget_ids.begin(), hit_traget_ids.end(), i->first) == hit_traget_ids.end())
            {
                // remove since it not satisfy stop condition
                i = wrong_direction_checking_status.erase(i);
                continue;
            }
            if (i->second >= check_min_hit_frames)
            {
                involve_targets.push_back(i->first);
            }
            i++;
        }
        if (involve_targets.size() > 0 && (meta->frame_index - last_notify) > (check_notify_interval * meta->fps))
        {
            wrong_direction_result = true;
            last_notify = meta->frame_index;
            // send record image and record video signal, recording actions would occur if record nodes exist in pipeline
            std::string image_file_name_without_ext = ""; // empty means no recording image
            std::string video_file_name_without_ext = ""; // empty means no recording video

            // send image record control meta
            if (need_record_image)
            {
                image_file_name_without_ext = vp_utils::time_format(NOW, "wrong_direction_image__<year><mon><day><hour><min><sec><mili>");
                auto image_record_control_meta = std::make_shared<vp_objects::vp_image_record_control_meta>(meta->channel_index, image_file_name_without_ext, true);
                pendding_meta(image_record_control_meta);
            }
            // send video record control meta
            if (need_record_video)
            {
                video_file_name_without_ext = vp_utils::time_format(NOW, "wrong_direction_video__<year><mon><day><hour><min><sec><mili>");
                auto video_record_control_meta = std::make_shared<vp_objects::vp_video_record_control_meta>(meta->channel_index, video_file_name_without_ext);
                pendding_meta(video_record_control_meta);
            }

            // std::vector<vp_objects::vp_point> involve_region{line.start, line.end};
            auto ba_result = std::make_shared<vp_objects::vp_ba_result>(vp_objects::vp_ba_type::WRONGDIRECTION,
                                                                        meta->channel_index,
                                                                        meta->frame_index,
                                                                        involve_targets,
                                                                        // involve_region,
                                                                        left_line,
                                                                        "wrong_direction", // meaningful label
                                                                        image_file_name_without_ext,
                                                                        video_file_name_without_ext);
            // fill back to frame meta
            meta->ba_results.push_back(ba_result);
            // info log
            VP_INFO(vp_utils::string_format("[%s] [channel %d] has found target wrong direction.", node_name.c_str(), meta->channel_index));
            if (need_record_image || need_record_video)
            {
                VP_INFO(vp_utils::string_format("[%s] [channel %d] image & video record file names are: [%s & %s]", node_name.c_str(), meta->channel_index, image_file_name_without_ext.c_str(), video_file_name_without_ext.c_str()));
            }
        }
        if (involve_targets.size() == 0 && wrong_direction_result)
        {
            wrong_direction_result = false;
            last_notify = meta->frame_index;
        }
        return meta;
    }
}