#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <vector>

#include <opencv2/dnn.hpp>

// #include "./clip.cpp/clip.h"
// #include "./clip.cpp/examples/common-clip.h"

using cv::Mat;
using std::cout;
using std::endl;
using std::string;
using std::vector;
//using namespace Ort;

int factor = 4; // downsample factor
double short_term_rate = 0.01; // short-term rate
int short_term_history = 200; // short-term history
double long_term_rate = 0.0005; // long-term rate
int long_term_history = 5000; // long-term history

float iou_threshold = 0.6; // iou threshold

string model_path = "models/ggml-model-f16.gguf"; // clip model path

string algo = "MOG2";

// 定义proposal的结构体
struct proposal{
    int x, y, w, h;
    int life;
    int status;
};

float compute_iou(proposal p1, proposal p2) {
    float x1 = p1.x;
    float y1 = p1.y;
    float w1 = p1.w;
    float h1 = p1.h;
    float x2 = p2.x;
    float y2 = p2.y;
    float w2 = p2.w;
    float h2 = p2.h;
    float area1 = w1 * h1;
    float area2 = w2 * h2;
    float x_overlap = std::max(0.0f, std::min(x1 + w1, x2 + w2) - std::max(x1, x2));
    float y_overlap = std::max(0.0f, std::min(y1 + h1, y2 + h2) - std::max(y1, y2));
    float overlap_area = x_overlap * y_overlap;
    float iou = overlap_area / (area1 + area2 - overlap_area);
    return iou;
}

int proposal_count = 0;

Mat solve_proposal(Mat& frame, proposal& p) {
    // 截取proposal区域
    cv::Rect rect(p.x, p.y, p.w, p.h);
    Mat roi = frame(rect);
    // 计算proposal的特征
    
    // 保存proposal的图片及序号
    string img_path = "./proposals/proposal_" + std::to_string(proposal_count) + ".jpg";
    //string img_path = "./proposals/proposal.jpg";
    proposal_count++;
    cv::imwrite(img_path, roi);
    printf("detect proposal: x=%d, y=%d, w=%d, h=%d\n", p.x, p.y, p.w, p.h);

    // 计算proposal的特征
    return roi;


}
int main() {
    // Read video
    cv::VideoCapture cap("/home/cl/onnx_run/abandoned_od/vedios/vedio-g.mp4");
    if (!cap.isOpened()) {
        cout << "Error opening video stream or file" << endl;
        return -1;
    }
    printf("Frame size: %d x %d\n", cap.get(cv::CAP_PROP_FRAME_WIDTH), cap.get(cv::CAP_PROP_FRAME_HEIGHT));
    printf("FPS: %f\n", cap.get(cv::CAP_PROP_FPS));

    int fps = cap.get(cv::CAP_PROP_FPS);
    int frame_delay = 1000 / fps; // delay between frames in ms

    // Resize video
    int width = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    int height = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    int new_width = width / factor;
    int new_height = height / factor;

    // Initialize background subtractor
    cv::Ptr<cv::BackgroundSubtractor> pShortBG, pLongBG;
    if (algo == "MOG2") {
        pLongBG= cv::createBackgroundSubtractorMOG2(long_term_history, 16.0, false);
        pShortBG = cv::createBackgroundSubtractorMOG2(short_term_history, 16.0, false);
        
    } else {
        cout << "Invalid algorithm name" << endl;
        return -1;
    }

    // Initialize variables

    // proposal List
    vector<proposal> proposals;
    // loop through video
    while(1){
        // 遍历frame
        // Read frame
        Mat origin_frame, frame, ltMask, stMask, subMask;
        cap >> frame;
        if (frame.empty()) {
            break;
        }
        origin_frame = frame.clone();
        // get index of frame
        int index = cap.get(cv::CAP_PROP_POS_FRAMES);
        //printf("Frame %d\n", index);
        // save frame
        cv::imwrite("./frames/frame.jpg", frame);

        // Resize frame
        cv::resize(frame, frame, cv::Size(new_width, new_height));
        // Guassian blur
        cv::GaussianBlur(frame, frame, cv::Size(5, 5), 0);
        // Convert to grayscale
        cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);
        cv::imwrite("./frames/gray.jpg", frame);
        // Update background model
        pLongBG->apply(frame, ltMask, long_term_rate);
        cv::imwrite("./masks/ltMask.jpg", ltMask);
        pShortBG->apply(frame, stMask, short_term_rate);
        cv::imwrite("./masks/stMask.jpg", stMask);

        // ltMask 减去 stMask
        cv::subtract(ltMask, stMask, subMask);
        cv::imwrite("./masks/subMask.jpg", subMask);

        // Denoise
        // erode and dilate
        Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
        cv::erode(subMask, subMask, kernel, cv::Point(-1,-1), 2, 1, 1);
        kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3));
        cv::dilate(subMask, subMask, cv::Mat(), cv::Point(-1,-1), 2, 1, 1);
        
        // gussian blur
        cv::GaussianBlur(subMask, subMask, cv::Size(49, 49), 7);
        cv::imwrite("./masks/subMask_denoised.jpg", subMask);
        // fgMask_denoised[fgMask_denoised <= 5] = 0
        subMask.setTo(0, subMask <= 5);
        
        // save subMask
        // 按顺序保存图片
        cv::imwrite("./masks/mask.jpg", subMask);
        //cv::imwrite("./masks/subMask.jpg", subMask);
        // find contours
        vector<vector<cv::Point>> contours;
        cv::findContours(subMask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

        double area_ts = 10000 / (factor * factor); // threshold for contour area
        double perimeter_ts = 400 / (factor); // threshold for contour perimeter

        //printf("Contours: %d\n", contours.size());
        // 遍历所有轮廓
        for (int i = 0; i < contours.size(); i++) {
            // 计算轮廓的面积和周长
            double area = cv::contourArea(contours[i]);
            double perimeter = cv::arcLength(contours[i], true);
            // 过滤掉太小的轮廓
            if (area > area_ts && perimeter > perimeter_ts) {
                cv::drawContours(frame, contours, i, cv::Scalar(0, 0, 255), 2);

                // 获取矩形的x,y,w,h
                cv::Rect rect = cv::boundingRect(contours[i]);
                //printf("x=%d, y=%d, w=%d, h=%d\n", rect.x, rect.y, rect.width, rect.height);
                // 画矩形
                cv::rectangle(frame, rect, cv::Scalar(0, 255, 0), 2);
                proposal p;
                p.x = rect.x * factor;
                p.y = rect.y * factor;
                p.w = rect.width * factor;
                p.h = rect.height * factor;
                //printf("x=%d, y=%d, w=%d, h=%d\n", p.x, p.y, p.w, p.h);
                p.life = 10;
                p.status = 0;
                for (int j = 0; j < proposals.size(); j++) {
                    // compute iou
                    float iou = 0;
                    iou = compute_iou(p, proposals[j]);
                    if (iou > iou_threshold) {
                        p.status = -1; // 重叠，不添加
                        proposals[j].life++; // 延长生命周期
                        if (proposals[j].status == 2){
                            continue; // 已经进行过检测
                        }
                        proposals[j].status = 1; // 未检测，有效proposal
                    }
                }
                if (p.status == 0){
                    // 新增proposal
                    proposals.push_back(p);
                    //printf("New proposal: x=%d, y=%d, w=%d, h=%d\n", p.x, p.y, p.w, p.h);
                }
                
            }

        
        }

        // 遍历proposal，更新状态
        for (int i = 0; i < proposals.size(); i++) {
            if (proposals[i].status == 1) {
                if (proposals[i].life > 60) {
                    // 生存超过50帧
                    Mat roi = solve_proposal(origin_frame, proposals[i]);
                    // 对roi进行clip visual feature extraction
                    // 保存结果

                    proposals[i].status = 2; // 已检测
                }
            }else {
                proposals[i].life--; // 生命周期减一
                if (proposals[i].life == 0) {
                    // 生命周期结束，删除proposal
                    proposals.erase(proposals.begin() + i);
                    i--;
                }
            }
        }

        // Display frame
        //cv::imshow("frame", frame);
        //cv::imshow("subMask", subMask);

        // Wait for key press
        // int key = cv::waitKey(frame_delay-10);
        // if (key == 27) {
        //     break;
        // }

    // Release video
    
    //cv::destroyAllWindows();
    
    }
    cap.release();
    //clip_free(ctx);
    return 0;
}