import math as m
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import random as rnd
import math
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)
        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)

    fixed_steps = [0.3, 0.32, 0.53, 0.74, 0.95]

    # Убедимся, что num_steps не превышает количество фиксиованных значений
    if num_steps > len(fixed_steps):
        raise ValueError("Количество шагов превышает количество доступных фиксированных значений.")

        # Используем фиксированные значения вместо случайных
    shag = np.array(fixed_steps[: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 = 5
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)

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()
при построении точек шагов оператора и ртак расстояние между оператором и ртк должно бить равно 5 или близко к этому но после прохождения 5 шагов расстояние между ртк и оператором 6,19