#include "Driver/Linux/camera.hpp"
#include "Serial/serial.h"
#include "Serial/nucCommu.h"
#include "AngleSolver/AngleSolverArmor.hpp"
#include "AngleSolver/AngleSolverEnergy.hpp"
#include "Armor/Armor.hpp"
#include "opencv2/core/core.hpp"
#include "tools/CLI11.hpp"
#include <iostream>
#include <cstdio>
#include <math.h>


/* read from Video or MindVision */
#define VIDEO
// #define MINDVISION

#ifdef VIDEO
//#define VIDEOPATH "../Energy/HitWindmills/redWindmills.avi"
//#define VIDEOPATH "../Energy/HitWindmills/test.mp4"
#endif


using namespace std;
using namespace cv;

Serial *newSerialPointer;
MVCamera newCamera;
ArmorDetector newDetector;
vector<Point2f> contourPoints;
Point2f centerPoint;
ArmorType type;
AngleSolverArmor newSolverArmor;
AngleSolverEnergy newSolverEnergy;
float yaw, pitch, r;
bool video, noSerial, debug, savepic, manually, showframe;
string videopath;

Color color = BLUE;

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);
                }
            }
        }
    }
}
 
/* distance between 2D points*/
double getPotDistance(Point2f a, Point2f b) {
    return (double)sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

/* distance between 3D points*/
double getPotDistance(Point3f a, Point3f b) {
    return (double)sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
}

Point2f getCrossPoint(Vec3f l1, Vec3f l2) {
    double A1 = l1[0], B1 = l1[1], C1 = l1[2];
    double A2 = l2[0], B2 = l2[1], C2 = l2[2];
    double D = A1 * B2 - A2 * B1;
    return Point2f((B1 * C2 - B2 * C1) / D, (A1 * C2 - A2 * C1) / D);
}

double getDirection(Point2f a, Point2f b) {
    double dx = b.x - a.x, dy = b.y - a.y;
    double adx = abs(dx), ady = abs(dy);
    double dir = (ady < adx) ? (atan(ady / adx)) : (CV_PI / 2 - atan(adx / ady));
    if(dx < 0) dir = CV_PI - dir;
    if(dy < 0) dir = -dir;
    while(dir < 0) dir += CV_2PI;
    while(dir > CV_2PI) dir -= CV_2PI;
    return dir;
}

Point2f getRectCross(RotatedRect A, RotatedRect B) {
    Vec3f last_line(
        sin(A.angle * CV_PI / 180),
        cos(A.angle * CV_PI / 180),
        A.center.y * cos(A.angle * CV_PI / 180) - A.center.x * sin(A.angle * CV_PI / 180));

    Vec3f this_line(
        sin(B.angle * CV_PI / 180),
        cos(B.angle * CV_PI / 180),
        B.center.y * cos(B.angle * CV_PI / 180) - B.center.x * sin(B.angle * CV_PI / 180));

    return getCrossPoint(last_line, this_line);
}

bool inside(Size o, Point2f i) {
    return 0.0 < i.x && 0.0 < i.y && i.x < o.width && i.y < o.height;
}

int main1(Color color) {
    newDetector.setEnemyColor(color);
    newDetector.setTargetNum(1);
    newDetector.loadSVM("../Armor/123svm.xml");
    if(!video && newCamera.OpenCamera() == newCamera.NO_CAMERA) {
        printf("No camera loaded!\n");
        return 0;
    }
    
    VideoCapture *cap;
    if(video) {
        cap = new VideoCapture(videopath);
        if (!cap->isOpened()) {
            cout << "Can not open the Video!" << endl;
            return 0;
        }
    }

    Mat CamImg;
    while(!video || cap->read(CamImg)){
        if(!video) CamImg = newCamera.ReadCamera();
        //cout << CamImg << endl;
        //cout << "@" << endl;
        newDetector.run(CamImg);
        //cout << "#" << endl;

        if(debug) newDetector.showDebugInfo(1, 1, 1, 1, 1, 1, 1);

        if(newDetector.isFoundArmor())
        {
            newDetector.getTargetInfo(contourPoints, centerPoint, type);
            newSolverArmor.solve(contourPoints, centerPoint, CamImg.size(), yaw, pitch, r);
            // Chenzhuoxun 2022-7-13 22:02 : i just turn it into real pitch and real yaw\
            //
            if(yaw > CV_PI)
                yaw -= CV_2PI;
            pitch = CV_PI/2 - pitch;
            if(debug) cout << "Solved: " << yaw << " " << pitch << " " << r << endl;
            if(!noSerial) newSerialPointer->sendTargetData(pitch, yaw, r, ((color == BLUE) ? COLOR_BLUE : COLOR_RED), 1, 1);
        }
        else if(!noSerial) newSerialPointer->sendTargetData(0.0f, 0.0f, 0.0f, ((color == BLUE) ? COLOR_BLUE : COLOR_RED), 0, 0);
        //cout << "$" << endl;
        if(!noSerial) waitKey(50);
        else if(debug) waitKey(1);
    }
    //cout << "=====================================================================" << endl;
    if(!video) newCamera.CloseCamera();
    else {
        cap->release();
        delete cap;
    }
    return 0;
}

/* the Operate System */
//#define WINDOWS
#define LINUX

/* Save or Show pictures */
// #define SAVEPIC
//#define DEBUG_1   // For adjust 1: Please make sure both SHOWPIC and DEBUG_1 are ON!!
// #define DEBUG_2 // For adjust 2: Please make sure both SHOWPIC and DEBUG_2 are ON!!

RotatedRect ThisEnergy;
vector<RotatedRect> rect_vec;

int DetectEnergy(Mat image, Mat &binary)
{
    vector<vector<Point>> contours; // idk if i can change the type...Point to Point2f
    findContours((Mat_<uchar>)image, contours, RETR_LIST, CHAIN_APPROX_NONE);

    int circle_cnt = 0;
    rect_vec.clear();

    for (size_t i = 0; i < contours.size(); i++)
    {

        vector<Point> points;
        double area = contourArea(contours[i]);
        if (area < (video ? 50 : 30) || 1e4 < area)
            continue;

        /**##############################################################
         * the floodfill will reduce the area
         */
        drawContours(binary, contours, static_cast<int>(i), Scalar(255), 2);
        points = contours[i];
        RotatedRect rrect = fitEllipse(points);
        cv::Point2f *vertices = new cv::Point2f[4];
        rrect.points(vertices);

        double aim = rrect.size.height / rrect.size.width;

        /*
        if(debug) {
            cv::namedWindow("DEBUG2", 0);
            cv::resizeWindow("DEBUG2",1240,960);
            imshow("DEBUG2", image);
            waitKey(1);
        }*/

        //#############the square may be much higher than 2.6##################
        if (aim > 1.2 && aim < (video ? 2.6 : 4))
        {
            // draw rectangle
            for (int j = 0; j < 4; j++) {
                cv::line(binary, vertices[j], vertices[(j + 1) % 4], cv::Scalar(0, 255, 0), 1);
            }
            rect_vec.push_back(rrect);
            // for (int j = 0; j < 4; j++)
            // cout << "j = " << vertices[j] << "\t(j+1)%4 = " << vertices[(j+1)%4] << endl;
            double middle = 100000;

            for (size_t j = 0; j < contours.size(); j++)
            {
                if(i == j) continue;
                vector<Point> pointsA;
                double area = contourArea(contours[j]);
                /**##############################################################
                 * th e floodfill will reduce the area %%%%%%%%%attention: there are 2 area in this code
                 */
                if (area < (video ? 50 : 10) || 1e4 < area)
                    continue;

                pointsA = contours[j];

                RotatedRect rrectA = fitEllipse(pointsA);

                circle(binary, rrectA.center, 2, Scalar(255, 255, 0));

                double aimA = rrectA.size.height / rrectA.size.width;
                // cout << aimA << endl;
                //  adjust here
                if (aimA > (video ? 3.0 : 1.7))
                {
                    // if (aimA > 3) {
                    //  adjust here
                    double distance = sqrt((rrect.center.x - rrectA.center.x) * (rrect.center.x - rrectA.center.x) +
                                          (rrect.center.y - rrectA.center.y) * (rrect.center.y - rrectA.center.y));

                    if (middle > distance)
                        middle = distance;
                    //cout << distance << "\t";
                }
            }
            // *** You need to adjust threshold ***
            // To See the hit point
            // Adjust Here
            // cout << middle << endl;

            if (middle > (video ? 100 : 10))
            {   //##########################important for find######################
                // Adjust Here
                circle_cnt++;
                ThisEnergy = rrect;
            }
        }
        delete vertices;
    }
    return circle_cnt;
}

const double OFFSET_MIN = 0.02;
const double OFFSET_STEP = 0.02;
double T = 5;

int main2(){

    //for(int i = 1; i <= 1000; i++) cout << getDirection(Point2f(0, 0), Point2f(cos(i), sin(i))) << endl;

    VideoCapture *cap;

    if(video) {
        cap = new VideoCapture(videopath);
        if (!cap->isOpened()) {
            cout << "Can not open the Video!" << endl;
            return 0;
        }
    }
    // TODO: input from mindVision
    else {
        if(newCamera.OpenCamera() == newCamera.NO_CAMERA) {
            cout << "No camera loaded!" << endl;
            return 0;
        } else {
            cout << "Camera loaded!" << endl;
        }
    }

    int stateNum = 5;
    int measureNum = 4;
    KalmanFilter KF(stateNum, measureNum, 0, CV_64F);
    Mat measurement = Mat::zeros(measureNum, 1, CV_64F);

    KF.transitionMatrix = (Mat_<double>(stateNum, stateNum) << 
        1, 0, 0, 0, 0,
        0, 1, 0, 0, 1,
        0, 0, 1, 0, 0,
        0, 0, 0, 1, 0,
        0, 0, 0, 0, 1);

    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(1));
    //randn(KF.statePost, Scalar::all(100), Scalar::all(10));

    int cnt_origin = 0;
    int cnt_process = 0;
    //int detectNum = 0;
    double gate_alpha_avg = 80;    //the color intensity threshold
    double gate_prefix = 1.0;
    double offset = OFFSET_MIN;
    double color_mask_avg = 1.0;
    double rand_range = 1.0;
    double colorsum_max = 0.0;
    int sgn = 1;
    int waitcnt = 0;
    int miscnt = 0;
    RNG randeng;
    Mat ImgSrc;
    unsigned long long around = 0;
    bool aroundflag = true;
    if(manually) {
        cout << "gate_alpha_avg: ";
        cin >> gate_alpha_avg;
        cout << "color_mask_avg: ";
        cin >> color_mask_avg;
    }
    RotatedRect LastEnergy(Point2f(1.0, 1.0), Size2f(1.0, 1.0), -1000.0);
    RotatedRect TempEnergy(Point2f(1.0, 1.0), Size2f(1.0, 1.0), -1000.0);
    double Theta0 = 0.0;
    while(!video || cap->read(ImgSrc)) {
        cout << endl;
        if(!video) ImgSrc = newCamera.ReadCamera();

        double areasum = ImgSrc.size().height * ImgSrc.size().width;
        //cout << image.size().height << " " << image.size().width << endl;
        //colorsum_max = areasum;
        //cout << colorsum_max << endl;

        if(savepic) {
            cout << cv::imwrite("../Energy/FindWindmills/OriginPic/" + to_string(++cnt_origin) + ".jpg", ImgSrc) << endl;
            cout << "OriginPic\t" << cnt_origin << "\tis writing..." << endl;
        }

        Mat image, binary;
        vector<Mat> imagesplit;
        double gate_alpha_sum = 0, color_mask_sum = 0;
        int match_cnt = 0;
        for(int i = 1; i <= (manually ? 1 : T); i++) {
            binary = image = ImgSrc;
            // cout << "#" << endl;
            //resize(image, image, Size(image.cols * 0.5, binary.rows * 0.5));
            // cout << "#" << endl;
            //resize(binary, binary, Size(binary.cols * 0.5, binary.rows * 0.5));
            // cout << "#" << endl;

            Mat Thres;
            cvtColor(image, Thres, COLOR_BGR2GRAY);
            double colmin, colmax;
            minMaxLoc(Thres, &colmin, &colmax);
            if(colmax > 254) {
                cout << "DownAeTarget();" << endl;
                newCamera.DownAeTarget();
            }
            if(colmax < 200) {
                cout << "UpAeTarget();" << endl;
                newCamera.UpAeTarget();
            }

            split(image, imagesplit);
            double color_range = manually ? 0 : 0.05;
            double color_rand = randeng.uniform(-color_range * rand_range, color_range * rand_range);
            double color_mask = color_mask_avg + color_rand;
            //double color_mask = color_mask_avg;
            
            // image = (imagesplit[2] * (1 - color_mask) + imagesplit[0] * color_mask);
            // blue
            if(color == BLUE)
                image = ((imagesplit[0] - imagesplit[2] * color_mask));
            else
                image = ((imagesplit[2] - imagesplit[0] * color_mask));
            // red

            minMaxLoc(image, &colmin, &colmax);
            image = ((image - colmin) * 255 / (colmax - colmin));
            Mat color_image;
            image.copyTo(color_image);
            // cout << image.depth() << endl;

            // cvtColor(image, image, COLOR_BGR2GRAY);

            // *** You need to adjust threshold ***
            // To See the Windmills
            /*
             * threshole(src, dst, thresh, maxval, type)
             *    src -- source pic
             *    dst -- output pic
             *    thresh -- thresh  smaller->more clear
             *    maxval -- the max val in dst   bigger->brighter in debug picture
             *    type -- the type of thresh:
             *        THRESH_BINARY\THRESH_BINARY_INV\THRESH_TOZERO\THRESH_TOZERO_INV
             *
             */

            // if(video) threshold(image, image, 80, 255, THRESH_BINARY);
            // else threshold(image, image, 20, 255, THRESH_BINARY);
            double alpha_range = manually ? 0 : 5;
            double alpha_rand = randeng.uniform(-alpha_range * rand_range, alpha_range * rand_range);
            
            double gate_alpha = gate_alpha_avg + alpha_rand;
            //double gate_alpha = gate_alpha_avg;
            threshold(image, image, gate_alpha, 255, THRESH_BINARY);
            Mat alpha_image;
            image.copyTo(alpha_image);

            if (debug)
            {
                cv::namedWindow("Ways substract", 1);
                imshow("Ways substract", color_image);
                waitKey(1);
            }

            if (debug)
            {
                cv::namedWindow("Bi-valued", 1);
                imshow("Bi-valued", alpha_image);
                waitKey(1);
            }

            //hitFan(alpha_image);

            double alphasum = sum(alpha_image)[0] / 255, bound = areasum * gate_prefix / 20;
            if (sgn == 0 && alphasum > bound)
                gate_alpha_avg += 10;

            Mat diff_image;
            absdiff(color_image, alpha_image, diff_image);
            double colorsum = sum(diff_image)[0] / 255;

            /*if(debug) {
                cv::namedWindow("DEBUG3", 1);
                imshow("DEBUG3", diff_image);
                waitKey(1);
            }*/

            // cout << color_image << endl;

            // cout << bound << " ~ " << alphasum << " ";
            // cout << gate_alpha << "->";
            // if(fabs((alphasum - bound) / areasum) > 0.005) gate_alpha += (alphasum - bound) * 5000 / (areasum * 255);
            // cout << gate_alpha << endl;

            // When in Fight, use WindMills
            // threshold(image, image, 30, 255, THRESH_BINARY);
            // Adjust Here

            //#############################################################################
            //"20"can be changed with holo(aperture)
            // Adjust Here


            dilate(image, image, Mat());
            erode(image, image, Mat());
            dilate(image, image, Mat());
            erode(image, image, Mat());

            // *** You need to adjust threshold ***
            /*
             * int floodFill(InputArray image, Point seedPoint, Scalar newVal,
             *      Rect* rect=0,Scalar ioDiff=Scalar(), Scalar upDiff=Scalar(),
             *      int flags=4)
             * image: input image
             * seedPoint: the flood fill algorithm's start point
             * newVal: the new color which is be dyed   {255 for complete white; 200 for human to look}
             * loDiff: val �� newVal - loDiff
             * upDiff: val �� upDiff - newVal
             * flags: FLOODFILL_FIXED_RANGE(assign color) / FLOODFILL_MASK_ONLY(assign position)
             * {may need to add another line for floodfill}
             */
            // Adjust Here

            if (video)
                floodFill(image, Point(5, 50), Scalar(255), 0, FLOODFILL_FIXED_RANGE);
            else
                floodFill(image, Point(5, 50), Scalar(150), 0, FLOODFILL_MASK_ONLY);
            // Adjust Here

            threshold(image, image, 80, 255, THRESH_BINARY_INV);

            if (debug)
            {
                cv::namedWindow("Flood fill", 1);
                imshow("Flood fill", image);
                waitKey(1);
            }

            /*
            alphasum = sum(image)[0] / 255, bound = areasum * gate_prefix / 400;
            cout << bound << " ~ " << alphasum << " ";
            cout << gate_alpha << "->";
            if(fabs((alphasum - bound) / areasum) > 0.5) gate_alpha += (alphasum - bound) * 50000 / (areasum * 255);
            cout << gate_alpha << endl;*/

            int detectnum = DetectEnergy(image, binary);
            cout << detectnum << endl;

            if (detectnum == 1)
            {
                match_cnt++;


                // gate_prefix += (alphasum / bound - gate_prefix) * 0.1;
                // gate_alpha = gate_alpha + 255 * offset * sgn / 2.0;
                // offset = OFFSET_MIN;

                gate_alpha_sum += gate_alpha;
                color_mask_sum += color_mask;
                
                //gate_alpha_avg = (gate_alpha_avg + gate_alpha * 9) / 10;
                //color_mask_avg = (color_mask_avg + color_mask * 9) / 10;
                /*if(1.0 / colorsum > colorsum_max) {
                    color_mask_avg = (color_mask_avg + color_mask * 9) / 10;
                    colorsum_max = 1.0 / colorsum;
                }*/
                // else continue;
                //if (rand_range > 0.05)
                //    rand_range *= 0.95;
            }

            // if(sgn == 0) offset += OFFSET_STEP;
            // if((gate_alpha + 255 * offset) > 255 && offset * 255 > gate_alpha) offset = OFFSET_MIN;
            // sgn = (sgn + 2) % 3 - 1;

            // waitKey(500);
        }
        if(match_cnt == 0 || getPotDistance(ThisEnergy.center, LastEnergy.center) > 1000) {
            // colorsum_max *= 0.99;

            T = 5;
            if (rand_range < 5.0) rand_range *= 1.05;
            if (!noSerial) newSerialPointer->sendTargetData(randeng.gaussian(1.0), 0.0f, 0.0f, COLOR_RED, 0, 0);
            if (waitcnt++ > 10 && !manually) {
                waitcnt = 0;
                gate_alpha_avg = randeng.uniform(0, 255);
            }

            /*if(showframe)
            {
                cv::namedWindow("frame", 1);
                imshow("frame", ImgSrc);
                waitKey(1);
            }*/
        }
        else {
            //if(T > 1) T *= 0.95;
            waitcnt = 0;
            gate_alpha_avg = gate_alpha_sum / match_cnt;
            color_mask_avg = color_mask_sum / match_cnt;

            cv::Point2f *vertices = new cv::Point2f[4];
            ThisEnergy.points(vertices);
            if(inside(binary.size(), ThisEnergy.center)) cv::circle(binary, Point(ThisEnergy.center.x, ThisEnergy.center.y), 15, cv::Scalar(0, 0, 255), 4);
            for(vector<RotatedRect>::iterator it = rect_vec.begin(); it != rect_vec.end(); it++) if(inside(binary.size(), it->center)) cv::circle(binary, it->center, 2, Scalar(255, 34, 255), -1);
            
            Mat prediction;
            KF.predict().copyTo(prediction);
            for(int i = 1; i <= 0; i++) prediction = KF.transitionMatrix * prediction;
            double predict_radius = prediction.at<double>(0), predict_theta = prediction.at<double>(1);
            Point2f Predict_Center = Point2f(prediction.at<double>(2), prediction.at<double>(3));
            // cout << "Here:  " <<prediction.at<double>(0)<<"\t"<<prediction.at<double>(1)<<endl;
            // cout << predict_theta << " " << measure_Theta << endl;

            //cout << KF.statePre << KF.statePre.at<double>(4) << endl;
            Point2f Predict_Pt;
            double Rotate_Speed = KF.statePre.at<double>(4) * 10;
            if(abs(Rotate_Speed) > 0.2) Rotate_Speed *= 0.2 / abs(Rotate_Speed);
            Predict_Pt.x = ThisEnergy.center.x;// + predict_radius * cos(getDirection(Predict_Center, ThisEnergy.center) + Rotate_Speed);
            Predict_Pt.y = ThisEnergy.center.y;// + predict_radius * sin(getDirection(Predict_Center, ThisEnergy.center) + Rotate_Speed);
            //cout << Predict_Pt << endl;

            double Theta;// = (ThisEnergy.angle + (ThisEnergy.center.x > LastEnergy.center.x ? 180 : 0)) * CV_PI / 180;
            
            //if(Theta - Theta0 > 3) Theta -= CV_PI;
            double Radius = 0, CenterX = 0, CenterY = 0;
            Theta = 0;
            cout << rect_vec.size() << endl;

            if (rect_vec.size() == 1)
            {
                // if (abs(ThisEnergy.center.x - LastEnergy.center.x) < 0.5) Radius = (ThisEnergy.center.y - LastEnergy.center.y) / (sin(Theta) - sin(Theta0));
                // else
                // Radius = (ThisEnergy.center.x - LastEnergy.center.x) / (cos(Theta) - cos(Theta0));
                // CenterX = ThisEnergy.center.x - Radius * cos(Theta);
                // CenterY = ThisEnergy.center.y - Radius * sin(Theta);

                Point2f CrossCenter = getRectCross(LastEnergy, ThisEnergy);

                Theta = getDirection(CrossCenter, ThisEnergy.center);
                Radius = getPointsDistance(CrossCenter, ThisEnergy.center);
                CenterX = CrossCenter.x;
                CenterY = CrossCenter.y;

                
            }
            else
            {
                for (vector<RotatedRect>::iterator it = rect_vec.begin(); it != rect_vec.end();)
                {
                    RotatedRect LastRect = *it;

                    it++;
                    if (it == rect_vec.end())
                        break;

                    RotatedRect ThisRect = *it;

                    Point2f CrossCenter = getRectCross(LastRect, ThisRect);

                    //Theta += getDirection(CrossCenter, ThisEnergy.center) / (rect_vec.size() - 1);
                    Radius += getPointsDistance(CrossCenter, ThisEnergy.center) / (rect_vec.size() - 1);
                    CenterX += CrossCenter.x / (rect_vec.size() - 1);
                    CenterY += CrossCenter.y / (rect_vec.size() - 1);
                }
                Theta = getDirection(Point2f(CenterX, CenterY), ThisEnergy.center);
            }

            /*if(abs(ThisEnergy.center.x - LastEnergy.center.x) < 0.5) {
                Theta = Theta0 + (ThisEnergy.center.y - LastEnergy.center.y) * (ThisEnergy.center.y < Predict_Pt.y ? 1 : -1) / predict_radius;
                cout << "#####" << endl;
            }*/

            // cout << ThisEnergy.center.x - LastEnergy.center.x << endl;
            // cout << ThisEnergy.center.y << " " << Radius * sin(Theta) << endl;
            if (aroundflag && Theta0 - Theta > 3) around++, aroundflag = false;
            if(!aroundflag && abs(ThisEnergy.center.x - CenterX) / Radius < 0.98) aroundflag = true;
            double measure_Theta = Theta + around * CV_PI * 2;
            //cout << Theta0 << " " << Theta << endl << measure_Theta << " " << around << " " << aroundflag << endl;
            Theta0 = Theta;

            //cout << KF.gain << endl;
            //cout << ThisEnergy.center << LastEnergy.center << getPointsDistance(ThisEnergy.center, LastEnergy.center) << endl;
            //cout << Radius << " " << Theta0 << " " << Theta << endl;
            if(rect_vec.size() > 1 && getPointsDistance(ThisEnergy.center, LastEnergy.center) > 80) {
                cout << "########## KF reset ##########" << endl;
                //delete KF;
                KF.init(stateNum, measureNum, 0, CV_64F);
                KF.transitionMatrix = (Mat_<double>(stateNum, stateNum) << 
                    1, 0, 0, 0, 0,
                    0, 1, 0, 0, 1,
                    0, 0, 1, 0, 0,
                    0, 0, 0, 1, 0,
                    0, 0, 0, 0, 1);

                setIdentity(KF.measurementMatrix);
                setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
                setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
                setIdentity(KF.errorCovPost, Scalar::all(1));
                //setIdentity(KF.gain, Scalar::all(1));
                //KF.statePost = Mat(Vec<double, 4>(Radius, measure_Theta, CenterX, CenterY));
                //KF.statePost.at<double>(4) = 0.0;
                //cout << KF.statePost << endl;
                KF.predict();
                //cout << KF.statePost << endl;
            }

            /*if (abs(ThisEnergy.angle - TempEnergy.angle) > 0.1){
                LastEnergy = TempEnergy;
                TempEnergy = ThisEnergy;
            }*/

            if(rect_vec.size() > 1 || abs(ThisEnergy.angle - LastEnergy.angle) > 100) LastEnergy = ThisEnergy;

            //KF.statePost.at<double>(4) = 0;

            measurement.at<double>(0) = Radius;
            measurement.at<double>(1) = measure_Theta;
            measurement.at<double>(2) = CenterX;
            measurement.at<double>(3) = CenterY;
            KF.correct(measurement);
            //cout << KF.statePost << endl;

            if(inside(binary.size(), Predict_Center)) cv::circle(binary, Predict_Center, 4, Scalar(34, 255, 255), -1);
            if(inside(binary.size(), Predict_Pt)) cv::circle(binary, Predict_Pt, 4, Scalar(255, 255, 34), -1);

            //ThisEnergy.center.x = CenterX;// + Radius * cos(Theta);
            //ThisEnergy.center.y = CenterY;// + Radius * sin(Theta);
            //ThisEnergy.center.x = Predict_Center.x;// + predict_radius * cos(predict_theta);
            //ThisEnergy.center.y = Predict_Center.y;// + predict_radius * sin(predict_theta);

            /*Old Version
            Mat prediction = KF.predict();
            Point predict_pt = Point((int)prediction.at<double>(0), (int)prediction.at<double>(1));
            // cout << "Here:  " <<prediction.at<double>(0)<<"\t"<<prediction.at<double>(1)<<endl;
            measurement.at<double>(0) = (double)rrectm.center.x;
            measurement.at<double>(1) = (double)rrectm.center.y;
            KF.correct(measurement);

            cv::circle(binary, predict_pt, 4, Scalar(34, 255, 255), -1);

            rrectm.center.x = (int)prediction.at<double>(0);
            rrectm.center.y = (int)prediction.at<double>(1);*/

            // Finded Target
            vector<Point2f> armor_Points;
            double a = getPotDistance(vertices[0], vertices[1]);
            double b = getPotDistance(vertices[1], vertices[2]);
            Point2f p1(0.0f, 0.0f);
            Point2f p2(a, 0.0f);
            Point2f p3(a, b);
            Point2f p4(0.0f, b);
            armor_Points.push_back(p1);
            armor_Points.push_back(p2);
            armor_Points.push_back(p3);
            armor_Points.push_back(p4);

            newSolverEnergy.solve(armor_Points, Predict_Pt, image.size(), yaw, pitch, r);
            // if(debug) cout << "Solved:  yaw = " << yaw << "\tpitch = "<<  pitch << "\tDistance = " << r <<  "m" << endl;

            if (yaw > CV_PI)
                yaw -= CV_2PI;
            pitch = CV_PI / 2 - pitch;
            cout << pitch << " " << yaw << " " << r << endl;

            if (!noSerial)
            {
                newSerialPointer->sendTargetData(pitch / 10, yaw / 10, r, COLOR_RED, 1, 1); //����
            }
            delete vertices;
        }

        if (showframe)
        {
            cv::namedWindow("frame", 1);
            imshow("frame", binary);
            waitKey(1);
        }

        if(match_cnt > 0 && rand_range > 0.05) rand_range *= 0.95;

        //cout << gate_alpha_avg << " " << color_mask_avg << endl;

        if(savepic) {
            cout << cv::imwrite("../Energy/FindWindmills/ProcessedPic/" + to_string(++cnt_process) + ".jpg", binary) << endl;
            cout << "ProcessedPic\t" << cnt_process << "\tis writing..." << endl;
        }

        if(!noSerial) waitKey(50);
        //waitKey(50);
        //waitKey(1000);
    }
    //delete KF;
    if(video) {
        cap->release();
        delete cap;
    }
    else newCamera.CloseCamera();
    return 0;
}

int main(int argc, char **argv) {

    for(int i=1;i<9;i++){
        standard[i]=imread("./standard_fan/standard"+to_string(i)+".jpg",IMREAD_GRAYSCALE);
    }

    CLI::App args{"RM Vision"};

    char MODE = 'E';
    char COLOR = 'R';
    bool NOSERIAL = false;
    string USEVIDEO;
    bool DBG = false;
    bool SAVE = false;
    bool MANUAL = false;
    bool SHOWFRAME = false;

    args.add_option("-m", MODE, "(A)rmor or (E)nergy");
    args.add_option("-c", COLOR, "Set color R/B");
    args.add_flag("-s", NOSERIAL, "Close serial");
    args.add_option("-v", USEVIDEO, "Use video");
    args.add_flag("-a", MANUAL, "Adjust manually");
    args.add_flag("-d", DBG, "Debug mode");
    args.add_flag("-p", SAVE, "Save picture");
    args.add_flag("-f", SHOWFRAME, "Show frame");

    CLI11_PARSE(args, argc, argv);

    video = !USEVIDEO.empty();
    videopath = USEVIDEO;
    noSerial = NOSERIAL;
    debug = DBG;
    savepic = SAVE;
    manually = MANUAL;
    showframe = SHOWFRAME;

    if(COLOR == 'R') color = RED;

    if(!NOSERIAL) newSerialPointer = new Serial(115200);

    if(MODE == 'A') main1(color);
    else main2();
    
    if(!NOSERIAL) delete newSerialPointer;
    return 0;
}