import math as m
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import math
import numpy as np
import statistics as stat
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)
        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')
        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='РТК')
        ax1.legend()
        ax2.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)


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.array([0.3, 0.32, 0.53, 0.74, 0.95, 0][:num_steps])

    angle_degrees = np.full(num_steps, 2)
    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)
    angle_degrees = np.insert(angle_degrees, 0, 0)
    return operator_x, operator_y, operator_x_n, operator_y_n, shag, angle_degrees



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 = 6
delta_shag = 0.57
x_oper,y_oper,x_oper_n,y_oper_n,m_shag, angle_degrees = 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)

print("Шаги РТК:", m_rtk)
print("Шаги опр:", m_shag)
last_point_operator = (x_oper_n[-1], y_oper_n[-1])
last_point_rtk = (x_rtk[-1], y_rtk[-1])

# Вычисляем расстояние
distance = math.dist(last_point_operator, last_point_rtk)

print(f"Расстояние от последней точки оператора до последней точки РТК: {distance:.2f}")

t_time = np.arange(dottes + 1)
fig, (ax1, ax2) = plt.subplots(1, 2)
manager = plt.get_current_fig_manager()
# manager.window.showMaximized()
ani = animation.FuncAnimation(fig, tochki_operator_and_rtk, interval=570, cache_frame_data=False)
ax1.grid()
ax2.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='-')
plt.show()
сделай так чтобы на графике средней скорости не учитывался из этой строчки последний нулевой шаг shag = np.array([0.3, 0.32, 0.53, 0.74, 0.95, 0][:num_steps])