import numpy as np
import math
from aux_functions import matrix_math as mm
from aux_functions import nav_transformations as nt




#Interpolate between points
def interpolateBetweenPoints(x1, y1, x2, y2, x_t):
    y_t = y1 + (y2 - y1) * (x_t - x1) / (x2 - x1)

    return y_t


#interpolate between position vertices
def vertexInterpolate(vertexBefore, vertexAfter, currentTime):
    x1 = vertexBefore[0]
    y1 = vertexBefore[1]
    z1 = vertexBefore[2]
    t1 = vertexBefore[3]

    x2 = vertexAfter[0]
    y2 = vertexAfter[1]
    z2 = vertexAfter[2]
    t2 = vertexAfter[3]

    vertexCurrent = np.zeros(4)
    vertexCurrent[0] = interpolateBetweenPoints(t1, x1, t2, x2, currentTime)
    vertexCurrent[1] = interpolateBetweenPoints(t1, y1, t2, y2, currentTime)
    vertexCurrent[2] = interpolateBetweenPoints(t1, z1, t2, z2, currentTime)
    #vertexCurrent[3] = currentTime

    return vertexCurrent

#cost function for cruise velocity calculation
def cruise_vel_cost(x, cruise_velocity):
    v_n = x[0]
    v_e = x[1]

    cost_val = (cruise_velocity ** 2) - (v_n ** 2) - (v_e ** 2)

    return cost_val

#Estimate cruise velocity
def estimate_cruise_velocity(start_position_lla, end_position_lla, cruise_speed):
    end_position_NED = nt.lla2ned(end_position_lla, start_position_lla)
    total_distance = np.sqrt((end_position_NED[0] ** 2) + (end_position_NED[1] ** 2) + (end_position_NED[2] ** 2))
    travel_time = total_distance / cruise_speed
    vel_NED = np.array([(end_position_NED[0] / travel_time), (end_position_NED[1] / travel_time), 0])

    return vel_NED

#estimate turn velocity
def estimate_turn_velocity(current_time, begin_velocity, trigger_time):
    theta_dot = math.radians(3) #turn rate (3 deg/s)
    theta = theta_dot * (current_time - trigger_time)

    current_velocity_NED = np.array([(math.cos(theta) * begin_velocity[0]), (math.sin(theta) * begin_velocity[1]), begin_velocity[2]])

    return current_velocity_NED

#Estimate climb/descent velocity
def estimate_climb_descent_velocity(start_position_lla, end_position_lla, climb_descent_rate):
    end_position_NED = nt.lla2ned(end_position_lla, start_position_lla)
    travel_time = math.fabs(start_position_lla[2] - end_position_lla[2]) / climb_descent_rate

    climb_sense = 1.0
    if (start_position_lla[2] < end_position_lla[2]):
        climb_sense = -1.0

    climb_descent_velocity_NED = np.array([(end_position_NED[0] / travel_time), (end_position_NED[1] / travel_time), (climb_sense * climb_descent_rate)])

    return climb_descent_velocity_NED