"""

Object shape recognition with circle fitting

author: Atsushi Sakai (@Atsushi_twi)

"""

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

show_animation = True


def circle_fitting(x, y):
    """
    Fits a circle to a given set of points using a least-squares approach.

    This function calculates the center (x, y) and radius of a circle that best fits
    the given set of points in a two-dimensional plane. It minimizes the squared
    errors between the circle and the provided points and returns the calculated
    center coordinates, radius, and the fitting error.

    Raises
    ------
    ValueError
        If the input lists x and y do not contain the same number of elements.

    Parameters
    ----------
    x : list[float]
        The x-coordinates of the points.
    y : list[float]
        The y-coordinates of the points.

    Returns
    -------
    tuple[float, float, float, float]
        A tuple containing:
        - The x-coordinate of the center of the fitted circle (float).
        - The y-coordinate of the center of the fitted circle (float).
        - The radius of the fitted circle (float).
        - The fitting error as a deviation metric (float).
    """

    sumx = sum(x)
    sumy = sum(y)
    sumx2 = sum([ix ** 2 for ix in x])
    sumy2 = sum([iy ** 2 for iy in y])
    sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])

    F = np.array([[sumx2, sumxy, sumx],
                  [sumxy, sumy2, sumy],
                  [sumx, sumy, len(x)]])

    G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
                  [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
                  [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])

    T = np.linalg.inv(F).dot(G)

    cxe = float(T[0, 0] / -2)
    cye = float(T[1, 0] / -2)
    re = math.sqrt(cxe**2 + cye**2 - T[2, 0])

    error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])

    return (cxe, cye, re, error)


def get_sample_points(cx, cy, cr, angle_reso):
    x, y, angle, r = [], [], [], []

    # points sampling
    for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
        nx = cx + cr * math.cos(theta)
        ny = cy + cr * math.sin(theta)
        nangle = math.atan2(ny, nx)
        nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)

        x.append(nx)
        y.append(ny)
        angle.append(nangle)
        r.append(nr)

    # ray casting filter
    rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)

    return rx, ry


def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
    rx, ry = [], []
    rangedb = [float("inf") for _ in range(
        int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]

    for i, _ in enumerate(thetal):
        angleid = math.floor(thetal[i] / angle_reso)

        if rangedb[angleid] > rangel[i]:
            rangedb[angleid] = rangel[i]

    for i, _ in enumerate(rangedb):
        t = i * angle_reso
        if rangedb[i] != float("inf"):
            rx.append(rangedb[i] * math.cos(t))
            ry.append(rangedb[i] * math.sin(t))

    return rx, ry


def plot_circle(x, y, size, color="-b"):  # pragma: no cover
    deg = list(range(0, 360, 5))
    deg.append(0)
    xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
    yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
    plt.plot(xl, yl, color)


def main():

    # simulation parameters
    simtime = 15.0  # simulation time
    dt = 1.0  # time tick

    cx = -2.0  # initial x position of obstacle
    cy = -8.0  # initial y position of obstacle
    cr = 1.0  # obstacle radious
    theta = np.deg2rad(30.0)  # obstacle moving direction
    angle_reso = np.deg2rad(3.0)  # sensor angle resolution

    time = 0.0
    while time <= simtime:
        time += dt

        cx += math.cos(theta)
        cy += math.cos(theta)

        x, y = get_sample_points(cx, cy, cr, angle_reso)

        ex, ey, er, error = circle_fitting(x, y)
        print("Error:", error)

        if show_animation:  # pragma: no cover
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect('key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
            plt.axis("equal")
            plt.plot(0.0, 0.0, "*r")
            plot_circle(cx, cy, cr)
            plt.plot(x, y, "xr")
            plot_circle(ex, ey, er, "-r")
            plt.pause(dt)

    print("Done")


if __name__ == '__main__':
    main()