import rospy
from clover import srv
from std_srvs.srv import Trigger
import math
import csv

rospy.init_node('flight')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
land = rospy.ServiceProxy('land', Trigger)

def wait_arrival(tolerance=0.2, debug=False, log_writer=None):
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        telem = get_telemetry(frame_id='navigate_target')
        distance = math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2)

        if debug:
            rospy.loginfo(f"текущее расстояние до цели: {distance}")

        if log_writer:
            global_telem = get_telemetry(frame_id='map')
            log_writer.writerow([rospy.get_time(), global_telem.x, global_telem.y, global_telem.z, global_telem.yaw])

        if distance < tolerance:
            break
        rate.sleep()
    rospy.sleep(2)

def navigate_and_wait(x, y, z, frame_id='body', auto_arm=False, debug=False, log_writer=None):
    navigate(x=x, y=y, z=z, frame_id=frame_id, auto_arm=auto_arm)
    if debug:
        rospy.loginfo(f"навигация к: x={x}, y={y}, z={z}, frame_id={frame_id}")
    wait_arrival(debug=debug, log_writer=log_writer)

for flight_num in range(1, 4):
    rospy.loginfo(f"начинаю тестовый полет {flight_num}")

    filename = f'flight_log_{flight_num}.csv'
    with open(filename, 'w', newline='') as csvfile:
        log_writer = csv.writer(csvfile)
        log_writer.writerow(['time', 'x', 'y', 'z', 'yaw'])

        navigate_and_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True, debug=True, log_writer=log_writer)
        rospy.loginfo("достиг высоты 1 метр")

        navigate_and_wait(x=1, y=0, z=1, frame_id='map', debug=True, log_writer=log_writer)
        rospy.loginfo("достиг точки (1, 0, 1)")

        navigate_and_wait(x=1, y=1, z=1, frame_id='map', debug=True, log_writer=log_writer)
        rospy.loginfo("достиг точки (1, 1, 1)")

        navigate_and_wait(x=0, y=1, z=1, frame_id='map', debug=True, log_writer=log_writer)
        rospy.loginfo("достиг точки (0, 1, 1)")


        navigate_and_wait(x=0, y=0, z=0.5, frame_id='map', debug=True, log_writer=log_writer)
        land()
        rospy.loginfo("посадка в точке (0, 0, 0)")

    rospy.loginfo(f"завершил тестовый полет {flight_num}")
    rospy.sleep(5)

# анализ данных полетов
for flight_num in range(1, 4):
    filename = f'flight_log_{flight_num}.csv'
    rospy.loginfo(f"анализ данных полета из файла {filename}")

    with open(filename, 'r') as csvfile:
        log_reader = csv.reader(csvfile)
        next(log_reader)  # пропускаю заголовок

        times = []
        xs = []
        ys = []
        zs = []
        yaws = []

        for row in log_reader:
            times.append(float(row[0]))
            xs.append(float(row[1]))
            ys.append(float(row[2]))
            zs.append(float(row[3]))
            yaws.append(float(row[4]))

        if times:
            total_time = times[-1] - times[0]
            rospy.loginfo(f"общая длительность полета {flight_num}: {total_time:.2f} секунд")

            total_distance = 0
            for i in range(1, len(xs)):
                dx = xs[i] - xs[i-1]
                dy = ys[i] - ys[i-1]
                dz = zs[i] - zs[i-1]
                total_distance += math.sqrt(dx**2 + dy**2 + dz**2)
            rospy.loginfo(f"общая пройденная дистанция {flight_num}: {total_distance:.2f} метров")

            average_speed = total_distance / total_time if total_time > 0 else 0
            rospy.loginfo(f"средняя скорость полета {flight_num}: {average_speed:.2f} м/с")
        else:
            rospy.loginfo(f"данные для полета {flight_num} не записаны")

rospy.loginfo("миссия завершена")