from tasks import aoc_comms, fc_comms, odid_comms
from pymavlink import mavutil
from drone_config import drone_config as drone_config
from mc_classes import mc_classes as mcc
from aux_functions import helper_functions as helper
from aux_functions import network_functions as nf
from aux_functions import signal_functions as signal


#Check and log fuel
#helper.fuelCheck()

#initialize land clearance global
helper.param_to_file("land_clearance", 0)

#connect to drone
signal.raiseFlag("ConnectingToDrone")
drone_obj = mavutil.mavlink_connection(device='/dev/ttyAMA3', baud=57600, source_system=191, source_component=0, dialect="common")
drone_obj.wait_heartbeat()  
signal.raiseFlag("ConnectedToDrone")

#connect to odid module
odid_obj = mavutil.mavlink_connection(device='/dev/ttyAMA4', baud=57600, source_system=191, source_component=0, dialect="common")
odid_obj.wait_heartbeat()


#wait for initial position estimate
signal.raiseFlag("ObtainingPosnEstimate")
while True:
    globalPosnMsg = drone_obj.recv_match(condition=None,type='GLOBAL_POSITION_INT')
    if globalPosnMsg:
        break
signal.raiseFlag("GlobalPosnObtained")

homePosn = globalPosnMsg

#send ODID message
odid_comms.sendMessagePack(drone_obj, odid_obj, homePosn) #send out own ODID information

#request takeoff
while (True):
takeoffRequestResponse = nf.requestTakeoff()

    if (takeoffRequestResponse['value']):
        signal.raiseFlag("ClearedForTakeoff")
        break
    else:
        signal.raiseFlag("TakeoffDenied")


#logon stage
aoc_logon_result = aoc_comms.aoc_logon()

#fc comms at logon
fc_comms.fc_logon(drone_obj, aoc_logon_result)


#activate 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])

#arm drone
signal.raiseFlag("ArmingDrone")
#drone_obj.mav.mav_cmd_component_arm_disarm_send(mcc.mav_enums.MAV_BOOL_TRUE, mcc.mav_enums.MAV_BOOL_FALSE)
arm_response = helper.mavlinkCmdSend(drone_obj, 400, [1, 0, 0, 0, 0, 0, 0])
signal.raiseFlag("DroneArmed")

#mission stage   
missionCounterVal = 0; 
while (True):
    odid_comms.receiveProcessMessagePack(odid_obj, drone_obj) #receive BLE ODID information
    data2fc = aoc_comms.aoc_mission(drone_obj)
    fc_comms.fc_mission(data2fc, drone_obj)
    odid_comms.sendMessagePack(drone_obj, odid_obj, homePosn) #send out own ODID information

    #check if mission complete
    mission_msg = drone_obj.recv_match(condition=None,type='MISSION_CURRENT', blocking=True)
    mission_state = mission_msg.mission_state

    if (mission_state == (mcc.mav_enums.MISSION_STATE_COMPLETE)):
        break
    
    missionCounterVal = missionCounterVal + 1

    if (missionCounterVal == drone_config.get_param("max_fuel_interval")):
        #helper.fuelCheck() #Check and log fuel
        missionCounterVal = 0 #reset counter

#logoff stage
aoc_comms.aoc_logoff()
fc_comms.fc_logoff(drone_obj)


#Maintenance functions
#helper.fuelCheck() #Check and log fuel