import dtpyodid
import time
from drone_config import drone_config as drone_config
import hashlib
import math
from aux_functions import helper_functions as helper
import pymavlink


#Authetication message
def createAuthMessage():
    drone_id = drone_config.get_param("uas_id")
    time_val = int (time.time()) - 1546300800
    #unHashedStr = drone_id + str(time_val)
    #hashedStr = hashlib.md5(unHashedStr.encode('utf-8')).hexdigest()
    hashedStr = drone_id

    authMessage = dtpyodid.messages.auth.Auth(
        auth_type=1, #UAS ID authentication
        auth_data_page=0, #page 0
        auth_last_page_index=0,
        auth_length=len(hashedStr),
        auth_timestamp=time_val,
        auth_data=hashedStr
        )
    
    return authMessage

#Basic ID message
def createBasicIDmessage():

    basicIDmessage = dtpyodid.messages.basicid.BasicID()
    basicIDmessage.id_type = dtpyodid.messages.basicid.BasicID_ID_Type.UTM_ASSIGNED_UUID
    basicIDmessage.ua_type = dtpyodid.messages.basicid.BasicID_UA_Type.HYBRID_LIFT
    basicIDmessage.uas_id = drone_config.get_param("display_name")

    return basicIDmessage


#Location message
def createLocationMessage(droneObj):
    locationMessage = dtpyodid.messages.location.Location(0, 0, 0)

    droneMessage = droneObj.messages

    #raw GPS data
    droneGPSraw = droneObj.recv_match(condition=None,type='GPS_RAW_INT')

    #position
    dronePosn = droneObj.recv_match(condition=None,type='GLOBAL_POSITION_INT')
    locationMessage.latitude = 1E-7 * dronePosn.lat
    locationMessage.longitude = 1E-7 * dronePosn.lon
    locationMessage.altitude_geo = 1E-3 * droneGPSraw.alt_ellipsoid

    #status
    droneHeartbeat = droneMessage["HEARTBEAT"]
    droneStatus = droneHeartbeat.system_status

    locationMessage.status = dtpyodid.messages.location.Location_Status.NONE
    if (droneStatus == pymavlink.dialects.v20.common.MAV_STATE_STANDBY):
        locationMessage.status = dtpyodid.messages.location.Location_Status.ON_GROUND
    elif (droneStatus == pymavlink.dialects.v20.common.MAV_STATE_ACTIVE):
        locationMessage.status = dtpyodid.messages.location.Location_Status.IN_AIR

    #height and altitude
    droneAltitude = droneObj.recv_match(condition=None,type='ALTITUDE')
    locationMessage.height = droneAltitude.altitude_relative
    locationMessage.height_type = dtpyodid.messages.location.Location_Height_Type.ABOVE_START
    locationMessage.altitude_baro = droneAltitude.altitude_amsl

    #horizontal speed
    droneHorzSpeed = 1E-2 * math.sqrt((dronePosn.vx ** 2) + (dronePosn.vy ** 2))
    locationMessage.speed_horizontal = int(droneHorzSpeed)

    speedMult = 0
    speedThreshold = 225 * 0.25
    if (droneHorzSpeed > speedThreshold):
        speedMult = 1
    
    locationMessage.speed_mult = speedMult

    #vertical speed
    locationMessage.speed_vertical = int(1E-2 * dronePosn.vz)
    
    #direction
    locationMessage.direction = int(1E-2 * dronePosn.hdg)

    ew_direction = 0
    if (dronePosn.hdg > 180):
        ew_direction = 1
    
    locationMessage.ew_direction = ew_direction
    
    #timestamp
    locationMessage.timestamp = time.time()

    #accuracy
    locationMessage.accuracy_horizontal = int(1E-3 * droneGPSraw.h_acc)
    locationMessage.accuracy_vertical = int(1E-3 * droneGPSraw.v_acc)
    locationMessage.accuracy_speed = int(1E-3 * droneGPSraw.vel_acc)

    return locationMessage


#Operator ID message
def createOperatorIDmessage():
    operatorIDmessage = dtpyodid.messages.operatorid.OperatorID()

    return operatorIDmessage


#Self ID message
def createSelfIDmessage():
    selfIDmessage = dtpyodid.messages.selfid.SelfID(
        desc = "Package delivery drone",
        desc_type = dtpyodid.messages.selfid.Type.TEXT
        )

    return selfIDmessage


#System message
def createSystemMessage(homePosn):
	systemMessage = dtpyodid.messages.system.System()
	systemMessage.operator_location_type = dtpyodid.messages.system.System_Operator_Location_Type.TAKEOFF
	systemMessage.latitude = 1E-7 * homePosn.lat
	systemMessage.longitude = 1E-7 * homePosn.lon

	return systemMessage


#Form message pack
def formMessagePack(droneObj, homePosn):    
	auth_msg = createAuthMessage()
	basicID_msg = createBasicIDmessage()
	loc_msg = createLocationMessage(droneObj)
	operatorID_msg = createOperatorIDmessage()
	selfID_msg = createSelfIDmessage()
	system_msg = createSystemMessage(homePosn)
	
	messagePackMessage = dtpyodid.messages.messagepack.MessagePack()
	messagePackMessage.messages = [auth_msg, basicID_msg, loc_msg, operatorID_msg, selfID_msg, system_msg]

	messagesInBuffer = messagePackMessage.pack()
	messagesInBuffer = messagesInBuffer + b"\0" * (225 - len(messagesInBuffer))

	return messagesInBuffer


#send message pack
def sendMessagePack(droneObj, odidObj, homePosn):
    messagesInBuffer = formMessagePack(droneObj, homePosn)

    odidObj.mav.open_drone_id_message_pack_send(
        target_system = 0, 
        target_component = 0, 
        id_or_mac = (0x0).to_bytes(20, "little"), 
        single_message_size = 25, 
        msg_pack_size = 6, 
        messages = messagesInBuffer, 
        force_mavlink1 = False
        )


#receive message pack
def receiveProcessMessagePack(odidObj, droneObj):
    try:
        odidMavlinkMessage = odidObj.messages["OPEN_DRONE_ID_MESSAGE_PACK"]
    except:
        return

    mavlinkMessageData = odidMavlinkMessage.messages

    messagePackMessage = dtpyodid.messages.messagepack.MessagePack() #placeholder
    messagePackData = messagePackMessage.parse(data = mavlinkMessageData)

    #process messages
    for message in messagePackMessage.messages:
        if (message.rid == 0x2): #authentication message
            auth_data = message.auth_data.decode('utf-8')

        elif (message.rid == 0x0): #basic id message
            uas_id = message.uas_id

        elif (message.rid == 0x1): #location message            
            latitude = message.latitude
            longitude = message.longitude
            altitude_geo = message.altitude_geo
            vx = message.speed_horizontal * math.cos(math.radians(message.direction))
            vy = message.speed_horizontal * math.sin(math.radians(message.direction))
            vz = -message.speed_vertical
            time_stamp = message.timestamp
    
    
    #generate and send obstacle distance message to drone
    helper.genObstacleDistance(droneObj, [latitude, longitude, altitude_geo], time_stamp)

    #Store other drone received data for later reporting to Tower
    odidStruct = {"uas_id" : uas_id, "auth_data" : auth_data, "location_state" : [latitude, longitude, altitude_geo, vx, vy, vz, time_stamp]}
    helper.param_to_file("odid_data", odidStruct)