#include "alg_jd_ng.h"
#include <direct.h> //所需的库文件

extern SysConf g_sys_conf;

// Initialize the parameters
static float confThreshold = 0.01; // Confidence threshold
static float nmsThreshold = 0.4;  // Non-maximum suppression threshold
static int inpWidth = 416;        // Width of network's input image
static int inpHeight = 416;       // Height of network's input image

static std::vector<std::string> classes;

bool AlgJd_ng::init_ng(QString model_path, QString model_name)
{
    // Load names of classes
    std::string classesFile;
    cv::String modelConfiguration;
    cv::String modelWeights;
    QString image_path;

    modelWeights = model_path.toStdString() + "/" + model_name.toStdString();
    modelConfiguration = model_path.toStdString() + "/jd_ng.cfg";
    classesFile = model_path.toStdString() + "/jd_ng.names";
    image_path = model_path + "/" + "alg_jd_ng.bmp";

    std::ifstream classNamesFile(classesFile.c_str());
    if (classNamesFile.is_open())
    {
        std::string className = "";
        while (std::getline(classNamesFile, className))
            classes.push_back(className);
    }
    else{
		return false;
    }

    // Load the network
    net_ng = cv::dnn::readNetFromDarknet(modelConfiguration, modelWeights);
    net_ng.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
    net_ng.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA);
    
	//cv::Mat image = cv::imread("alg_jd.bmp");
    cv::Mat image = cv::imread(image_path.toStdString());

	//识别一张图，测试模型是否正确，并且完成GPU数据加载
	if (!image.data)	return false;	//判断测试图片是否正常读取
	std::vector<std::pair<int, cv::Rect> > results;
	detect_ng(image, image, results);
	if (results.size() > 0)
		return true;	//检测到目标，则初始化成功
	else
		return false;	//否则初始化失败
}

bool AlgJd_ng::test_detect_ng()
{
    cv::Mat m1;
    m1 = cv::Mat(544, 728, CV_8UC3, cv::Scalar(0, 0, 0));
	std::vector<std::pair<int, cv::Rect> > results;
    double t = (double)cv::getTickCount();
	detect_ng(m1, m1, results);
    t = ((double)cv::getTickCount() - t) / cv::getTickFrequency();
    DEBUG("    test_detect time process:%f\n", t);
    return true;
}

bool AlgJd_ng::test_detect_batcht_ng(int shoot)
{
    std::vector<cv::Mat> vec_in;
    std::vector<cv::Mat> vec_out;
    std::vector<std::vector<std::pair<int, cv::Rect> > > vec_results;
    cv::Mat m1, m2, m3;
    m1 = cv::Mat(544, 728, CV_8UC3, cv::Scalar(0, 0, 0));
    m2 = cv::Mat(544, 728, CV_8UC3, cv::Scalar(0, 0, 0));
    m3 = cv::Mat(544, 728, CV_8UC3, cv::Scalar(0, 0, 0));

    double t = (double)cv::getTickCount();
    switch(shoot){
        case 1:{
            std::vector<std::pair<int, cv::Rect> > results;
            detect_ng(m1, m1, results);
            break;
        }
        case 3:vec_in.push_back(m3);
        case 2:vec_in.push_back(m2);
        default:{
            vec_in.push_back(m1);
            detect_batch_ng(vec_in, vec_out, vec_results);
        }
    }
    t = ((double)cv::getTickCount() - t) / cv::getTickFrequency();
    DEBUG("    test_detect_batcht time process:%f\n", t);
    return true;
}

int AlgJd_ng::detect_ng(cv::Mat& _frame, cv::Mat &out, std::vector<std::pair<int, cv::Rect> > &results)
{
    cv::Mat frame = _frame.clone();
    cv::Mat image_clone=frame.clone();
    // Process frames.
    // Create a 4D blob from a frame.
    cv::Mat blob;
    cv::dnn::blobFromImage(frame, blob, 1/255.0, cv::Size(inpWidth, inpHeight), cv::Scalar(0,0,0), true, false);

    //Sets the input to the network
    net_ng.setInput(blob);


    // Runs the forward pass to get output of the output layers
    std::vector<cv::Mat> outs;
    net_ng.forward(outs, getOutputsNames_ng(net_ng));


    // Remove the bounding boxes with low confidence
    post_process_ng(frame, outs, results);


    // Write the frame with the detection boxes
    cv::Mat detectedFrame;
    frame.convertTo(detectedFrame, CV_8U);
    //show detectedFrame
    out=detectedFrame.clone();
	return results.size();
}

int AlgJd_ng::detect_ng(cv::Mat& _frame, cv::Mat& draw_frame, cv::Mat& out, std::vector<std::pair<int, cv::Rect>>& results)
{
    cv::Mat frame = _frame.clone();

    // Process frames.
    // Create a 4D blob from a frame.
    // 创建4D的blob用于网络输入
    cv::Mat blob;
    cv::dnn::blobFromImage(frame, blob, 1 / 255.0, cv::Size(inpWidth, inpHeight), cv::Scalar(0, 0, 0), true, false);

    //Sets the input to the network
    net_ng.setInput(blob);


    // Runs the forward pass to get output of the output layers
    std::vector<cv::Mat> outs;
    net_ng.forward(outs, getOutputsNames_ng(net_ng));


    // Remove the bounding boxes with low confidence
    // 删除低置信度的边界框
    post_process_ng(draw_frame, outs, results);

    // Write the frame with the detection boxes
    cv::Mat detectedFrame;
    draw_frame.convertTo(detectedFrame, CV_8U);

    //show detectedFrame
    out = detectedFrame.clone();
    return results.size();
}

void AlgJd_ng::CircleImagePro_ng(cv::Mat src, cv::Mat dst, std::vector<float> radius) {
    QStringList TestData;
    src.copyTo(dst);
    float c = float(20.5) / float(68.9);
    std::vector<std::vector<cv::Point>> contours;
    // 图像预处理
    cv::Mat img;
    cv::cvtColor(dst, img, cv::COLOR_BGR2GRAY);
    GaussianBlur(img, img, cv::Size(3, 3), 1);
    Canny(img, img, 50, 150, 3);
    imshow("img", img);

    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); // 获取卷积核
    dilate(img, img, kernel, cv::Point(-1, -1), 2);
    findContours(img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    img.release();

    std::vector<cv::Point2f> center(contours.size());
    std::vector<float> rad(contours.size());
    if (contours.size() == 0) {
        std::cout << "未找到检测物" << std::endl;
        return;
    }
    for (size_t i = 0; i < contours.size(); ++i) {
        minEnclosingCircle(contours[i], center[i], rad[i]);
        radius.push_back(rad[i] * c);
    }

    for (size_t j = 0; j < center.size(); ++j) {
        std::string str = "radius: ";
        std::string str1;
        float r = 0.0;
        circle(dst, center[j], rad[j], cv::Scalar(0, 0, 255), 1);
        r = rad[j] * c;
        str1 = format("%.4f", r);
        str.insert(str.end(), str1.begin(), str1.end());
        TestData.append(QString::fromStdString(str));
        cv::imshow("dst", dst);
        cv::putText(dst, str, center[j], 1, 2, cv::Scalar(0, 255, 0), 2, 8);
        cv::waitKey(1);
    }
    TestData.clear();
}

void AlgJd_ng::analyse_ng(cv::Mat vec_in, std::vector<std::pair<int, cv::Rect>>& vec_results)
{
    bool IsNG = false;
    std::vector<cv::Rect> vec_jd_results;	// 0 胶点
    std::vector<cv::Rect> vec_kz_results;	// 1 卡纸距离

    for (int i = 0; i < vec_results.size(); i++)
    {
        if (vec_results[i].first == 0)	// jd 
        {
            vec_jd_results.push_back(vec_results[i].second);
        }
        else if (vec_results[i].first == 1) // 卡纸距离
        {
            vec_kz_results.push_back(vec_results[i].second);
        }
    }
    std::sort(vec_jd_results.begin(), vec_jd_results.end(), sort_rect_by_y_center_ng);//升序排列，//jinhuan+

    //jinhuan+
    if (vec_jd_results.size() && vec_kz_results.size())
    {
        //从上往下第一个胶点的中心y值
        int top_jd_y = vec_jd_results[0].y + vec_jd_results[0].height / 2;
        //从上往下最后一个胶点的中心y值
        int bottom_jd_y = vec_jd_results[vec_jd_results.size()-1].y + vec_jd_results[vec_jd_results.size()-1].height / 2;
        //卡纸的上边沿y值
        int top_kz_y = vec_kz_results[0].tl().y;
        //卡纸的下边沿y值
        int bottom_kz_y = vec_kz_results[0].br().y;

        //if(abs(top_jd_y- top_kz_y)>50);//50个像素
        //if (abs(bottom_kz_y - bottom_jd_y)>40);//40个像素
    } else {
        IsNG = true;//卡纸和胶点数量不够
    }
    //if (vec_kz_results.size() != 1) {//卡纸有褶皱
    //    IsNG = true;
    //}

    //jinhuan-
    cv::waitKey(1);
}

// Get the names of the output layers
std::vector<cv::String> AlgJd_ng::getOutputsNames_ng(const cv::dnn::Net& net)
{
    std::vector<cv::String> names;
    if (names.empty())
    {
        //Get the indices of the output layers, i.e. the layers with unconnected outputs
        std::vector<int> outLayers = net.getUnconnectedOutLayers();

        //get the names of all the layers in the network
        std::vector<cv::String> layersNames = net.getLayerNames();

        // Get the names of the output layers in names
        names.resize(outLayers.size());
        for (size_t i = 0; i < outLayers.size(); ++i)
        names[i] = layersNames[outLayers[i] - 1];
    }
    return names;
}

void AlgJd_ng::post_process_ng(cv::Mat& frame, std::vector<cv::Mat>& outs, std::vector<std::pair<int, cv::Rect> > &results)
{
    std::vector < std::vector<int>> classIds(classes.size());
    std::vector < std::vector<float>> confidences(classes.size());
    std::vector < std::vector<cv::Rect>> boxes(classes.size());
    for (size_t i = 0; i < outs.size(); ++i)
    {
        // Scan through all the bounding boxes output from the network and keep only the
        // ones with high confidence scores. Assign the box's class label as the class
        // with the highest score for the box.
        // 得到每个输出的数据
        float* data = (float*)outs[i].data;
        for (int j = 0; j < outs[i].rows; ++j, data += outs[i].cols)
        {
            // 得到每个输出的所有类别的分数
            cv::Mat scores = outs[i].row(j).colRange(5, outs[i].cols);
            cv::Point classIdPoint;
            double confidence;
            // Get the value and location of the maximum score
            cv::minMaxLoc(scores, 0, &confidence, 0, &classIdPoint);

            //if (confidence > g_sys_conf.ConfThresholds[classIdPoint.x]*0.0001)
            if (confidence > confThreshold)
            {
                int centerX = (int)(data[0] * frame.cols);
                int centerY = (int)(data[1] * frame.rows);
                int width = (int)(data[2] * frame.cols);
                int height = (int)(data[3] * frame.rows);
                int left = centerX - width / 2;
                int top = centerY - height / 2;

                classIds[classIdPoint.x].push_back(classIdPoint.x);
                confidences[classIdPoint.x].push_back((float)confidence);
                boxes[classIdPoint.x].push_back(cv::Rect(left, top, width, height));
            }
        }
    }


    // Perform non maximum suppression to eliminate redundant overlapping boxes with
    // lower confidences

    for (size_t i = 0; i < classes.size(); ++i)
    {
        std::vector<int> indices;
        cv::dnn::NMSBoxes(boxes[i], confidences[i], confThreshold, nmsThreshold, indices);
        for (size_t j = 0; j < indices.size(); ++j)
        {
            int idx = indices[j];
            cv::Rect box = boxes[i][idx];
            //if (classIds[i][idx] == 2)   continue;
            //else if (classIds[i][idx] == 3)  continue;

            drawPred_ng(classIds[i][idx], confidences[i][idx], box.x, box.y,
                    box.x + box.width, box.y + box.height, frame);
            results.push_back(std::make_pair(classIds[i][idx], box));
        }
    }
}

// Draw the predicted bounding box
void AlgJd_ng::drawPred_ng(int classId, float conf, int left, int top, int right, int bottom, cv::Mat& frame)
{
	cv::Scalar color;
    if (classId == 0)  //jd
        color = cv::Scalar(0, 0, 255);  // 绿色
    else if (classId == 1)   // kz
        color = cv::Scalar(0, 255, 0);  //
    else if (classId == 2)   // bm
        color = cv::Scalar(0, 255, 0);
    else if (classId == 3)   // kzx
        color = cv::Scalar(0, 255, 0);
    else
        color = cv::Scalar(255, 255, 255);  // 白色

    //Draw a rectangle displaying the bounding box
    cv::rectangle(frame, cv::Point(left, top), cv::Point(right, bottom), color, 4);
   
    //Get the label for the class name and its confidence
    std::string label = cv::format("%.2f%%", (conf*100));///
    if (!classes.empty())
    {
        CV_Assert(classId < (int)classes.size());
        label = classes[classId] + ": " + label;
    }
    else
    {
        std::cout<<"classes is empty..."<<std::endl;
    }

    //Display the label at the top of the bounding box
    int baseLine;
    cv::Size labelSize = cv::getTextSize(label, cv::FONT_HERSHEY_SIMPLEX, 1, 1, &baseLine);
    top = std::max(top, labelSize.height);
    cv::putText(frame, label, cv::Point(left, top-5), cv::FONT_HERSHEY_SIMPLEX, 0.8, color, 1);
}

void AlgJd_ng::detect_batch_ng(std::vector<cv::Mat>& vec_in, std::vector<cv::Mat> &vec_out, std::vector<std::vector<std::pair<int, cv::Rect>>> &vec_results)
{
	cv::Mat blobs;
    std::vector<cv::Mat> image;
    
	cv::dnn::blobFromImages(vec_in, blobs, 1 / 255.0, cv::Size(inpWidth, inpHeight), cv::Scalar(0, 0, 0), true, false);

	//Sets the input to the network
	net_ng.setInput(blobs);

	// Runs the forward pass to get output of the output layers
	std::vector<cv::Mat> outs;
	net_ng.forward(outs, getOutputsNames_ng(net_ng));

	for (int i = 0; i < vec_in.size(); i++)
	{
		image.push_back(vec_in[i].clone());
	}

	// Remove the bounding boxes with low confidence
	post_process_batch_ng(image, outs, vec_results);


	// Write the frame with the detection boxes
	for (int i = 0; i < vec_in.size(); i++)
	{
		cv::Mat detectedFrame, out;
		image[i].convertTo(detectedFrame, CV_8U);
		out = detectedFrame.clone();
		vec_out.push_back(out);
	}
}

void AlgJd_ng::detect_batch_ng(std::vector<cv::Mat>& vec_in, std::vector<cv::Mat>& transits, std::vector<cv::Mat>& vec_out, std::vector<std::vector<std::pair<int, cv::Rect> > >& vec_results)
{
    cv::Mat blobs;
    //std::vector<cv::Mat> image= transits;

    cv::dnn::blobFromImages(vec_in, blobs, 1 / 255.0, cv::Size(inpWidth, inpHeight), cv::Scalar(0, 0, 0), true, false);

    //Sets the input to the network
    net_ng.setInput(blobs);

    // Runs the forward pass to get output of the output layers
    std::vector<cv::Mat> outs;
    net_ng.forward(outs, getOutputsNames_ng(net_ng));
    //for (int i = 0; i < vec_in.size(); i++)
    //{
    //    //image.push_back(vec_in[i].clone());
    //    transits.push_back(vec_in[i].clone());
    //}

    // Remove the bounding boxes with low confidence
    //post_process_batch_ng(image, outs, vec_results);
    post_process_batch_ng(transits, outs, vec_results);

    // Write the frame with the detection boxes
    for (int i = 0; i < vec_in.size(); i++)
    {
        cv::Mat detectedFrame, out;
        //image[i].convertTo(detectedFrame, CV_8U);
        transits[i].convertTo(detectedFrame, CV_8U);
        out = detectedFrame.clone();
        vec_out.push_back(out);
    }
}

void AlgJd_ng::post_process_batch_ng(std::vector<cv::Mat>& vec_frame, std::vector<cv::Mat>& outs, std::vector<std::vector<std::pair<int, cv::Rect> > > &vec_results)
{
	int batch = vec_frame.size();
    double confidence;///
	for (int k = 0; k < batch; k++)
	{
        std::vector < std::vector<int>> classIds(classes.size());
        std::vector < std::vector<float>> confidences(classes.size());
        std::vector < std::vector<cv::Rect>> boxes(classes.size());

        //std::cout << "outs.size()\t" << outs.size() << std::endl;
		//std::cout << "Type\t" << outs[0].type() << std::endl;

		for (size_t i = 0; i < outs.size(); ++i)
		{
			//std::cout << "outs.dims\t" << outs[i].dims << std::endl;
			//std::cout << "outs[" << i << "].rows\t" << outs[i].size[0] << std::endl;
			//std::cout << "outs[" << i << "].cols\t" << outs[i].size[1] << std::endl;
			//std::cout << "outs[" << i << "].cols\t" << outs[i].size[2] << std::endl;
			// Scan through all the bounding boxes output from the network and keep only the
			// ones with high confidence scores. Assign the box's class label as the class
			// 
			cv::Mat m0(outs[i].size[1], outs[i].size[2], CV_32F, outs[i].data + outs[i].step[0] * k);
			// with the highest score for the box.
			float* data = (float*)m0.data;
			for (int j = 0; j < m0.rows; ++j, data += m0.cols)
			{
				cv::Mat scores = m0.row(j).colRange(5, m0.cols);
				cv::Point classIdPoint;
				//double confidence;
				// Get the value and location of the maximum score
				cv::minMaxLoc(scores, 0, &confidence, 0, &classIdPoint);

				//if (confidence > g_sys_conf.ConfThresholds[classIdPoint.x]*0.0001)
                if (confidence > confThreshold)
				{
					int centerX = (int)(data[0] * vec_frame[k].cols);
					int centerY = (int)(data[1] * vec_frame[k].rows);
					int width = (int)(data[2] * vec_frame[k].cols);
					int height = (int)(data[3] * vec_frame[k].rows);
					int left = centerX - width / 2;
					int top = centerY - height / 2;

					classIds[classIdPoint.x].push_back(classIdPoint.x);
					confidences[classIdPoint.x].push_back((float)confidence);
					boxes[classIdPoint.x].push_back(cv::Rect(left, top, width, height));
				}
			}
		}
		std::vector<std::pair<int, cv::Rect> > results;
        // Perform non maximum suppression to eliminate redundant overlapping boxes with
        // lower confidences
        for (size_t i = 0; i < classes.size(); ++i)
        {
            std::vector<int> indices;
            cv::dnn::NMSBoxes(boxes[i], confidences[i], confThreshold, nmsThreshold, indices);
            for (size_t j = 0; j < indices.size(); ++j)
            {
                int idx = indices[j];
                cv::Rect box = boxes[i][idx];
                drawPred_ng(classIds[i][idx], confidences[i][idx], box.x, box.y,
                        box.x + box.width, box.y + box.height, vec_frame[k]);
                results.push_back(std::make_pair(classIds[i][idx], box));
            }
		}
		vec_results.push_back(results);
	}
}

//jinhuan+
bool sort_rect_by_x_center_ng(cv::Rect r1, cv::Rect r2)
{
	return (r1.x + r1.width / 2) < (r2.x + r2.width / 2);
}
bool sort_rect_by_y_center_ng(cv::Rect r1, cv::Rect r2)
{
	return (r1.y + r1.height / 2) < (r2.y + r2.height / 2);
}
bool sort_rect_by_height_ng(cv::Rect r1, cv::Rect r2)
{
	return (r1.height) < (r2.height);
}
bool sort_rect_by_width_ng(cv::Rect r1, cv::Rect r2)
{
	return (r1.width) < (r2.width);
}
//jinhuan-