.C

#include "stdafx.h"
#include "RobotOCV.h"

# define PI (3.14159265358979323846)

#define MAX_FIGURE_SQUARE (2000)

using namespace cv;

Mat imgHSVTest;
int hMinTest = 0, hMaxTest = 255, sMinTest = 0, sMaxTest = 255, vMinTest = 0, vMaxTest = 255;
Mat imgCannyTest;
int CannyTestSize;
Mat imgHoughCirclesTest;
int min_distTest = 14, param1Test = 210, param2Test = 11, min_radiusTest = 0, max_radiusTest = 21;

void CalibrationChessboard(Mat img) {
    namedWindow("Chessboard", WINDOW_NORMAL);
    resizeWindow("Chessboard", 600, 600);
    Size patternsize(8, 6);
    Mat gray;
    vector<Point2f> corners;
    cvtColor(img, gray, CV_BGR2GRAY);
    bool patternfound = findChessboardCorners(gray, patternsize, corners,
        CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE
        + CALIB_CB_FAST_CHECK);
    if (patternfound)
        cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
            TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
    drawChessboardCorners(img, patternsize, Mat(corners), patternfound);
    imshow("Chessboard", img);

    int numSquares = patternsize.width * patternsize.height;
    vector<vector<Point3f>> object_points;
    vector<vector<Point2f>> image_points;
    vector<Point3f> objectPoints;
    double a = 0.1;
    for (int y = 0; y < patternsize.height; ++y) {
        for (int x = 0; x < patternsize.width; ++x)
            objectPoints.push_back(Point3f(x*a, y*a, 0));
    }
    for (int n = 0; n<numSquares; ++n)
        object_points.push_back(objectPoints);

    Mat intrinsic = Mat(3, 3, CV_32FC1);
    Mat distCoeffs;
    vector<Mat> rvecs;
    vector<Mat> tvecs;

    for (int n = 0; n < numSquares; ++n)
        image_points.push_back(corners);

    intrinsic.ptr<float>(0)[0] = 1;
    intrinsic.ptr<float>(1)[1] = 1;
    calibrateCamera(object_points, image_points, img.size(), intrinsic, distCoeffs, rvecs, tvecs);

    Mat imageUndistorted;

    undistort(img, imageUndistorted, intrinsic, distCoeffs);

    imshow("win1", img);
    imshow("win2", imageUndistorted);
}

void OnChangeTestTrackBar(int i) {
    Mat out;
    if (hMinTest > hMaxTest) {
        Mat buff;
        inRange(imgHSVTest, Scalar(hMinTest, sMinTest, vMinTest), Scalar(255, sMaxTest, vMaxTest), buff);
        inRange(imgHSVTest, Scalar(0, sMinTest, vMinTest), Scalar(hMaxTest, sMaxTest, vMaxTest), out);
        out = out | buff;
    }
    else {
        inRange(imgHSVTest, Scalar(hMinTest, sMinTest, vMinTest), Scalar(hMaxTest, sMaxTest, vMaxTest), out);
    }
    imshow("TestPhoto", out);
}

void TestPhoto(char *name) {
    Mat img = imread(name);
    namedWindow("TestPhoto", WINDOW_NORMAL);
    resizeWindow("TestPhoto", 600, 600);
    cvtColor(img, imgHSVTest, CV_BGR2HSV);
    cvCreateTrackbar("HMin", "TestPhoto", &hMinTest, 255, OnChangeTestTrackBar);
    cvCreateTrackbar("hMax", "TestPhoto", &hMaxTest, 255, OnChangeTestTrackBar);
    cvCreateTrackbar("sMin", "TestPhoto", &sMinTest, 255, OnChangeTestTrackBar);
    cvCreateTrackbar("sMax", "TestPhoto", &sMaxTest, 255, OnChangeTestTrackBar);
    cvCreateTrackbar("vMin", "TestPhoto", &vMinTest, 255, OnChangeTestTrackBar);
    cvCreateTrackbar("vMax", "TestPhoto", &vMaxTest, 255, OnChangeTestTrackBar);
    namedWindow("Photo", WINDOW_NORMAL);
    resizeWindow("Photo", 600, 600);
    imshow("Photo", img);
    OnChangeTestTrackBar(0);
}

void OnChangeCannyTest(int p) {
    Mat out;
    Canny(imgCannyTest, out, CannyTestSize /2, CannyTestSize, 3);
    imshow("CannyTest", out);
}

void CannyTest(Mat img) {
    namedWindow("CannyTest", WINDOW_NORMAL);
    resizeWindow("CannyTest", 600, 600);
    imgCannyTest = img;
    cvCreateTrackbar("Canny", "CannyTest", &CannyTestSize, 255, OnChangeCannyTest);
    OnChangeCannyTest(0);
}

void OnChangeHoughCirclesTest(int p) {
    vector<Vec3f> circles;
    Mat out = imgHoughCirclesTest.clone();
    HoughCircles(out, circles, CV_HOUGH_GRADIENT, 1, min_distTest, param1Test, param2Test, min_radiusTest, max_radiusTest);
    //HoughCircles(out, circles, CV_HOUGH_GRADIENT, 3, 10, 255, 20, 0, 200);
    for (int i = 0; i < circles.size(); i++) {
        Vec3i c = circles[i];
        Point center = Point(c[0], c[1]);
        // circle center
        circle(out, center, 3, Scalar(255, 255, 255), -1, 8, 0);
    }
    imshow("HoughCirclesTest", out);
}

void HoughCirclesTest(Mat img) {
    namedWindow("HoughCirclesTest", WINDOW_NORMAL);
    resizeWindow("HoughCirclesTest", 600, 600);
    imgHoughCirclesTest = img;
    cvCreateTrackbar("min_dist", "HoughCirclesTest", &min_distTest, 255, OnChangeHoughCirclesTest);
    cvCreateTrackbar("param1", "HoughCirclesTest", &param1Test, 255, OnChangeHoughCirclesTest);
    cvCreateTrackbar("param2", "HoughCirclesTest", &param2Test, 255, OnChangeHoughCirclesTest);
    cvCreateTrackbar("min_radius", "HoughCirclesTest", &min_radiusTest, 255, OnChangeHoughCirclesTest);
    cvCreateTrackbar("max_radius", "HoughCirclesTest", &max_radiusTest, 255, OnChangeHoughCirclesTest);
    OnChangeHoughCirclesTest(0);
}


int distance2Center(int x, int y, Point center) {
    return sqrt((x - center.x) * (x - center.x) + (y - center.y) * (y - center.y));
}

void doMasc(Mat img, Mat *masc, figure_t figure, const cv_figure_t *AllMasc) {
    int i = (int)figure - 1;
    cvtColor(img, *masc, CV_BGR2HSV);
    if (AllMasc[i].hMin > AllMasc[i].hMax) {
        Mat buff1, buff2;
        inRange(*masc, Scalar(AllMasc[i].hMin, AllMasc[i].sMin, AllMasc[i].vMin), Scalar(255, AllMasc[i].sMax, AllMasc[i].vMax), buff1);
        inRange(*masc, Scalar(0, AllMasc[i].sMin, AllMasc[i].vMin), Scalar(AllMasc[i].hMax, AllMasc[i].sMax, AllMasc[i].vMax), buff2);
        *masc = buff1 | buff2;
    }
    else {
        inRange(*masc, Scalar(AllMasc[i].hMin, AllMasc[i].sMin, AllMasc[i].vMin), Scalar(AllMasc[i].hMax, AllMasc[i].sMax, AllMasc[i].vMax), *masc);
    }
    erode(*masc, *masc, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(*masc, *masc, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(*masc, *masc, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    erode(*masc, *masc, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
}

figure_t GetFigure(Mat img, Rect *rectFigure) {
    Mat hsv;
    vector<vector<Point>> contours;
    figure_t maxFoundFigure = Free_f;
    int buffDistance;
    Rect FigureRect;
    Point center = ((img.size().width / 2.0), (img.size().height / 2.0));
    int distanceFigure = 10000000;//distance2Center((img.size().width), (img.size().height), center);
    for (int i = 0; i < NUMBER_REAL_FIGURE; i++) {
        doMasc(img, &hsv, (figure_t)(i + 1), AllRealFigures);
        findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
        //waitKey(0);
        for (int j = 0; j < contours.size(); j++) {
            if (MAX_FIGURE_SQUARE < contourArea(contours[j])) {
                Rect rect = boundingRect(contours[j]);
                buffDistance = distance2Center(rect.x + (rect.width / 2.0), rect.y + (rect.height / 2.0), center);
                if (buffDistance < distanceFigure) {
                    //if (((i == 2) || (i == 5))) { //j or i
                    //  RotatedRect r_rect = minAreaRect(contours[j]);
                    //  Size size;
                    //  size.width = min(r_rect.size.height, r_rect.size.width);
                    //  size.height = max(r_rect.size.height, r_rect.size.width);
                    //  if (((i == 2) && (size.height / (double)size.width) < 2) || ((i == 5) && (size.height / (double)size.width) >= 2)) {
                    //      distanceFigure = buffDistance;
                    //      maxFoundFigure = (figure_t)(i + 1);
                    //      FigureRect = rect;
                    //  }
                    //  continue;
                    //}
                    distanceFigure = buffDistance;
                    maxFoundFigure = (figure_t)(i + 1);
                    FigureRect = rect;
                }
            }
        }
    }
    *rectFigure = FigureRect;
    return maxFoundFigure;
}


figure_t GetFigureFromStack(Mat img) {
    Mat hsv;
    vector<vector<Point>> contours;
    int squareMax = 700, squareBuff;
    figure_t maxFoundFigure = Free_f;
    for (int i = 0; i < NUMBER_REAL_FIGURE; i++) {
        doMasc(img, &hsv, (figure_t)(i + 1), AllFiguresInStack);
        findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
        for (int j = 0; j < contours.size(); j++) {
            squareBuff = contourArea(contours[j]);
            if (squareBuff > squareMax) {
                squareMax = squareBuff;
                maxFoundFigure = (figure_t)(i + 1);
            }
        }
    }
    return maxFoundFigure;
}

int getImgFrame(Mat img, Mat *imgFrame) {
    Mat hsv;
    cvtColor(img, hsv, CV_BGR2HSV);
    inRange(hsv, Scalar(0, 0, 140), Scalar(255, 255, 255), hsv);
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    vector<vector<Point>> contours;
    findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    Size imgSize = img.size();
    int indFrame = -1;
    for (int i = 0; i < contours.size(); i++) {
        for (int j = 0; j < contours[i].size(); j++) {
            int nextInd = (j + 1) % contours[i].size();
            if ((contours[i][j].y == contours[i][nextInd].y) && (contours[i][j].y == 0) &&
                (((contours[i][j].x <= (imgSize.width / 2)) && ((imgSize.width / 2) <= contours[i][nextInd].x)) ||
                ((contours[i][j].x >= (imgSize.width / 2)) && ((imgSize.width / 2) >= contours[i][nextInd].x)))) {
                indFrame = i;
            }
        }
    }
    if (indFrame == -1)
        return 1;
    RotatedRect rect = minAreaRect(contours[indFrame]);
    Rect brect = rect.boundingRect();
    //rectangle(img, brect, Scalar(0, 255, 0));
    *imgFrame = img(brect);
    return 0;
}

figure_t GetFigureFromFrame(Mat img) {
    Mat hsv, imgFrame;
    if (getImgFrame(img, &imgFrame))
        imgFrame = img;
    vector<vector<Point>> contours;
    int squareMax = 700, squareBuff;
    figure_t maxFoundFigure = Free_f;
    for (int i = 0; i < NUMBER_REAL_FIGURE; i++) {
        doMasc(imgFrame, &hsv, (figure_t)(i + 1), AllFiguresInFrame);
        findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
        for (int j = 0; j < contours.size(); j++) {
            squareBuff = contourArea(contours[j]);
            if (squareBuff > squareMax) {
                if (((i == 2) || (i == 5))) { //j or i
                    RotatedRect rect = minAreaRect(contours[j]);
                    if (((i == 2) && (rect.size.height / (double)rect.size.width) < 2.5) || ((i == 5) && (rect.size.height / (double)rect.size.width) >= 2.5)) {
                        squareMax = squareBuff;
                        maxFoundFigure = (figure_t)(i + 1);
                    }
                    continue;
                }
                squareMax = squareBuff;
                maxFoundFigure = (figure_t)(i + 1);
            }
        }
    }
    return maxFoundFigure;
}

Point GetHole(Mat img_start, figure_t figure, Rect rectFigure) {
    Mat img = img_start(rectFigure);
    Mat hsv;

    int i = (int)figure - 1;
    cvtColor(img, hsv, CV_BGR2HSV);
    inRange(hsv, Scalar(AllFiguresHoles[i].hMin, AllFiguresHoles[i].sMin, AllFiguresHoles[i].vMin), Scalar(AllFiguresHoles[i].hMax, AllFiguresHoles[i].sMax, AllFiguresHoles[i].vMax), hsv);
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));

    cvtColor(img, img, CV_RGB2GRAY);//
    bitwise_and(img, hsv, img);//
    GaussianBlur(img, img, Size(3, 3), 1);
    HoughCirclesTest(img);
    //Canny(img, img, 75, 255, 3);
    //vector<Vec3f> circles;
    //HoughCircles(img, circles, CV_HOUGH_GRADIENT, 1, 13, 100, 12, 1, 30);
    //for (int i = 0; i < circles.size(); i++) {
    //  Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
    //  int radius = cvRound(circles[i][2]);
    //  circle(img, center, 3, Scalar(255, 255, 255), -1, 8, 0);
    //}
    imshow("out", img);
    return {0, 0};
}

int cmp_left_point_x(const void *a, const void *b) {
    Point p1 = *((Point *)a), p2 = *((Point *)b);
    return p1.x - p2.x;
}

int AngFigure(Mat img, figure_t figure, double *ang) {
    Mat hsv;
    doMasc(img, &hsv, figure, AllRealFigures);
    vector<vector<Point>> contours;
    findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    int squareMax = 500, squareBuff;
    int index = -1;
    for (int j = 0; j < contours.size(); j++) {
        squareBuff = contourArea(contours[j]);
        if (squareBuff > squareMax) {
            squareMax = squareBuff;
            index = j;
        }
    }
    if (index == -1) {
        return 1;
    }
    RotatedRect rect = minAreaRect(contours[index]);
    int ang_ofset = 0;
        Mat dst, cdst;
        Canny(hsv, dst, 50, 200, 3);
        cvtColor(dst, cdst, CV_GRAY2BGR);
        vector<Vec4i> lines;
        HoughLinesP(dst, lines, 1, CV_PI / 180, 15, SupportSideLengthFigure[int(figure) - int(O_f)], 10); // Z S 33 //L J 55// точность в длине, угол, значение акума >, минимальная длина, максимальный разрыв между точкаи
        double sum_ang = 0;
        if (lines.size() == 0) {
            return 1;
        }
        *ang = 360;
        for (size_t i = 0; i < lines.size(); i++){
            Vec4i l = lines[i];
            int minInd;
            if (l[0] < l[2]) {
                minInd = 0;
            }
            else {
                minInd = 2;
            }
            double tg = (-l[minInd + 1] + l[(minInd + 2) % 4 + 1]) / (double)(-l[minInd] + l[(minInd + 2) % 4]);
            double arctan;
            if (tg == INFINITY) {
                arctan = PI / (float)2;
            }
            else {
                arctan = atan(tg);
            }
            if (O_f == figure) {
                if (fabs(atan(tg)) < fabs(*ang))
                    *ang = arctan;
            }
            else {
                if (arctan < 0)
                    arctan += PI;
                sum_ang += arctan;
            }
            line(img, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255, 255, 255), 5, CV_AA);
        }
        if (figure != O_f) {
            *ang = (sum_ang / lines.size());
            if (*ang > (PI / (float)2))
                *ang -= PI;
        }
        *ang *= (180 / PI);
        return 0;
}

Point getCenter(Mat img, figure_t figure) {
    Mat hsv;
    doMasc(img, &hsv, figure, AllRealFigures);
    vector<vector<Point>> contours;
    findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    int squareMax = 500, squareBuff;
    int index = -1;
    for (int j = 0; j < contours.size(); j++) {
        squareBuff = contourArea(contours[j]);
        if (squareBuff > squareMax) {
            squareMax = squareBuff;
            index = j;
        }
    }
    if (index == -1) {
        return {0, 0};
    }
    RotatedRect rect = minAreaRect(contours[index]);
    //circle(img, rect.center, 1, Scalar(0, 255, 0), 3);
    return rect.center;
}

int BuseFrame(Mat img, frame_t frame) {
    //Если плохо работает, то написать аккомулятор точек и выравнить изображение
    const int offset_img_x = 10,
        offset_img_y = 14,
        block_size_x = 17,
        block_size_y = 22;
    Mat imgFrame, hsv;
    if(getImgFrame(img, &imgFrame))
        return 1;
    cvtColor(imgFrame, hsv, CV_BGR2HSV);
    inRange(hsv, Scalar(0, 60, 0), Scalar(255, 255, 255), hsv);
    const int smooth = 15;
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(smooth, smooth)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(smooth, smooth)));
    dilate(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(smooth, smooth)));
    erode(hsv, hsv, getStructuringElement(MORPH_ELLIPSE, Size(smooth, smooth)));
    Size sizeFrame = imgFrame.size();
    for(int i = 0; i < FRAME_SIZE_X; i++)
        for (int j = 0; j < FRAME_SIZE_Y; j++) {
            frame[i][j] = int(hsv.at<uchar>(Point(sizeFrame.width - offset_img_x - int(block_size_x * i), offset_img_y + block_size_y * j)));
            //cout << int(hsv.at<uchar>(Point(sizeFrame.width - offset_img_x - int(block_size_x * i), offset_img_y + block_size_y * j))) << " " << i << " " << j << endl;
            //circle(imgFrame, Point(sizeFrame.width - offset_img_x - int(block_size_x * i), offset_img_y + block_size_y * j), 1, Scalar(0, 255, 0));
        }

    /*vector<vector<Point>> contours;
    int areaSum = 0;
    findContours(hsv, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    for (int j = 0; j < contours.size(); j++)
        areaSum += contourArea(contours[j]);*/
    //430 площадь одной
    return 0;

}