from mc_classes import mc_classes as mcc
from aux_functions import mission_functions, helper_functions, matrix_math, network_functions
from drone_config import drone_config
import time

#FC comms at logon
def fc_logon(drone_obj, aoc_logon_result):
    print("entered fc logon")
    waypoints = aoc_logon_result['waypoints']
    num_waypoints = aoc_logon_result['num_waypoints']
    takeoff_included = aoc_logon_result['takeoff_included']
    land_included = aoc_logon_result['land_included']
    parsed_response = aoc_logon_result['parsed_response']
    
    #Save parsed response
    helper_functions.param_to_file("parsed_response", parsed_response)

    #create and run mission
    mission_functions.create_mission(drone_obj, waypoints, num_waypoints, takeoff_included, land_included)
    
    print("left fc logon")


#FC comms at logoff
def fc_logoff(drone_obj):
    print("entered fc logoff")
    
    #Disarm
    disarm_response = helper_functions.mavlinkCmdSend(drone_obj, 400, [0, 0, 0, 0, 0, 0, 0])
    
    #clear mission
    drone_obj.waypoint_clear_all_send()
    print("exit fc logoff")


#FC Comms in mission
def fc_mission(aoc_data, drone_obj):
    print("entered fc_comms")
    
    compulsory_land = aoc_data['compulsory_land']

    if (compulsory_land):
        #put drone in land mode
        #drone_obj.mav.mav_cmd_do_set_standard_mode_send(mcc.mav_enums.MAV_STANDARD_MODE_LAND)
        land_response = helper_functions.mavlinkCmdSend(drone_obj, 262, [7, 0, 0, 0, 0, 0, 0])

        print("leaving fc comms")

        return
    
    posn_estimate = aoc_data['posn_estimate']

    #retrieve EKF origin
    global_posn_origin = drone_obj.recv_match(condition=None,type='HOME_POSITION', blocking=True)
    ekf_origin = [global_posn_origin.latitude, global_posn_origin.longitude, global_posn_origin.altitude]

    bool_mission_changed = aoc_data['mission_changed']

    if (bool_mission_changed): #processing if mission has been changed
        waypoints = aoc_data['waypoints']
        num_waypoints = aoc_data['num_waypoints']
        takeoff_included = aoc_data['takeoff_included']
        land_included = aoc_data['land_included']
        parsed_response = aoc_data['parsed_response']
        
        #Save parsed response
        helper_functions.param_to_file("parsed_response", parsed_response)
        
        #create and run mission
        orbit_response = helper_functions.mavlinkCmdSend(drone_obj, 34, [10, 7, 0, 0, float("nan"), float("nan"), float("nan")])
        mission_functions.create_mission(drone_obj, waypoints, num_waypoints, takeoff_included, land_included)
        mission_response = helper_functions.mavlinkCmdSend(drone_obj, 262, [6, 0, 0, 0, 0, 0, 0])
        
    #process traffic data
    traffic_data = aoc_data['traffic_data']
    helper_functions.traffic2adsb(drone_obj, traffic_data)
        
    #Check if drone is landing and send landing target updates
    mission_progress = mission_functions.mission_progress(drone_obj)
    if (mission_progress == 1): #Land phase

        #check if land clearance has been granted
        land_clearance = helper_functions.param_from_file("land_clearance")

        #request land
        if (land_clearance == 0):
            positionInCircuit = network_functions.requestLand()

            if (positionInCircuit != 0): #reply is wait in circuit

                #put vehicle in orbit mode
                orbitCmdResult = helper_functions.mavlinkCmdSend(drone_obj, 34, [10, 7, 0, 0, float("nan"), float("nan"), (15.24 + positionInCircuit * 15.24)])
            else:
                helper_functions.param_to_file("land_clearance", 1)

        else:
            #return to mission mode
            #drone_obj.mav.mav_cmd_do_set_standard_mode_send(mcc.mav_enums.MAV_STANDARD_MODE_MISSION)
            mission_response = helper.mavlinkCmdSend(drone_obj, 262, [6, 0, 0, 0, 0, 0, 0])

            #compute relative position to landing target
            land_target = helper_functions.landing_target_posn_estimate(ekf_origin, posn_estimate)
            drone_obj.mav.landing_target_send(
                time_usec=(time.time_ns() * 1000), 
                frame=mcc.mav_enums.MAV_FRAME_LOCAL_NED,  
                x=land_target[0], 
                y=land_target[1],
                z=land_target[2],
                q=[1, 0, 0, 0],
                type=mcc.mav_enums.LANDING_TARGET_TYPE_RADIO_BEACON,
                posn_valid=1)

    if (mission_progress != 1):        
        #Calculate and set speed
        parsed_response = helper_functions.param_from_file("parsed_response")
        ref_speed = helper_functions.calc_ref_velocity(parsed_response)
        
        #drone_obj.mav.mav_cmd_do_change_speed_send(mcc.mav_enums.SPEED_TYPE_GROUNDSPEED, ref_speed, -1)
        speed_response = helper_functions.mavlinkCmdSend(drone_obj, 178, [1, ref_speed, -1, 0, 0, 0, 0])

    print("leaving fc comms")