from mc_classes import mc_classes as mcc
from drone_config import drone_config as drone_config
from aux_functions import matrix_math as m_math
from aux_functions import signal_functions as signal
from aux_functions import helper_functions as helper
from pymavlink import mavwp


#Create mission
def create_mission(drone_obj, waypoints, num_waypoints, bool_includes_takeoff, bool_includes_land):
    print("creating mission")
    
    #clear any old mission
    drone_obj.mav.mission_clear_all_send(drone_obj.target_system, drone_obj.target_component)

    #Initiate mission upload
    drone_obj.mav.mission_count_send(drone_obj.target_system, drone_obj.target_component, num_waypoints)
    
    #respond to drone's request for each mission item
    for idx in range(num_waypoints):
        msg = drone_obj.recv_match(type='MISSION_REQUEST_INT', blocking=True, timeout=5)
       
        curr_waypoint = m_math.select_array(waypoints, [0, 2], [idx, idx], 4)
        curr_waypoint = [float(curr_waypoint[0]), float(curr_waypoint[1]), float(curr_waypoint[2])]
        
        if msg:
            print(f"sending waypoint {idx} to drone")

            if ((bool_includes_takeoff) and (idx == 0)):
                
                #takeoff item
                drone_obj.mav.mission_item_int_send(
                    1, 
                    1, 
                    0, 
                    0, 
                    84, 
                    1, 
                    1, 
                    0.0, 
                    1.0, 
                    0.0, 
                    float("nan"), 
                    int(1E7*curr_waypoint[0]), 
                    int(1E7*curr_waypoint[1]), 
                    curr_waypoint[2],
                    0,
                    False)      

            elif ((bool_includes_land) and (idx == num_waypoints - 2)):
                
                #Loiter (to land) item
                drone_obj.mav.mission_item_int_send(
                    1, 
                    1, 
                    (num_waypoints - 2), 
                    0, 
                    31, 
                    0, 
                    1, 
                    1.0, 
                    0.0, 
                    0.0, 
                    0.0, 
                    int(1E7*curr_waypoint[0]), 
                    int(1E7*curr_waypoint[1]), 
                    curr_waypoint[2])
                
            elif ((bool_includes_land) and (idx == num_waypoints - 1)):
                
                #Land item
                drone_obj.mav.mission_item_int_send(
                    1, 
                    1, 
                    (num_waypoints - 1), 
                    0, 
                    21, 
                    0, 
                    1, 
                    0.0, 
                    1.0, 
                    0.0, 
                    float("nan"), 
                    int(1E7*curr_waypoint[0]), 
                    int(1E7*curr_waypoint[1]), 
                    curr_waypoint[2])


                helper.param_to_file("land_location", curr_waypoint)

            else:
                #standard waypoint
                drone_obj.mav.mission_item_int_send(
                    1, 
                    1, 
                    idx, 
                    0, 
                    16, 
                    0, 
                    1, 
                    0.0, 
                    5.0, 
                    0.0, 
                    float("nan"), 
                    int(1E7*curr_waypoint[0]), 
                    int(1E7*curr_waypoint[1]), 
                    curr_waypoint[2])
    
    #Wait for drone to confirm mission upload
    ackMsg = drone_obj.recv_match(type='MISSION_ACK', blocking=True)

    if ackMsg:
        print("all waypoints loaded successfully")
    else:
        print("some waypoints not uploaded")


#determine mission progress
def mission_progress(drone_obj):
    seq_msg = drone_obj.messages["MISSION_CURRENT"]
    curr_seq = seq_msg.seq
    total_seq = seq_msg.total
        
    result = curr_seq / total_seq
    
    return result