import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
from Ransac_Process import RANSAC
 
 
 
def lj_img(img):
    wlj, hlj = img.shape[1], img.shape[0]
    lj_dis = 7      # 連接白色區(qū)域的判定距離
    for ilj in range(wlj):
        for jlj in range(hlj):
            if img[jlj, ilj] == 255:    # 判斷上下左右是否存在白色區(qū)域并連通
                for im in range(1, lj_dis):
                    for jm in range(1, lj_dis):
                        if ilj - im >= 0 and jlj - jm >= 0  and img[jlj - jm, ilj - im] == 255:
                            cv2.line(img, (jlj, ilj), (jlj - jm, ilj - im), (255, 255, 255), thickness=1)
                        if ilj + im < wlj and jlj + jm < hlj and img[jlj + jm, ilj + im] == 255:
                            cv2.line(img, (jlj, ilj), (jlj + jm, ilj + im), (255, 255, 255), thickness=1)
    return img
 
def cul_area(x_mask, y_mask, r_circle, mask):
    mask_label = mask.copy()
    num_area = 0
    for xm in range(x_mask+r_circle-10, x_mask+r_circle+10):
        for ym in range(y_mask+r_circle-10, y_mask+r_circle+10):
            # print(mask[ym, xm])
            if (pow((xm-x_mask), 2) + pow((ym-y_mask), 2) - pow(r_circle,  2)) == 0 and mask[ym, xm][0] == 255:
                num_area += 1
                mask_label[ym, xm] = (0, 0, 255)
    cv2.imwrite('./test2/mask_label.png', mask_label)
    print(num_area)
    return num_area
 
def mainFigure(img, point0):
    # params = cv2.SimpleBlobDetector_Params()  # 黑色斑點(diǎn)面積大小：1524--1581--1400--周?chē)蓴_面積: 1325--1695--1688--
    # # Filter by Area.   設(shè)置斑點(diǎn)檢測(cè)的參數(shù)
    # params.filterByArea = True  # 根據(jù)大小進(jìn)行篩選
    # params.minArea = 10e2
    # params.maxArea = 10e4
    # params.minDistBetweenBlobs = 40  # 設(shè)置兩個(gè)斑點(diǎn)間的最小距離 10*7.5
    # # params.filterByColor = True             # 跟據(jù)顏色進(jìn)行檢測(cè)
    # params.filterByConvexity = False  # 根據(jù)凸性進(jìn)行檢測(cè)
    # params.minThreshold = 30  # 二值化的起末閾值,只有灰度值大于當(dāng)前閾值的值才會(huì)被當(dāng)成特征值
    # params.maxThreshold = 30 * 2.5  # 75
    # params.filterByColor = True  # 檢測(cè)顏色限制，0黑色，255白色
    # params.blobColor = 255
    # params.filterByCircularity = True
    # params.minCircularity = 0.3
 
    point_center = []
    # cv2.imwrite('./test2/img_source.png', img)
    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    # cv2.imwrite('./test2/img_hsv.png', img_hsv)
    w, h = img.shape[1], img.shape[0]
    w_hsv, h_hsv = img_hsv.shape[1], img_hsv.shape[0]
    for i_hsv in range(w_hsv):
        for j_hsv in range(h_hsv):
            if img_hsv[j_hsv, i_hsv][0] < 200 and img_hsv[j_hsv, i_hsv][1] < 130 and img_hsv[j_hsv, i_hsv][2] > 120:
                # if hsv[j_hsv, i_hsv][0] < 100 and hsv[j_hsv, i_hsv][1] < 200 and hsv[j_hsv, i_hsv][2] > 80:
                img_hsv[j_hsv, i_hsv] = 255, 255, 255
            else:
                img_hsv[j_hsv, i_hsv] = 0, 0, 0
    # cv2.imwrite('./test2/img_hsvhb.png', img_hsv)
    # cv2.imshow("hsv", img_hsv)
    # cv2.waitKey()
 
    # 灰度化處理圖像
    grayImage = cv2.cvtColor(img_hsv, cv2.COLOR_BGR2GRAY)
    # mask = np.zeros((grayImage.shape[0], grayImage.shape[1]), np.uint8)
    # mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
    # cv2.imwrite('./mask.png', mask)
 
    # 嘗試尋找輪廓
    contours, hierarchy = cv2.findContours(grayImage, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
 
    # 合并輪廓
    if len(contours) > 1:
        # print(contours)
        # 去掉離圖中心最遠(yuǎn)的圓
        max_idex, dis_max = 0, 0
        for c_i in range(len(contours)):
            c = contours[c_i]
            cx, cy, cw, ch = cv2.boundingRect(c)
            dis = math.sqrt(pow((cx + cw / 2 - w / 2), 2) + pow((cy + ch / 2 - h / 2), 2))
            if dis > dis_max:
                dis_max = dis
                max_idex = c_i
        contours.pop(max_idex)
        # print(contours)
 
        if len(contours) > 1:
            contours_merge = np.vstack([contours[0], contours[1]])
            for i in range(2, len(contours)):
                contours_merge = np.vstack([contours_merge, contours[i]])
            cv2.drawContours(img, contours_merge, -1, (0, 255, 255), 1)
            cv2.imwrite('./test2/img_res.png', img)
            # cv2.imshow("contours_merge", img)
            # cv2.waitKey()
        else:
            contours_merge = contours[0]
    else:
        contours_merge = contours[0]
 
 
 
    # RANSAC擬合
    points_data = np.reshape(contours_merge, (-1, 2))  # ellipse edge points set
 
    print("points_data", len(points_data))
    # 2.Ransac fit ellipse param
    Ransac = RANSAC(data=points_data, threshold=0.5, P=.99, S=.5, N=20)
    # Ransac = RANSAC(data=points_data, threshold=0.05, P=.99, S=.618, N=25)
 
    (X, Y), (LAxis, SAxis), Angle = Ransac.execute_ransac()
    # print( (X, Y), (LAxis, SAxis))
    # 擬合圓
    cv2.ellipse(img, ((X, Y), (LAxis, SAxis), Angle), (0, 0, 255), 1, cv2.LINE_AA)  # 畫(huà)圓
    cv2.circle(img, (int(X), int(Y)), 3, (0, 0, 255), -1)  # 畫(huà)圓心
    point_center.append(int(X))
    point_center.append(int(Y))
 
 
 
    rrt = cv2.fitEllipse(contours_merge)  # x, y）代表橢圓中心點(diǎn)的位置（a, b）代表長(zhǎng)短軸長(zhǎng)度，應(yīng)注意a、b為長(zhǎng)短軸的直徑，而非半徑,angle 代表了中心旋轉(zhuǎn)的角度
    # print("rrt", rrt)
    cv2.ellipse(img, rrt, (255, 0, 0), 1, cv2.LINE_AA)  # 畫(huà)圓
    x, y = rrt[0]
    cv2.circle(img, (int(x), int(y)), 3, (255, 0, 0), -1)  # 畫(huà)圓心
    point_center.append(int(x))
    point_center.append(int(y))
    # print("no",(x,y))
 
    cv2.imshow("fit circle", img)
    cv2.waitKey()
    # cv2.imwrite("./test2/fitcircle.png", img)
 
    # # 嘗試尋找輪廓
    # contours, hierarchy = cv2.findContours(grayImage, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    # # print('初次檢測(cè)數(shù)量: ', len(contours))
    # if len(contours) == 1:
    #     cv2.drawContours(mask, contours[0], -1, (255, 255, 255), 1)
    #     cv2.imwrite('./mask.png', mask)
    #     x, y, w, h = cv2.boundingRect(contours[0])
    #     cv2.circle(img, (int(x+w/2), int(y+h/2)), 1, (0, 0, 255), -1)
    #     cv2.rectangle(img, (x, y), (x + w + 1, y + h + 1), (0, 255, 255), 1)
    #     point_center.append(x + w / 2 + point0[0])
    #     point_center.append(y + h / 2 + point0[1])
    #     cv2.imwrite('./center1.png', img)
    # else:
    #     # 去除小面積雜點(diǎn)， 連接輪廓，求最小包圍框
    #     kernel1 = np.ones((3, 3), dtype=np.uint8)
    #     kernel2 = np.ones((2, 2), dtype=np.uint8)
    #     grayImage = cv2.dilate(grayImage, kernel1, 1)  # 1:迭代次數(shù)，也就是執(zhí)行幾次膨脹操作
    #     grayImage = cv2.erode(grayImage, kernel2, 1)
    #     cv2.imwrite('./img_dilate_erode.png', grayImage)
    #     contours, hierarchy = cv2.findContours(grayImage, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    #     if len(contours) == 1:
    #         cv2.drawContours(mask, contours[0], -1, (255, 255, 255), 1)
    #         cv2.imwrite('./mask.png', mask)
    #         x, y, w, h = cv2.boundingRect(contours[0])
    #         cv2.circle(img, (int(x + w / 2), int(y + h / 2)), 1, (0, 0, 255), -1)
    #         cv2.rectangle(img, (x, y), (x + w + 1, y + h + 1), (0, 255, 255), 1)
    #         point_center.append(x + w / 2 + point0[0])
    #         point_center.append(y + h / 2 + point0[1])
    #         cv2.imwrite('./center1.png', img)
    #     else:
    #         gray_circles = cv2.HoughCircles(grayImage, cv2.HOUGH_GRADIENT, 4, 10000, param1=100, param2=81, minRadius=10, maxRadius=19)
    #         # cv2.imwrite('./img_gray_circles.jpg', gray_circles)
    #         if len(gray_circles[0]) > 0:
    #             print('霍夫圓個(gè)數(shù):', len(gray_circles[0]))
    #             for (x, y, r) in gray_circles[0]:
    #                 x = int(x)
    #                 y = int(y)
    #                 cv2.circle(grayImage, (x, y), int(r), (255, 255, 255), -1)
    #             cv2.imwrite('./img_hf.jpg', grayImage)
    #
    #             detector = cv2.SimpleBlobDetector_create(params)
    #             keypoints = list(detector.detect(grayImage))
    #             for poi in keypoints:  # 回歸到原大圖坐標(biāo)系
    #                 x_poi, y_poi = poi.pt[0], poi.pt[1]
    #                 cv2.circle(img, (int(x_poi), int(y_poi)), 20, (255, 255, 255), -1)
    #                 point_center.append(poi.pt[0] + point0[0])
    #                 point_center.append(poi.pt[1] + point0[1])
    #                 cv2.imwrite('./img_blob.png', img)
    #         else:
    #             for num_cont in range(len(contours)):
    #                 cont = cv2.contourArea(contours[num_cont])
    #                 # if cont > 6:
    #                 #     contours2.append(contours[num_cont])
    #                 if cont <= 6:
    #                     x, y, w, h = cv2.boundingRect(contours[num_cont])
    #                     cv2.rectangle(grayImage, (x, y), (x + w, y + h), (0, 0, 0), -1)
    #             cv2.imwrite('./img_weilj.png', grayImage)
    #             grayImage = lj_img(grayImage)
    #             cv2.imwrite('./img_lj.png', grayImage)
    #             contours, hierarchy = cv2.findContours(grayImage, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    #             # print('再次檢測(cè)數(shù)量: ', len(contours))
    #
    #             cv2.drawContours(mask, contours[0], -1, (255, 255, 255), 1)
    #             cv2.imwrite('./mask.png', mask)
    #             x, y, w, h = cv2.boundingRect(contours[0])
    #             cv2.circle(img, (int(x + w / 2), int(y + h / 2)), 1, (0, 0, 255), -1)
    #             cv2.rectangle(img, (x, y), (x + w + 1, y + h + 1), (0, 255, 255), 1)
    #             point_center.append(x + w / 2 + point0[0])
    #             point_center.append(y + h / 2 + point0[1])
    #             cv2.imwrite('./center1.png', img)
 
    return point_center[0], point_center[1]
 
if __name__ == "__main__":
 
    for i in range(1,6):
        imageName = "s"
        imageName += str(i)
        path = './Images/danHoles/' + imageName + '.png'
        print(path)
        img = cv2.imread(path)
        point0 = [0, 0]
        cir_x, cir_y = mainFigure(img, point0)
 
    # img = cv2.imread('./Images/danHoles/s2.png')
    # point0 = [0, 0]
    # cir_x, cir_y = mainFigure(img, point0)

import cv2
import math
import random
import numpy as np
from numpy.linalg import inv, svd, det
import time
 
class RANSAC:
    def __init__(self, data, threshold, P, S, N):
        self.point_data = data  # 橢圓輪廓點(diǎn)集
        self.length = len(self.point_data)  # 橢圓輪廓點(diǎn)集長(zhǎng)度
        self.error_threshold = threshold  # 模型評(píng)估誤差容忍閥值
 
        self.N = N  # 隨機(jī)采樣數(shù)
        self.S = S  # 設(shè)定的內(nèi)點(diǎn)比例
        self.P = P  # 采得N點(diǎn)去計(jì)算的正確模型概率
        self.max_inliers = self.length * self.S  # 設(shè)定最大內(nèi)點(diǎn)閥值
        self.items = 10
 
        self.count = 0  # 內(nèi)點(diǎn)計(jì)數(shù)器
        self.best_model = ((0, 0), (1e-6, 1e-6), 0)  # 橢圓模型存儲(chǔ)器
 
    def random_sampling(self, n):
        # 這個(gè)部分有修改的空間，這樣循環(huán)次數(shù)太多了，可以看看別人改進(jìn)的ransac擬合橢圓的論文
        """隨機(jī)取n個(gè)數(shù)據(jù)點(diǎn)"""
        all_point = self.point_data
        select_point = np.asarray(random.sample(list(all_point), n))
        return select_point
 
    def Geometric2Conic(self, ellipse):
        # 這個(gè)部分參考了GitHub中的一位大佬的，但是時(shí)間太久，忘記哪個(gè)人的了
        """計(jì)算橢圓方程系數(shù)"""
        # Ax ^ 2 + Bxy + Cy ^ 2 + Dx + Ey + F
        (x0, y0), (bb, aa), phi_b_deg = ellipse
 
        a, b = aa / 2, bb / 2  # Semimajor and semiminor axes
        phi_b_rad = phi_b_deg * np.pi / 180.0  # Convert phi_b from deg to rad
        ax, ay = -np.sin(phi_b_rad), np.cos(phi_b_rad)  # Major axis unit vector
 
        # Useful intermediates
        a2 = a * a
        b2 = b * b
 
        # Conic parameters
        if a2 > 0 and b2 > 0:
            A = ax * ax / a2 + ay * ay / b2
            B = 2 * ax * ay / a2 - 2 * ax * ay / b2
            C = ay * ay / a2 + ax * ax / b2
            D = (-2 * ax * ay * y0 - 2 * ax * ax * x0) / a2 + (2 * ax * ay * y0 - 2 * ay * ay * x0) / b2
            E = (-2 * ax * ay * x0 - 2 * ay * ay * y0) / a2 + (2 * ax * ay * x0 - 2 * ax * ax * y0) / b2
            F = (2 * ax * ay * x0 * y0 + ax * ax * x0 * x0 + ay * ay * y0 * y0) / a2 + \
                (-2 * ax * ay * x0 * y0 + ay * ay * x0 * x0 + ax * ax * y0 * y0) / b2 - 1
        else:
            # Tiny dummy circle - response to a2 or b2 == 0 overflow warnings
            A, B, C, D, E, F = (1, 0, 1, 0, 0, -1e-6)
 
        # Compose conic parameter array
        conic = np.array((A, B, C, D, E, F))
        return conic
 
    def eval_model(self, ellipse):
        # 這個(gè)地方也有很大修改空間，判斷是否內(nèi)點(diǎn)的條件在很多改進(jìn)的ransac論文中有說(shuō)明，可以多看點(diǎn)論文
        """評(píng)估橢圓模型，統(tǒng)計(jì)內(nèi)點(diǎn)個(gè)數(shù)"""
        # this an ellipse ?
        a, b, c, d, e, f = self.Geometric2Conic(ellipse)
        E = 4 * a * c - b * b
        if E <= 0:
            # print('this is not an ellipse')
            return 0, 0
 
        #  which long axis ?
        (x, y), (LAxis, SAxis), Angle = ellipse
        LAxis, SAxis = LAxis / 2, SAxis / 2
        if SAxis > LAxis:
            temp = SAxis
            SAxis = LAxis
            LAxis = temp
 
        # calculate focus
        Axis = math.sqrt(LAxis * LAxis - SAxis * SAxis)
        f1_x = x - Axis * math.cos(Angle * math.pi / 180)
        f1_y = y - Axis * math.sin(Angle * math.pi / 180)
        f2_x = x + Axis * math.cos(Angle * math.pi / 180)
        f2_y = y + Axis * math.sin(Angle * math.pi / 180)
 
        # identify inliers points
        f1, f2 = np.array([f1_x, f1_y]), np.array([f2_x, f2_y])
        f1_distance = np.square(self.point_data - f1)
        f2_distance = np.square(self.point_data - f2)
        all_distance = np.sqrt(f1_distance[:, 0] + f1_distance[:, 1]) + np.sqrt(f2_distance[:, 0] + f2_distance[:, 1])
 
        Z = np.abs(2 * LAxis - all_distance)
        delta = math.sqrt(np.sum((Z - np.mean(Z)) ** 2) / len(Z))
 
        # Update inliers set
        inliers = np.nonzero(Z < 0.8 * delta)[0]
        inlier_pnts = self.point_data[inliers]
 
        return len(inlier_pnts), inlier_pnts
 
    def execute_ransac(self):
        Time_start = time.time()
        while math.ceil(self.items):
            # print(self.max_inliers)
 
            # 1.select N points at random
            select_points = self.random_sampling(self.N)
 
            # 2.fitting N ellipse points
            ellipse = cv2.fitEllipse(select_points)
 
            # 3.assess model and calculate inliers points
            inliers_count, inliers_set = self.eval_model(ellipse)
 
            # 4.number of new inliers points more than number of old inliers points ?
            if inliers_count > self.count:
                ellipse_ = cv2.fitEllipse(inliers_set)  # fitting ellipse for inliers points
                self.count = inliers_count  # Update inliers set
                self.best_model = ellipse_  # Update best ellipse
                # print("self.count", self.count)
 
                # 5.number of inliers points reach the expected value
                if self.count > self.max_inliers:
                    print('the number of inliers: ', self.count)
                    break
 
                # Update items
                # print(math.log(1 - pow(inliers_count / self.length, self.N)))
                self.items = math.log(1 - self.P) / math.log(1 - pow(inliers_count / self.length, self.N))
 
        return self.best_model
 
 
if __name__ == '__main__':
 
 
    # 1.find ellipse edge line
    contours, hierarchy = cv2.findContours(grayImage, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_NONE)
 
    # 2.Ransac fit ellipse param
    points_data = np.reshape(contours, (-1, 2))  # ellipse edge points set
    Ransac = RANSAC(data=points_data, threshold=0.5, P=.99, S=.618, N=10)
    (X, Y), (LAxis, SAxis), Angle = Ransac.execute_ransac()