from xml.dom import minidom
import geopy.distance
import os 
from math import atan2, pi
class Point:
    def __init__(self, x, y):
        self.x = x
        self.y = y
    def angleFromSelf(self, other):
        angle = atan2(self.y-other.y, self.x-other.x)
        if angle<0:
            angle+=2*pi
        return angle
def rad_2_deg(angle):
    return angle*180/pi


def ConvertToKML(plan_instance, tree_coordinates_array, coordinates):
    # Convert plan to KML format
        i = 1
        root = minidom.Document()
        xml = root.createElement('mission') 
        root.appendChild(xml)

        for coordinate in coordinates:
            # waypoint coordinates with speed data
            waypoint = root.createElement('missionitem')
            waypoint.setAttribute('no', str(i))
            waypoint.setAttribute('action', "WAYPOINT")
            waypoint.setAttribute('lat', str(coordinate[1]))
            waypoint.setAttribute('lon',str(coordinate[0]))
            waypoint.setAttribute('alt',str(2))
            waypoint.setAttribute('parameter1', "0")
            waypoint.setAttribute('parameter2', "0")
            waypoint.setAttribute('parameter3', "0")
            waypoint.setAttribute('flag', "0")
            xml.appendChild(waypoint)

            i = i + 1
            lengthLine = 1000000
            closestTree = 0
            for j in range(len(tree_coordinates_array)):
                tree_coordinates = tree_coordinates_array[j]
                point_coordinates = [coordinate[1], coordinate[0]]
                distance = geopy.distance.geodesic(tree_coordinates, point_coordinates).m
                if lengthLine >= distance:
                    lengthLine = distance
                    closestTree = tree_coordinates
            
            tree_point = Point(closestTree[0],closestTree[1])
            flight_point = Point(coordinate[1],coordinate[0])
            turn_degrees = rad_2_deg(tree_point.angleFromSelf(flight_point))
            print(closestTree[0],closestTree[1])
            print(coordinate[1],coordinate[0])
            print(turn_degrees)
            set_head = root.createElement('missionitem')
            set_head.setAttribute('no', str(i))
            set_head.setAttribute('action', "SET_HEAD")
            set_head.setAttribute('lat', "0")
            set_head.setAttribute('lon', "0")
            set_head.setAttribute('alt', "0")
            set_head.setAttribute('parameter1', str(int(turn_degrees)))
            set_head.setAttribute('parameter2', "0")
            set_head.setAttribute('parameter3', "0")
            set_head.setAttribute('flag', "0")
            xml.appendChild(set_head)
            i = i + 1

        # landing point
        last_coordinate = plan_instance.flight_plan['geometry']['coordinates'][0][0]
        landingpoint = root.createElement('missionitem')
        landingpoint.setAttribute('no', str(i))
        landingpoint.setAttribute('action', "LAND")
        landingpoint.setAttribute('lat', str(last_coordinate[1]))
        landingpoint.setAttribute('lon',str(last_coordinate[0]))
        landingpoint.setAttribute('alt',"0")
        landingpoint.setAttribute('parameter1', "2")
        landingpoint.setAttribute('parameter2', "0")
        landingpoint.setAttribute('parameter3', "0")
        landingpoint.setAttribute('flag', "0")
        xml.appendChild(landingpoint)


        return root.toprettyxml() 


# QGC WPL <VERSION>
# <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LATITUDE> <PARAM6/Y/LONGITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE>
#
#
# 
def ConvertToArduPilot(plan_instance, tree_coordinates_array, coordinates):
    # Convert plan to ArduPilot format
        # find the largest altitude in coordinates
        max_altitude = 0
        for coordinate in coordinates:
            if coordinate[2] > max_altitude:
                max_altitude = coordinate[2]



        plain_text = "QGC WPL 110\n0\t1\t0\t0\t0\t0\t0\t0\t0\t0\t0\t1\n"
        i = 1
        plain_text = plain_text + str(i) + "\t0\t3\t22\t0.000000\t0.000000\t0.000000\t0.000000\t0.000000\t0.000000\t"+ str(max_altitude) +"\t1\n"
        i = i + 1

        for coordinate in coordinates:
            plain_text += str(i) + "\t0\t3\t16\t0.000000\t0.000000\t0.000000\t0.000000\t" + str(coordinate[1]) + "\t" + str(coordinate[0]) + "\t"+ str(max_altitude) +"\t1\n"
            i = i + 1
            lengthLine = 1000000
            closestTree = 0
            for j in range(len(tree_coordinates_array)):
                tree_coordinates = tree_coordinates_array[j]
                point_coordinates = [coordinate[1], coordinate[0]]
                distance = geopy.distance.geodesic(tree_coordinates, point_coordinates).m
                if lengthLine >= distance:
                    lengthLine = distance
                    closestTree = tree_coordinates
            tree_point = Point(closestTree[0],closestTree[1])
            flight_point = Point(coordinate[1],coordinate[0])
            turn_degrees = rad_2_deg(tree_point.angleFromSelf(flight_point))
            plain_text += str(i) + "\t0\t3\t115\t" + str(int(turn_degrees)) + "\t0.000000\t0.000000\t0.000000\t0.000000\t0.000000\t"+ str(max_altitude) +"\t1\n"
            i = i + 1
            # take a picture
            plain_text += str(i) + "\t0\t3\t2000\t1\t0.000000\t0.000000\t"+ str(coordinate[4])  + "\t0.000000\t0.000000\t"+ str(max_altitude) +"\t1\n"
            i = i + 1

        plain_text = plain_text + str(i+1) + "\t0\t3\t21\t0.000000\t0.000000\t0.000000\t0.000000\t" + str(coordinate[1]) + "\t" + str(coordinate[0]) + "\t"+ str(max_altitude) +"\t1"

        return plain_text