#include <iostream>
#include "opencv2/opencv.hpp"
using namespace cv;
using namespace cv::ml;
using namespace std;
Mat standard[8];    //用作参考标准的扇叶

//计算点间距离
double getDistance(Point A,Point B){
    return sqrt(pow((A.x-B.x),2)+pow((A.y-B.y),2));
}

//模板匹配
double TemplateMatch(cv::Mat image, cv::Mat tepl, cv::Point &point, int method)
{
    int result_cols =  image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method );

    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    switch(method){
        case TM_SQDIFF:
        case TM_SQDIFF_NORMED:
            point = minLoc;
            return minVal;
        default:
            point = maxLoc;
            return maxVal;
    }
}


void hitFan(Mat srcImage){
    //查找轮廓
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(srcImage,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE);

    RotatedRect rect_tmp;
    bool findTarget=0;

    //遍历轮廓
    if(!hierarchy.size()) return;   //无轮廓信息
    for(int i=0; i>=0; i=hierarchy[i][0]){//利用hierarcky[][0]，每次更新为下一个轮廓
        rect_tmp = minAreaRect(contours[i]);
        Point2f P[4];
        rect_tmp.points(P);

        Point2f srcRect[4];
        Point2f dstRect[4];

        double width;
        double height;

        //矫正提取的叶片的宽高
        width=getDistance(P[0],P[1]);
        height=getDistance(P[1],P[2]);
        if(width>height){
            srcRect[0]=P[0];
            srcRect[1]=P[1];
            srcRect[2]=P[2];
            srcRect[3]=P[3];
        }
        else{
            swap(width,height);
            srcRect[0]=P[1];
            srcRect[1]=P[2];
            srcRect[2]=P[3];
            srcRect[3]=P[0];
        }
        //通过面积筛选
        double area=height*width;
        if(area>5000){
            dstRect[0]=Point2f(0,0);
            dstRect[1]=Point2f(width,0);
            dstRect[2]=Point2f(width,height);
            dstRect[3]=Point2f(0,height);
            // 应用透视变换，矫正成规则矩形
            Mat transform = getPerspectiveTransform(srcRect,dstRect);
            Mat perspectMat;
            warpPerspective(srcImage,perspectMat,transform,srcImage.size());
            // 提取扇叶图片
            Mat test_img;
            test_img = perspectMat(Rect(0,0,width,height));
            //未找到扇叶
            if(test_img.empty())
                return;
            if(debug) 
                imshow("test_img",test_img);
            //开始和标准扇叶匹配
            cv::Point matchLoc;
            resize(test_img,test_img,Size(42,20));
            double score;
            vector<double> UnhitScore;
            vector<double> HitScore;
            //计算和未击打的扇叶的相似程度
            for(int j=1;j<=6;j++){
                score = TemplateMatch(test_img, standard[j], matchLoc, TM_CCOEFF_NORMED);
                UnhitScore.push_back(score);
            }
            //计算和已击打的扇叶的相似程度
            for(int j=7;j<=8;j++){
                score = TemplateMatch(test_img, standard[j], matchLoc, TM_CCOEFF_NORMED);
                HitScore.push_back(score);
            }
            //判断是否为合适击打扇叶
            int max_unhit = 0, max_hit = 0;
            for(int t1=0;t1<6;t1++){
                if(UnhitScore[t1]>UnhitScore[max_unhit]){
                    max_unhit=t1;
                }
            }
            for(int t2=0;t2<2;t2++)
            {
                if(HitScore[t2]>HitScore[max_hit])
                {
                    max_hit=t2;
                }
            }
            //击打条件：unhit的得分大于hit且unhit的得分大于阈值0.6（待调试）
            if(UnhitScore[max_unhit]>HitScore[max_hit] && UnhitScore[max_unhit]>0.6){
                //利用父轮廓
                if(hierarchy[i][2]>=0){
                    RotatedRect rect_temp=minAreaRect(contours[hierarchy[i][2]]);
                    Point2f Pnt[4];
                    rect_temp.points(Pnt);
                    //阈值参数：待调试
                    const float maxHWRatio=0.7153846;
                    const float maxArea=2000;
                    const float minArea=500;

                    float width=rect_temp.size.width;
                    float height=rect_temp.size.height;
                    if(height>width)
                        swap(height,width);
                    float area=width*height;

                    if(height/width>maxHWRatio||area>maxArea ||area<minArea){
                            continue;
                    }
                    Point centerP=rect_temp.center;
                    //确定打击点
                    circle(srcImage,centerP,1,Scalar(0,255,0),2);
                    imshow("hit point",srcImage);
                }
            }
        }
    }
}
int main(){
    //读入参考扇叶
    for(int i=1;i<9;i++){
        standard[i]=imread("./standard_fan/standard"+to_string(i)+".jpg",IMREAD_GRAYSCALE);
    }
    Mat alpha_image;
    //伪代码
    while(){//读取每一帧
        //预处理
        preTreatment();
        //寻找待击打扇叶
        hitFan(alpha_image);
    }
    
}