#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "circlefitsolver.h"
#include <iostream>

const double CENTER_X = 120;
const double CENTER_Y = 150;
const double RADIUS = 99;
const double PI = 3.14159;

using namespace cv;

static int rd()
{
    return rand() % 9 - 4;
}

static void test()
{
    vector<POINT> points;
    for(int i = 0; i < 300; i++)
    {
        int x = CENTER_X + RADIUS * cos(i / 180.0 * M_PI) + rd();
        int y = CENTER_Y + RADIUS * sin(i / 180.0 * M_PI) + rd();

        cout << x << " " << y << endl;
        points.push_back(POINT(x, y));
    }
    for(int i = 0; i < 60; i++)
    {
        int x = 50 + RADIUS * cos(i / 60.0 * 2 * M_PI) / 10.0;
        int y = 50 + RADIUS * sin(i / 60.0 * 2 * M_PI) / 10.0;

        cout << x << " " << y << endl;
        points.push_back(POINT(x, y));
    }

    double x, y ,r = -1;
    CircleFitSolver solver;
    r = -1;
    solver.circleFitL1(points, x, y, r);
    //circleLeastFit(points, x, y, r);

    cerr << x << endl;
    cerr << y << endl;
    cerr << r << endl;

}

void test3()
{
    CircleFitSolver solver;
    double x, y, r;
    Mat grayImage = imread("Q:/10.jpg", CV_LOAD_IMAGE_GRAYSCALE);
    cv::threshold(grayImage, grayImage, 128, 255, CV_THRESH_OTSU);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_CLOSE, kernel);
    cv::Canny(grayImage, grayImage, 128, 200, 3);
    std::vector<POINT> points;

    for(int row = 0; row < grayImage.rows; row++)
    {
        const uchar * pLine = grayImage.ptr<const uchar>(row);
        for(int col = 0; col < grayImage.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(POINT(col, row));
            }
        }
    }
    circleLeastFit(points, x, y, r);
    cv::circle(grayImage, cv::Point(x, y), r, cv::Scalar(255), 2);

    // 抹去外圆
    std::vector<POINT> p2;
    int N = points.size();
    for(int i = 0; i < N; i++)
    {
        int col = points[i].real();
        int row = points[i].imag();
        if(hypot(col - x, row - y) < r)
        {
            p2.push_back(POINT(col, row));
        }
    }
    solver.circleFitL1(p2, x, y, r);
    // 再次拟合就只剩下内圆了
    //circleLeastFit(p2, x, y, r);
    cv::circle(grayImage, cv::Point(x, y), r, cv::Scalar(255), 2);
    cv::namedWindow("image2", cv::WINDOW_NORMAL);
    cv::imshow("image2", grayImage);
}