dronepath

import folium
import json
from geopy.distance import geodesic
import datetime
import decimal


#create map object
#different tiles (map styles) can be used, like 'Stamen Toner', 'Stamen Terrain', ...

#SETUP MAPPA
m = folium.Map(location=[43.81839701513138, 10.874407867954424], tiles='OpenStreetMap', zoom_start=10)

#CONFIGURAZIONE DRONE
v = 30 #km/h
v_ms = v/3.6 #m/s

battery_capacity = 30*60 #%s
current_capacity = battery_capacity
low_battery_percentage = 0.25

takeoff_time = 10
charging_capacity = 3 #s/s

delta = 1

#SETUP GRAFO
file = "CP.json"
km = 20


path = []



A = [10.868582820343955,43.943903280475745]
B = [11.3556223267028783,43.776762971467626]
pisa = [10.53406560359761, 43.827866455929644]
firenze = [11.31860339831112,43.768645934352094]
spezia = [10.059203674430062, 44.21233580919796]
agliata = [11.040129230323007,43.904717805526865]
viareggio = [10.411424635775173, 43.93842229074731]


def inserisciMarker(mappa, lat,lng, popup, icon, color):
    folium.Marker(location=[lat, lng], tooltip=popup, icon=folium.Icon(icon=icon, color=color)).add_to(mappa)


def inserisciLinea(mappa, points, color, w, opacity, tooltip):
    folium.PolyLine(points, tooltip= tooltip , color=color, weight=w, opacity=opacity).add_to(mappa)


def inserisciCerchio(mappa, lat, lng, radius):
    folium.CircleMarker([lat, lng],radius=radius*10/2, fill=True,opacity= 0.1).add_to(mappa)

def inserisciPunto(mappa, lat, lng, txt):
    folium.CircleMarker([lat, lng],radius=5,fill=True, color="black", popup=txt).add_to(mappa)

def properTime(t):
    return str(datetime.timedelta(seconds=t))[:7]

def computeDistance(p1,p2):
    p1tuple = (p1[1], p1[0])
    p2tuple = (p2[1], p2[0])
    return geodesic(p1tuple, p2tuple)


def lowBattery(battery_left):
    return battery_left <= low_battery_percentage*battery_capacity


def createGraph(file,dist):
    with open(file) as json_file:
        json_data = json.load(json_file)
        grafo = {}

        print(len(json_data['features']))

        for p in json_data['features']:
            coord = p["geometry"]["coordinates"]
            s = p["properties"]["code"]
            inserisciMarker(m, coord[1], coord[0], s, "", "blue")
            #inserisciCerchio(m, coord[1], coord[0], km)
            #print(s)
            grafo[s] = {}
            grafo[s]["coords"] = coord
            grafo[s]["adjacent"] = []
            #print(coord)
            coordtuple = (coord[1],coord[0])
            for pp in json_data['features']:
                if s != pp["properties"]["code"]:
                    pptuple = (pp["geometry"]["coordinates"][1], pp["geometry"]["coordinates"][0])
                    d = geodesic(coordtuple, pptuple).km
                    if d < dist:
                        l = []
                        l.append(coordtuple)
                        l.append(pptuple)
                        #inserisciLinea(m,l,"blue",3,0.2, d)
                        grafo[s]["adjacent"].append(pp["properties"]["code"])

        with open('data.json', 'w') as fp:
            json.dump(grafo, fp, indent=4)

        return grafo


def reachable(point, destination, battery_left):
    dist = computeDistance(point, destination).m
    t = dist / v_ms
    if t < battery_left:
        return True
    return False

def goToCharge(point, destination, battery_left):
    dist = computeDistance(point, destination).m
    t = dist / v_ms
    charging_time = (battery_capacity - battery_left - t) / charging_capacity
    #uso charging time per il marker da mettere con il tempo impiegato
    return [battery_capacity - t, charging_time]

def move(source, destination, x):  #returns new point, x km closer to destination
    dist = computeDistance(source,destination).km
    t = x/dist  #t is not time, it's an angle
    #t = decimal.Decimal.from_float(x/dist)
    #val = decimal.Decimal.from_float(1-t)
    # (((1−𝑡)𝑥0+𝑡𝑥1),((1−𝑡)𝑦0+𝑡𝑦1))
    return [(1-t)*source[0] + t*destination[0], (1-t)*source[1] + t*destination[1]]
    #return [(val)*source[0] + t*destination[0], (val)*source[1] + t*destination[1]]

def fromLocGetClosestStation(location,file):
    with open(file) as json_file:
        json_data = json.load(json_file)
        minimo = 10000000000
        closest = None
        finaltuple = None
        locationtuple = (location[1], location[0])
        for p in json_data['features']:
            coord = p["geometry"]["coordinates"]
            coordtuple = (coord[1], coord[0])
            s = p["properties"]["code"]
            dist = geodesic(coordtuple, locationtuple).km
            if dist < minimo:
                closest = s
                minimo = dist
                finaltuple = coordtuple

        l = []
        l.append(locationtuple)
        l.append(finaltuple)
        #inserisciLinea(m, l, "red", 4, 1, "")
        return [finaltuple[1],finaltuple[0]]

def fromLocGetClosestStationOld(location,file):
    with open(file) as json_file:
        json_data = json.load(json_file)
        minimo = 10000000000
        closest = None
        finaltuple = None
        locationtuple = (location[1], location[0])
        for p in json_data['features']:
            coord = p["geometry"]["coordinates"]
            coordtuple = (coord[1], coord[0])
            s = p["properties"]["code"]
            dist = geodesic(coordtuple, locationtuple).km
            if dist < minimo:
                closest = s
                minimo = dist
                finaltuple = coordtuple

        l = []
        l.append(locationtuple)
        l.append(finaltuple)
        #inserisciLinea(m, l, "red", 4, 1, "")
        return closest

def getClosestReachableStation(location,file, time_left):
    station = fromLocGetClosestStation(location,file)
    print("stazione: " + str(station))
    flag = reachable(location,station,time_left)
    if flag:
        return station
    return None

def fromStationsGetClosestStation(sourceStation,destStation,grafo):
    minimo = 10000000000
    closest = None

    print(grafo[sourceStation]["adjacent"])
    for node in grafo[sourceStation]["adjacent"]:
        coord = grafo[node]["coords"]
        coordtuple = (coord[1], coord[0])

        locationtuple = (grafo[destStation]["coords"][1], grafo[destStation]["coords"][0])
        dist = geodesic(coordtuple, locationtuple).km
        if dist < minimo:
            minimo = dist
            closest = node

    return closest


def recursivePath(source,destination, punti,battery_left, delta):

    t = delta*1000 / v_ms
    battery_left -= t
    #print(battery_left)
    dist = computeDistance(source,destination).km
    print(dist)
    if dist < delta and reachable(source, destination, battery_left) and len(punti) == 0:
        print("raggiunto")
        return True

    #print(lowBattery(battery_left))

    if lowBattery(battery_left):
        s = getClosestReachableStation(source,file,battery_left)
        #print("stazione: " + s)
        if s == None:
            return False
        else:
            gtc = goToCharge(source, s, battery_left)  # variabile di comodo!!!!!
            battery_left = gtc[0]
            path.append([[[source[1],source[0]],[s[1],s[0]]], 2, gtc[1]])

    if dist < delta and reachable(source, destination, battery_left) and len(punti) > 0:
        print("ciaooooooo")
        path.append([destination, 1]) #deve diventare 4
        new_source = destination
        new_destination = punti.pop(0)
        flag = recursivePath(new_source, new_destination, punti, battery_left, delta)
        if flag:
            return True
        else:
            path.pop()
            punti.insert(0,new_destination)
            s = getClosestReachableStation(source, file, battery_left)
            if s == None:
                return False
            else:
                gtc = goToCharge(source, s, battery_left) #variabile di comodo!!!!!
                battery_left = gtc[0]
                path.append([[[source[1],source[0]],[s[1],s[0]]], 3, gtc[1]])
                path.append([destination, 1]) #diventerà 4!!!!!!!!!!!
                new_source = destination
                new_destination = punti.pop(0)
                flag = recursivePath(new_source,new_destination,punti,battery_left,delta)
                if flag:
                    return True
                else:
                    path.pop()
                    path.pop()
                    punti.insert(0, new_destination)
                    return False

    else:
        new_point = move(source, destination, delta)

        path.append([new_point, 1])
        flag = recursivePath(new_point,destination,punti,battery_left,delta)
        if flag:
            return True
        else:
            print("falsooooooo")
            path.pop()
            s = getClosestReachableStation(source, file, battery_left)
            if s == None:
                return False
            else:
                gtc = goToCharge(source, s, battery_left) #variabile di comodo!!!!!
                battery_left = gtc[0]
                path.append([[[source[1],source[0]],[s[1],s[0]]], 3, gtc[1]])
                path.append([new_point, 1])
                flag = recursivePath(new_point,destination,punti,battery_left,delta)
                if flag:
                    return True
                else:
                    path.pop() #doppio backtracking!!!!!!
                    path.pop()
                    return False


def computePath(punti, battery_left, delta):
    x = len(punti)
    if x < 2:
        print("ERRORE")
        return False
    source = punti.pop(0)
    first_destination = punti.pop(0)
    print(x)
    if x == 2:
        return recursivePath(source,first_destination,[],battery_left, delta)

    return recursivePath(source,first_destination,punti,battery_left, delta)



def disegnaLinee(lista):
    for i in range(len(lista)-1):
        l = []
        l.append((lista[i][1], lista[i][0]))
        l.append((lista[i+1][1], lista[i+1][0]))
        inserisciLinea(m, l, "blue", 6, 0.8, "")


def disegnaPercorso(path):
    total_time = 0
    total_dist = 0
    t = delta * 1000 / v_ms

    for el in path:
        print(el)
        if el[1] == 1:
            total_time += t
            total_dist += delta
            inserisciPunto(m,el[0][1],el[0][0],str(datetime.timedelta(seconds=total_time))[:7])
        else:
            distance_to_reach = computeDistance(el[0][0],el[0][1])
            time_to_reach = distance_to_reach.m / v_ms
            time_to_charge = el[2]
            total_dist += (distance_to_reach.km*2)
            total_time += (time_to_reach*2) + el[2]
            inserisciMarker(m,el[0][1][0],el[0][1][1],str(datetime.timedelta(seconds=time_to_charge))[:7],"","purple")

            if el[1] == 2:
                inserisciLinea(m, el[0],"red",4,1,"andata e ritorno: " + str(datetime.timedelta(seconds=time_to_reach*2))[:7] + ", " + str(distance_to_reach.km*2)[:5] + "km")
            if el[1] == 3:
                inserisciLinea(m, el[0],"green",4,1,"andata e ritorno: " + str(datetime.timedelta(seconds=time_to_reach*2))[:7] + ", " + str(distance_to_reach.km*2)[:5] + "km")

    return [total_time,total_dist]

source = pisa
destination = firenze
punti = [A,B,firenze,pisa, viareggio]
disegnaLinee(punti)
dist = computeDistance(source,destination).km
grafo = createGraph(file,km)


l = []
l.append((source[1],source[0]))
l.append((destination[1],destination[0]))
#inserisciLinea(m, l, "blue", 6, 0.8, "")




print(dist)
np = source

# while dist > 10:
#     np = move(np,destination,10)
#     dist -= 10
#     inserisciMarker(m, np[1], np[0], "source", "", "purple")


#print(recursivePath(source,destination,[],battery_capacity,delta))
print(computePath(punti,battery_capacity,delta))
print(path)
res = disegnaPercorso(path)

inserisciMarker(m, source[1], source[0], "source: " + str(dist)[:5] + "km", "", "red")
inserisciMarker(m, destination[1], destination[0], "destination reached in " + properTime(res[0]) + " doing " + str(res[1])[:5] + "km in total", "", "green")


m.save('inspection.html')