import numpy as np
import matplotlib.pyplot as plt

def fit_circle(points):
    """Fit a circle to the given points using least squares method."""
    x, y = points[:, 0], points[:, 1]
    A = np.vstack([-x, -y, np.ones(len(x))]).T
    #print(A.shape)
    B = -np.array([x ** 2 + y ** 2]).T
   # print(B.shape)
    C_matrix = A.T.dot(A)
    result = np.linalg.inv(C_matrix).dot(A.T.dot(B))
    #print(result.shape)
    center = [result[0] * 0.5, result[1] * 0.5]
    return center, np.sqrt(center[0] ** 2 + center[1] ** 2 - result[2])
    # (x-a)^2+(y-b)*2=^2
    # x^2 + a^2 - 2ax + y^2 + b^2 - 2bx =r^2
    # -2ax -2by + a^2 + b^2 - r^2 =  - (x^2 + y^2) 
    # [-x -y 1] [2a 2b (a^2 + b^2 - r^2) ] = - (x^2 + y^2)


    
    #center = np.linalg.lstsq(A, y, rcond=None)[0]
    #radius = np.sqrt((center[0]**2 + center[1]**2 - (x**2 + y**2).mean())**2)
    #return center, radius

def ransac_circle(points, max_trials=1000, threshold=1):
    """Fit a circle to points using the RANSAC algorithm."""
    best_fit = 0
    best_error = np.inf
    n_points = len(points)
    
    for _ in range(max_trials):
        sample_indices = np.random.choice(n_points, 3, replace=False)
        sample_points = points[sample_indices]
        center, radius = fit_circle(sample_points)
        
        # Calculate distance from each point to the fitted circle
        distances = np.sqrt((points[:, 0] - center[0])**2 + (points[:, 1] - center[1])**2) # - radius
        inliers = np.logical_and( np.abs(distances) <= (radius + threshold) , np.abs(distances) >= (radius - threshold))
        
        # Check if this is the best fit so far
        if sum(inliers) > best_fit:
            best_fit = sum(inliers)

            center, radius = fit_circle(points[inliers])
            best_center = center
            best_radius = radius
            best_inliers = inliers


    
    return best_center, best_radius, best_inliers

# Generate some example data points including noise
# np.random.seed(0)
n_samples = 100
true_center = (1, 0)
true_radius = 10
angles = np.linspace(0, 2 * np.pi, n_samples)
true_points = np.vstack([true_center[0] + true_radius * np.cos(angles),
                         true_center[1] + true_radius * np.sin(angles)]).T
noise = np.random.normal(size=(n_samples, 2), scale=1)
points = true_points + noise

# Fit circle using RANSAC
center, radius, inliers = ransac_circle(points, max_trials=1000, threshold=1.5)

# Plot results

print(f"true_center {true_center}, true_radius {true_radius} ")
print(f"center {center}, radius {radius} ")
plt.figure(figsize=(5, 5))
plt.scatter(points[:, 0], points[:, 1], label='Data Points')
plt.scatter(points[inliers, 0], points[inliers, 1], color='red', label='Inliers')
plt.scatter([center[0]], [center[1]], color='black')
theta = np.linspace(0, 2 * np.pi, 100)
plt.plot(center[0] + radius * np.cos(theta), center[1] + radius * np.sin(theta), label='Fitted Circle')
plt.legend(loc='upper right')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
import random
import math

# 數(shù)據(jù)量。
SIZE = 50
# 產(chǎn)生數(shù)據(jù)。np.linspace 返回一個一維數(shù)組，SIZE指定數(shù)組長度。
# 數(shù)組最小值是0，最大值是10。所有元素間隔相等。
X = np.linspace(0, 10, SIZE)
Y = 3 * X + 10

fig = plt.figure()
# 畫圖區(qū)域分成1行1列。選擇第一塊區(qū)域。
ax1 = fig.add_subplot(1,1, 1)
# 標(biāo)題
ax1.set_title("RANSAC")


# 讓散點(diǎn)圖的數(shù)據(jù)更加隨機(jī)并且添加一些噪聲。
random_x = []
random_y = []
# 添加直線隨機(jī)噪聲
for i in range(SIZE):
    random_x.append(X[i] + random.uniform(-0.5, 0.5)) 
    random_y.append(Y[i] + random.uniform(-0.5, 0.5)) 
# 添加隨機(jī)噪聲
for i in range(SIZE):
    random_x.append(random.uniform(0,10))
    random_y.append(random.uniform(10,40))
RANDOM_X = np.array(random_x) # 散點(diǎn)圖的橫軸。
RANDOM_Y = np.array(random_y) # 散點(diǎn)圖的縱軸。

# 畫散點(diǎn)圖。
ax1.scatter(RANDOM_X, RANDOM_Y)
# 橫軸名稱。
ax1.set_xlabel("x")
# 縱軸名稱。
ax1.set_ylabel("y")

# 使用RANSAC算法估算模型
# 迭代最大次數(shù)，每次得到更好的估計會優(yōu)化iters的數(shù)值
iters = 100000
# 數(shù)據(jù)和模型之間可接受的差值
sigma = 1
# 最好模型的參數(shù)估計和內(nèi)點(diǎn)數(shù)目
best_a = 0
best_b = 0
pretotal = 0
# 保存的最好的內(nèi)點(diǎn)
best_inner_x = []
best_inner_y = []
# 希望的得到正確模型的概率
P = 0.99
for i in range(iters):
    print("i", i)
    # 隨機(jī)在數(shù)據(jù)中紅選出兩個點(diǎn)去求解模型
    sample_index = random.sample(range(SIZE * 2),2)
    x_1 = RANDOM_X[sample_index[0]]
    x_2 = RANDOM_X[sample_index[1]]
    y_1 = RANDOM_Y[sample_index[0]]
    y_2 = RANDOM_Y[sample_index[1]]

    # y = ax + b 求解出a，b
    a = (y_2 - y_1) / (x_2 - x_1)
    b = y_1 - a * x_1

    # 算出內(nèi)點(diǎn)數(shù)目
    total_inlier = 0
    best_inner_x_dummpy = []
    best_inner_y_dummpy = []
    for index in range(SIZE * 2):
        y_estimate = a * RANDOM_X[index] + b
        if abs(y_estimate - RANDOM_Y[index]) < sigma:
            best_inner_x_dummpy.append(RANDOM_X[index])
            best_inner_y_dummpy.append(RANDOM_Y[index])
            total_inlier = total_inlier + 1

    # 判斷當(dāng)前的模型是否比之前估算的模型好
    if total_inlier > pretotal:
        iters = math.log(1 - P) / math.log(1 - pow(total_inlier / (SIZE * 2), 2))
        pretotal = total_inlier
        best_a = a
        best_b = b
        best_inner_x = best_inner_x_dummpy
        best_inner_y = best_inner_y_dummpy
        print(f"iters {iters}, pretotal {pretotal}, best_a {best_a}, best_b {best_b}")

    if i >= iters:
        break 

# 用我們得到的最佳估計畫圖
Y = best_a * RANDOM_X + best_b

# 直線圖
ax1.plot(RANDOM_X, Y)

# 畫散點(diǎn)圖。
ax1.scatter(best_inner_x, best_inner_y)

text = "best_a = " + str(best_a) + "\nbest_b = " + str(best_b)
plt.text(5,10, text, fontdict={'size': 8, 'color': 'r'})
plt.show()