from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException, Command
import time
import socket
import exceptions
import math
import argparse


## MY START UP SEQUENCE:  THIS IS ON MY RASPBERRY PI ##
def connectMyRover():
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()

    connection_string = args.connect
    baud_rate = 57600

    vehicle = connect(connection_string,baud=baud_rate,wait_ready=True)
    return vehicle


vehicle = connectMyRover()

#version and attributes
vehicle.wait_ready('autopilot_version')
print('Autopilot version: %s'%vehicle.version)

# Does th firmware support the companion pc to set the attitude
print('Supports set attitude from companion: %s'%vehicle.capabilities.set_attitude_target_local_ned)

#Read actual position -- note: use this function to record routes
print('Position: %s'%vehicle.location.global_relative_frame)

#Read the actual attitude roll, pitch, yaw
print('Attitude: %s:'%vehicle.attitude)

#Read the actual velocity (m/s)
print('Velocity: %s'%vehicle.velocity) #North, East, Down

#When did we recieve last heartbeat
print('Last heartbeat: %s'%vehicle.last_heartbeat)

#Is the vehicle good to arm
print('Is the vehicle armable: %s'%vehicle.is_armable)

#What is total groundspeed
print('Groundspeed: %s'%vehicle.groundspeed)

vehicle.mode = VehicleMode("HOLD")

#What is the actual flight mode
print('Mode: %s'%vehicle.mode.name)


vehicle.armed = False
while vehicle.armed:
    print("Waiting for vehicle to disarm...")
    time.sleep(1)
#Is the vehicle armed
print('Armed: %s'%vehicle.armed)


# vehicle.mode = VehicleMode("MANUAL")

#Is state estimation filter ok
print('EKF ok: %s'%vehicle.ekf_ok)

time.sleep(2)


##  These are the functions for what you need ##

def start_mission(run):
    vehicle.mode = VehicleMode(run)
    while not vehicle.mode.name == vehicle.mode:
        print("Waiting for mode change...")
        time.sleep(1)


def readmission(fileName):
    print("\nReading mission from file: %s" % fileName)
    cmds = vehicle.commands
    missionlist = []
    with open(fileName) as f:
        for i , line in enumerate(f):
            if i==0:
                if not line.startswith('QGC WPL 110'):
                    raise Exception('File is not supported WP version')
            else:
                linearray=line.split("\t")

                print(linearray)
                ln_index=int(linearray[0])
                ln_currentwp=int(linearray[1])
                ln_frame=int(linearray[2])
                ln_command=int(linearray[3])
                ln_param1=float(linearray[4])
                ln_param2=float(linearray[5])
                ln_param3=float(linearray[6])
                ln_param4=float(linearray[7])
                ln_param5=float(linearray[8])
                ln_param6=float(linearray[9])
                ln_param7=float(linearray[10])
                ln_autocontinue=int(linearray[11].strip())
                cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
                missionlist.append(cmd)

    return missionlist



def upload_mission(fileName):
    missionlist = readmission(fileName)

    print("\nUpload mission from file: %s" % fileName)

    print('Clear Current Mission')
    cmds = vehicle.commands
    cmds.clear()

    for command in missionlist:
        cmds.add(command)
    print('Upload Mission')
    vehicle.commands.upload()
    print('Mission Uploaded')
    upload_mission_label["text"] = "Mission Uploaded: listfile.txt"


def download_mission():

    print(" Download mission from vehicle")
    missionlist=[]
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()
    for cmd in cmds:
        missionlist.append(cmd)
    return missionlist

def save_mission(fileName):
    """
    Save a mission in the Waypoint file format
    (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
    """
    print("\nSave mission from Vehicle to file: %s" % fileName)
    #Download mission from vehicle
    missionlist = download_mission()
    #Add file-format information
    output='QGC WPL 110\n'
    #Add home location as 0th waypoint
    home = vehicle.home_location
    output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
    #Add commands
    for cmd in missionlist:
        commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
        output+=commandline
    with open(fileName, 'w') as file_:
        print(" Write mission to file")
        file_.write(output)


import_mission_filename = 'listfile.txt'
export_mission_filename = 'exportedmission.txt'