import math as m
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import random as rnd
import numpy as np
import statistics as stat
import scipy.interpolate as interpol
from scipy.optimize import fsolve
from fuzzylogic.classes import Domain, Rule
from fuzzylogic.functions import triangular


def fuzz(phi, v0):
    # Объявление области функций
    # Курсовой угол
    fi = Domain("Angle", 0, 11.4, res=0.001)
    # Скорость оператора
    V0 = Domain("Operator_speed", 0, 1.52, res=0.001)
    # Скорость мгновенной РТК
    Vp = Domain("Speed_module", 0, 1.51, res=0.001)

    # Курсовой угол (плавный)
    fi.smooth = triangular(0, 4.22)
    # Курсовой угол (средний)
    fi.medium = triangular(4, 6.75)
    # Курсовой угол (крутой)
    fi.steep = triangular(6.3, 11.4)

    # Скорость оператора (полшага)
    V0.half_step = triangular(0, 0.6)
    # Скорость оператора (шаг)
    V0.step = triangular(0.57, 1)
    # Скорость оператора (1,5 шага)
    V0.step_and_half = triangular(0.9, 1.52)

    # Модуль мгновенной скорости РТК (короткий)
    Vp.short = triangular(0, 0.58)
    # Модуль мгновенной скорости РТК (средний)
    Vp.medium = triangular(0.52, 1)
    # Модуль мгновенной скорости РТК (длинный)
    Vp.long = triangular(0.9, 1.51)

    # Запись правил
    R1 = Rule({(fi.smooth, V0.half_step): Vp.short})
    R2 = Rule({(fi.smooth, V0.step): Vp.medium})
    R3 = Rule({(fi.smooth, V0.step_and_half): Vp.long})
    R4 = Rule({(fi.medium, V0.half_step): Vp.medium})
    R5 = Rule({(fi.medium, V0.step): Vp.medium})
    R6 = Rule({(fi.medium, V0.step_and_half): Vp.long})
    R7 = Rule({(fi.steep, V0.half_step): Vp.medium})
    R8 = Rule({(fi.steep, V0.step): Vp.medium})
    R9 = Rule({(fi.steep, V0.step_and_half): Vp.long})

    # Объединение правил
    rules = R1 | R2 | R3 | R4 | R5 | R6 | R7 | R8 | R9

    # Входные данные
    values = {fi: phi, V0: v0}

    # Вывод результата
    return rules(values)


def pereshet(x, y, angle, y_r):
    # Вычисляем синус и косинус угла поворота
    sin_a = m.sin(angle)
    cos_a = m.cos(angle)
    x_2 = y_r * cos_a
    y_2 = y_r * sin_a
    # Поворачиваем координаты
    x_new = (x - x_2) * cos_a + (y - y_2) * sin_a
    y_new = -(x - x_2) * sin_a + (y - y_2) * cos_a
    # Возвращаем повернутые и смещенные координаты
    return (x_new, y_new)


def tochki_operator_and_rtk(i):
    if i < dottes + 1:
        message_1 = f'Расстояние: {rast[i]}; Отклонение: {otklon[i]:.2f}'
        ax1.set_title(message_1)
        message_2 = f'Скорость оператора: {velocity_operator[i]:.2f};\n Скорость РТК: {velocity_rtk[i]:.2f}'
        ax2.set_title(message_2)
        ax3.set_title("Синусоидное движение")

        ax1.scatter(x_oper_n[i], y_oper_n[i], marker='o', color='blue')
        ax1.scatter(x_rtk[i], y_rtk[i], marker='o', color='red')
        ax2.scatter(t_time[i], m_shag_sum[i], marker='o', color='blue')
        ax2.scatter(t_time[i], m_rtk_sum[i], marker='o', color='red')

        ax3.scatter(t_time[i], np.sin(m_shag_sum[i]), marker='o', color='blue')
        ax3.scatter(t_time[i], np.sin(m_rtk_sum[i]), marker='o', color='red')

        fig.tight_layout()
    else:
        ani.event_source.stop()
        ax1.plot(x_oper_n, y_oper_n, color='blue', linestyle='-', label='Оператор')
        ax1.plot(x_rtk, y_rtk, color='red', linestyle='-', label='РТК')
        ax2.plot(t_time, m_shag_sum, color='blue', linestyle='-', label='Оператор')
        ax2.plot(t_time, m_rtk_sum, color='red', linestyle='-', label='РТК')

        ax3.plot(t_time, np.sin(np.array(m_shag_sum)), color='blue', linestyle='-', label='Оператор')
        ax3.plot(t_time, np.sin(np.array(m_rtk_sum)), color='red', linestyle='-', label='РТК')

        ax1.legend()
        ax2.legend()
        ax3.legend()
        message_1 = f'Среднее расстояние: {rast_mean:.2f}; Среднее отклонение: {otklon_mean:.2f}'
        ax1.set_title(message_1)
        message_2 = f'Средняя скорость оператора: {vel_op_mean:.2f};\n Средняя скорость РТК: {vel_rtk_mean:.2f}'
        ax2.set_title(message_2)

        ax3.set_title("Синусоидное движение")


def coor_operator(num_steps):
    p_operator_x = []
    p_operator_y = []
    p_operator_x.append(5)
    p_operator_y.append(0)
    n_operator_x = []
    n_operator_y = []
    n_operator_x.append(5)
    n_operator_y.append(0)
    shag = np.random.uniform(0.5, 0.8, num_steps)
    shag = np.round(shag, 2)
    angle_degrees = np.random.uniform(-10, 10, num_steps)
    angle_radians = np.deg2rad(angle_degrees)
    current_angle = 0
    for i in range(num_steps):
        current_angle += angle_radians[i]
        dob_x = shag[i] * m.cos(current_angle)
        dob_y = shag[i] * m.sin(current_angle)
        p_operator_x.append(dob_x)
        p_operator_y.append(dob_y)
        n_operator_x.append(dob_x)
        n_operator_y.append(dob_y)
    operator_x = np.cumsum(p_operator_x)
    operator_y = np.cumsum(p_operator_y)
    operator_x_n = np.cumsum(n_operator_x)
    operator_y_n = np.cumsum(n_operator_y)
    shag = np.insert(shag, 0, 0)
    return operator_x, operator_y, operator_x_n, operator_y_n, shag


def coor_rtk(x, y):
    x_rtk = []
    y_rtk = []
    x_rtk_start = 0
    y_rtk_start = 0
    x_rtk.append(x_rtk_start)
    y_rtk.append(y_rtk_start)
    m_rtk = []
    m_rtk.append(0)
    phi = []
    for i in range(1, len(x)):
        phi_1 = m.atan(y[i] / x[i])
        phi.append(phi_1)
        r_1 = m.sqrt(x[i] ** 2 + y[i] ** 2)
        v0_1 = m.sqrt((x[i - 1] ** 2) + r_1 ** 2 - 2 * x[i - 1] * r_1 * m.cos(phi_1))
        ugol = m.degrees(phi_1)
        if abs(ugol) < 1e-5:
            vrtk_0 = v0_1
        else:
            vrtk_0 = fuzz(m.degrees(phi_1), v0_1)
        if vrtk_0 == None:
            vrtk_0 = v0_1
        m_rtk.append(vrtk_0)
        for j in range(i - 1, len(x), 1):
            (x[j], y[j]) = pereshet(x[j], y[j], phi_1, vrtk_0)
    ygol_itog = 0
    for (ygol, velocity) in zip(phi, m_rtk):
        ygol_itog += ygol
        x_end = x_rtk_start + velocity * np.cos(ygol_itog)
        y_end = y_rtk_start + velocity * np.sin(ygol_itog)
        x_rtk_start = x_end
        y_rtk_start = y_end
        x_rtk.append(x_rtk_start)
        y_rtk.append(y_rtk_start)
    return x_rtk, y_rtk, m_rtk


def s_statistic(x_o, y_o, x_r, y_r, number):
    rast = []
    otklon = []
    for i in range(0, number + 1, 1):
        point_1 = np.array((x_o[i], y_o[i]))
        point_2 = np.array((x_r[i], y_r[i]))
        R = m.dist(point_1, point_2)
        rast.append(R)
        delta_r = R - 5
        otklon.append(delta_r)
    rast_mean = stat.mean(rast)
    otklon_mean = stat.mean(otklon)
    return rast, otklon, rast_mean, otklon_mean


def v_vel(shag, rtk, dt, number):
    velocity_operator = []
    velocity_rtk = []
    for i in range(0, number + 1, 1):
        velocity_operator.append(shag[i] / dt)
        velocity_rtk.append(rtk[i] / dt)
    vel_op_mean = stat.mean(velocity_operator)
    vel_rtk_mean = stat.mean(velocity_rtk)
    return velocity_operator, velocity_rtk, vel_op_mean, vel_rtk_mean


def puti(shag, rtk, number):
    m_shag_sum = []
    m_shag_sum.append(5)
    m_rtk_sum = []
    m_rtk_sum.append(0)
    for i in range(1, number + 1, 1):
        m_shag_sum.append(m_shag_sum[i - 1] + shag[i])
        m_rtk_sum.append(m_rtk_sum[i - 1] + rtk[i])
    return m_shag_sum, m_rtk_sum


dottes = 35
delta_shag = 0.57
x_oper, y_oper, x_oper_n, y_oper_n, m_shag = coor_operator(dottes)
x_rtk, y_rtk, m_rtk = coor_rtk(x_oper, y_oper)
m_shag_sum, m_rtk_sum = puti(m_shag, m_rtk, dottes)
rast, otklon, rast_mean, otklon_mean = s_statistic(x_oper_n, y_oper_n, x_rtk, y_rtk, dottes)
velocity_operator, velocity_rtk, vel_op_mean, vel_rtk_mean = v_vel(m_shag, m_rtk, delta_shag, dottes)


def animate_sine_graph(i):
    if i < dottes + 1:
        ax_sine.scatter(t_time[i], np.sin(m_shag_sum[i]), marker='o', color='blue')
        ax_sine.scatter(t_time[i], np.sin(m_rtk_sum[i]), marker='o', color='red')
    else:
        sine_animation.event_source.stop()
        ax_sine.plot(t_time, np.sin(np.array(m_shag_sum)), color='blue', linestyle='-', label='Оператор')
        ax_sine.plot(t_time, np.sin(np.array(m_rtk_sum)), color='red', linestyle='-', label='РТК')
        ax_sine.legend()
        ax_sine.set_title("Синусоидное движение")


t_time = np.arange(dottes + 1)
fig, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(18, 6))
manager = plt.get_current_fig_manager()

fig_sine, ax_sine = plt.subplots()
sine_animation = animation.FuncAnimation(fig_sine, animate_sine_graph, interval=570, cache_frame_data=False)

# manager.window.showMaximized()
ani = animation.FuncAnimation(fig, tochki_operator_and_rtk, interval=570, cache_frame_data=False)
ax1.grid()
ax2.grid()
ax3.grid()
ax1.axhline(y=0, color='k', linestyle='-')
ax1.axvline(x=0, color='k', linestyle='-')
ax2.axhline(y=0, color='k', linestyle='-')
ax2.axvline(x=0, color='k', linestyle='-')
ax3.axhline(y=0, color='k', linestyle='-')
ax3.axvline(x=0, color='k', linestyle='-')
plt.show()
вывод