#include <opencv2/opencv.hpp>
#include <iostream>
#include <cmath>
#include "Eigen/Dense"
#include <algorithm>

using namespace cv;

Mat cameraIntrinsics = (Mat_<float>(3, 3)
        << 1.118687327324564e+03, 0.0, 0.0, 0, 1.135263811351046e+03, 0.0, 703.25397, 482.50630, 1.0);


std::vector<Point2f> center(std::vector<std::vector<cv::Point>> contours) {
    std::vector<Moments> mu(contours.size());
    for (int i = 0; i < contours.size(); ++i) {
        mu[i] = moments(contours[i], false);
    }
    std::vector<Point2f> mc(contours.size());
    for (int j = 0; j < contours.size(); ++j) {
        mc[j] = Point2f(mu[j].m10 / mu[j].m00, mu[j].m01 / mu[j].m00);
    }
    return mc;
}

/*
 * p0---p1
 *     /
 *    /
 *   /
 * p2---p3
 *
 * */
void sortPointsForPerspective(std::vector<Point2f> &corners) {
    for (int i = 0; i < 4; ++i) {
        for (int j = i; j > 0; j--) {
            if (corners[j].x < corners[j - 1].x)
                swap(corners[j], corners[j - 1]);
            else break;
        }
    }
    if (corners[0].y > corners[1].y) { swap(corners[0], corners[1]); }
    if (corners[2].y > corners[3].y) { swap(corners[2], corners[3]); }
    swap(corners[1], corners[2]);
}

Mat imgPreProcessing(const Mat &img) {
//    Mat img1;
//    medianBlur(img,img1,3);
    Mat imgGray;
    cvtColor(img, imgGray, COLOR_BGR2GRAY); // 转灰度图
    Mat imgBinary;
    double thresh = 50;
    threshold(imgGray, imgBinary, thresh, 255, THRESH_OTSU | THRESH_BINARY); // 转二值图
    return imgBinary;
}

Mat imgFindQr(const Mat &imgBinary, Mat img) {
    std::vector<cv::Vec4i> hierarchy;
    std::vector<std::vector<cv::Point>> contours;
    findContours(imgBinary, contours, hierarchy, RETR_TREE, CHAIN_APPROX_NONE); // 轮廓

    std::vector<Point2f> mc = center(contours);
    std::vector<Vec4i> blockConfid; // 信任特征块序号
    for (int i = 0; i < contours.size(); ++i) {
        const double minMidArea = 100; //中间块最小面积
        const double maxMidArea = pow(min(img.rows, img.cols), 2) * 25 / 441;//中间块最大面积
        int firstChild = hierarchy[i][2]; //子块
        int parent = hierarchy[i][3]; //父块
        if (firstChild < 0 || parent < 0) continue; //筛选掉无父子的轮廓
        int uncle0 = hierarchy[parent][0];
        int uncle1 = hierarchy[parent][1];
        int grand = hierarchy[parent][3];
        if ((uncle0 < 0 && uncle1 < 0) || grand < 0)continue; //筛选掉独立轮廓
        double area = contourArea(contours[i]);
        if (area < minMidArea || maxMidArea < area) continue; //面积筛选掉面积过大过小轮廓
        double ratio1 = contourArea(contours[parent]) / area,
                ratio2 = area / contourArea(contours[firstChild]);
        std::vector<float> centroidVec{mc[i].x, mc[i].y, 1.0, mc[parent].x, mc[parent].y, 1.0, mc[firstChild].x,
                                       mc[firstChild].y, 1.0};
        Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> centroid(centroidVec.data(),
                                                                                                   3, 3);
        if (34.0 / 25.0 <= ratio1 && ratio1 <= 64.0 / 25.0 //面积比例 外/中 = 49/25
            && 16.0 / 9.0 <= ratio2 && ratio2 <= 34.0 / 9.0 // 面积比例 中/内 = 25/9
            && abs(centroid.determinant()) <= 1) {//三中心三角形面积 < 1
            blockConfid.emplace_back(firstChild, i, parent, grand);
        }
    }
    int outLabelTemp = -1;
    std::vector<int> outLabel;
    for (auto &i : blockConfid) { // 挑选共同外框
        int outLabelNow = i[3];
        if (outLabelNow != outLabelTemp) {
            outLabel.push_back(outLabelNow);
            outLabelTemp = outLabelNow;
        }
    }
    int outAll[outLabel.size()][4];
    for (int i = 0; i < outLabel.size(); ++i) { outAll[i][0] = outLabel[i]; }
    int num[outLabel.size()];
    memset(num, 0, sizeof(num));
    for (auto &i : blockConfid) {
        for (int j = 0; j < outLabel.size(); ++j) {
            if (i[3] == outLabel[j]) {
                outAll[j][num[j] + 1] = i[2];
                num[j]++;
                break;
            }
        }
    }
    int num1 = 0;
    std::vector<Point2f> approx;
    std::vector<std::vector<Point2f>> quaCorners;
    std::vector<Mat> qrCodes;
    std::vector<Mat> straight_qrcode;
    std::vector<std::string> decoded_info;

    std::vector<Point2f> qrFrame(4);
    qrFrame[0] = Point2f(-20, -20);
    qrFrame[1] = Point2f(230, -20);
    qrFrame[2] = Point2f(-20, 230);
    qrFrame[3] = Point2f(230, 230);
    for (int i = 0; i < sizeof(outAll) / sizeof(outAll[0]); ++i) {
        Mat qrCode;
        approxPolyDP(contours[outAll[i][0]], approx, 3, true);
        putText(img, std::to_string(outAll[i][0]), contours[outAll[i][0]][0], FONT_HERSHEY_PLAIN, 2.0,
                Scalar(0, 0, 255), 2);
        drawContours(img, contours, outAll[i][0], Scalar(0, 0, 255), 1, 8);
        sortPointsForPerspective(approx);
        Mat perTran = getPerspectiveTransform(approx, qrFrame);
        std::cout << perTran << std::endl;
        warpPerspective(imgBinary, qrCode, perTran, Size(210, 210));

//        std::string qrframe = "qrcode";
//        qrframe.append(std::to_string(i + 1));
//        imshow(qrframe, qrCode);
        quaCorners.push_back(approx);
        qrCodes.push_back(qrCode);
    }
    return img;

}


int main(int argc, char **argv) {
//    Mat img, res, imgBinary;
//
//    img = imread("/home/ryoo/CLionProjects/QRcode/cameraOpen/qrcode.JPG");
//    resize(img, img, Size(img.cols / 4, img.rows / 4), 0, 0, INTER_LINEAR);
////    std::vector<std::string> decoded_info;
////    std::vector<Point> points;
////    std::vector<Mat> straight_qrcode;
////    QRCodeDetector qrdetector =QRCodeDetector();
////    qrdetector.detectAndDecodeMulti(img,decoded_info,points,straight_qrcode);
////    std::string name ="demo";
////
////    for (int i = 0; i < straight_qrcode.size(); ++i) {
////        name.append(std::to_string(i+1));
////        imshow(name, straight_qrcode[i]);
////
////    }
////    waitKey();
//    imgBinary = imgPreProcessing(img);
//    res = imgFindQr(imgBinary, img);
//    imshow("demo", img);
//    waitKey();


    Mat image, res, imgBinary;
    VideoCapture cap;
    cap.open(1);

    if (!cap.isOpened())
        return -1;
    cap.set(CAP_PROP_FRAME_WIDTH, 1280);
    cap.set(CAP_PROP_FRAME_HEIGHT, 720);
    std::cout<<"The Intrinsics of camera is "<<std::endl<<cameraIntrinsics<<std::endl;
    while (1) {
        cap >> image;//等价于cap.read(frame)
        imgBinary = imgPreProcessing(image);
        res = imgFindQr(imgBinary, image);
        imshow("test", res);
        char c = waitKey(66);
        if (image.empty())
            break;
        if (c == 27)
            break;

    }
    cap.release();
    destroyAllWindows();
    return 0;
}