a
    f1l                     @   s   d dl mZ d dlZd dlZd dlmZmZm	Z	 d dl
Zd dlZd dlmZ d dlmZ d dlmZmZmZmZmZmZmZ dd	d
ZdS )    N)Point
LineStringPolygon)r   )geodesic)DroneStationsMissionsFlightPlansBlocksTreesTreeRowProtectedRegions   TFc                    s  t jj| dddds$tdztjj| d}W n tjyR   tdY n0 tjj| dj	}t
jj|d	}g |D ]8}|j}	t|	d
 d d }
tj|
dd}
|
 qztd |j|jgfddfdd}fdd}dd fddfdd}fddfddfd d!fd"d#}fd$d%d&d' fd(d)fd*d+	fd,d-
dC	
fd.d/	}d0d1 fd2d3  fd4d5fd6d7}dDd9d:}dEd<d=fd>d?}t| | g D ]}|||d@ q4|g D ]}|dA dBkr`| q`tg D ]}| qt||}||}||}|||}|dBkr|}|S )FNF)block_idZrow_id__isnullrow_ididu#   Šajā kvartālā nav neviena koka.)r   zNav pievienota drona stacija.)r   )	garden_idgeometrycoordinatesr      Zaxis c                    s0   t d} D ]}|j| kr|j|k r|j}q|S )Ninf)floatr   r   )r   Z	lowest_idtreetrees_from_database 5/var/www/html/django/DPS/api/flight_plan_algorithm.pyfind_first_tree_in_row?   s    

z1path_calculations.<locals>.find_first_tree_in_rowc               	      s   g } D ]}|j  |jkr<| |j|jdd|j |jg q|j  |jd krp| |j|jdd|j |jg q| |j|jdd|j |jg q| S )Nr   r   r   )r   r   append	longitudelatitude)tree_coordinatesr   )r   r   r   r   convert_from_jsonG   s       z,path_calculations.<locals>.convert_from_jsonc                     s(   g }  D ]}|j | vr| |j  q| S N)r   r   )tree_row_idsr   r   r   r   find_tree_row_idsS   s
    
z,path_calculations.<locals>.find_tree_row_idsc           	      S   s   t | tkst |tkr tdt| d }t|d }t|d | d  }t|t| }t|t| t|t| t|  }t||}t|}|d d }|S )Nz&Only tuples are supported as argumentsr   r   ih  )	typetuple	TypeErrormathradianssincosatan2degrees)	ZpointAZpointBZlat1Zlat2ZdiffLongxyZinitial_bearingZcompass_bearingr   r   r   !calculate_initial_compass_bearingZ   s    2
z<path_calculations.<locals>.calculate_initial_compass_bearingc                    s.   | d | d f} |d |d f} | |}|S )Nr   r   r   )point1point2bearing)r2   r   r   get_angle_between_two_pointsl   s    
z7path_calculations.<locals>.get_angle_between_two_pointsc                    sT   g }g }D ]<}|j  | kr*|j|jg}|j  | d kr|j|jg}q||S )Nr   )r   r    r!   )r   
first_tree	last_treeZ
coordinate)r   r6   r   r   r   tree_row_angler   s    z)path_calculations.<locals>.tree_row_anglec                    s   | d }| d }d}D ]}|d | d kr|d }qd d df | d k d d }d d df | d k d d }t dt|||d }|j}|j}	||	g||g}
||	 |
| d | d d	d
gS )Nr   r   r      angle   ZmetersZ   FTr   destinationr   r!   r    )pointr0   r1   r5   rowtree_xtree_y	point_newx_newy_newangle_to_treeflying_altituder6   max_distance_to_tree_metersrow_id_anglesr"   r   r   create_point_left|   s    
$$z,path_calculations.<locals>.create_point_leftc                    s   | d }| d }d}D ]}|d | d kr|d }qd d df | d k d d }d d df | d k d d }||g||g}t dt|||d }|j}	|j}
|	|
g||g}|	|
 || d | d d	d	gS )
Nr   r   r   r:   r;   r<   r=   r>   Fr?   )rA   r0   r1   r5   rB   rC   rD   rH   rE   rF   rG   rI   r   r   create_point_right   s    
$$z-path_calculations.<locals>.create_point_rightc                    s   | d }| d }| d |d  }| d |d  }t ||}d d df | d k d d }d d df | d k d d }	||g||	g}
| d |t |  }| d |t |  }||t |  ||t |   |
| d | d ddgS )Nr   r   r<   r:   TF)r*   r.   r-   r,   )rA   Zother_point max_distance_to_tree_coordinatesr0   r1   ZdxZdyr;   rC   rD   rH   )rJ   r6   r"   r   r   create_corner_point   s    $$z.path_calculations.<locals>.create_corner_pointc                    s   g }t t| D ]|}| | d dkrJ|| |  || |  q| | | | |d   || |  || |  qt|S )N   Tr   )rangelenr   nparray)r"   tree_path_pointsi)rP   rM   rN   rO   r   r   create_tree_rows_outer_path   s    z6path_calculations.<locals>.create_tree_rows_outer_pathc                    s   g }t d}  d d df | k }|D ]V}td |d  d d |d  d  }||d |d ||k r(|}|}q(|S )Nr   r:   r   r   r   )rA   distance)r   r*   sqrtr   )r   Zpointssmallest_distanceZrow_endpointsrW   rY   closest_point)corner_pointsstarting_pointr   r   $find_closest_point_to_starting_point   s    .z?path_calculations.<locals>.find_closest_point_to_starting_pointc                 S   sT   t d}|D ]B}t| d |d  d | d |d  d  }||k r|}|}q|S )Nr   r   r   r   )r   r*   rZ   )rA   Zpoints_leftr[   rW   rY   r\   r   r   r   find_closest_point   s    .z-path_calculations.<locals>.find_closest_pointc           	         s  g }d d df | k }| }||d d df dk }||d d df dk }||d d df dk }| | tt|D ]8}|  |d | ||d d df |d d k }qtdd	 |D }tt|}t|dkr<|d d |d kr*|d d |d kr*|d }| | n|d }| | tt|D ]:}|  |d | ||d d df |d d k }qH| | |S )
Nr:      r   r      Tc                 s   s   | ]}t |V  qd S r$   )r(   ).0r0   r   r   r   	<genexpr>       zPpath_calculations.<locals>.create_shortest_path_flying_around.<locals>.<genexpr>)r   rR   rS   setrT   rU   list)	r   path
row_pointsstarting_corner_pointleftside_pointsrightside_pointscorner_pointrW   Zunique_corner_pointsr`   r_   rV   r   r   "create_shortest_path_flying_around   s.    
",
$
z=path_calculations.<locals>.create_shortest_path_flying_aroundc                    s   g }d d df | k }| }||d d df dk }||d d df dk }| | tt|D ]L}t|dkr|d }|  |d | ||d d df |d d k }qj| | | | |S )Nr:   ra   r   rb   Fr   rc   r   rR   rS   )r   ri   rj   rk   rl   rW   first_side_pointro   r   r   *create_shortest_path_flying_left_side_only   s    
"

zEpath_calculations.<locals>.create_shortest_path_flying_left_side_onlyc                    s   g }d d df | k }| }||d d df dk }||d d df dk }| | tt|D ]L}t|dkr|d }|  |d | ||d d df |d d k }qj| | | | |S )	Nr:   ra   r   rb   Fr   rc   r   rq   )r   ri   rj   rk   rm   rW   rr   ro   r   r   +create_shortest_path_flying_right_side_only  s    
"

zFpath_calculations.<locals>.create_shortest_path_flying_right_side_onlyc                    sT   g }| D ]8}|dkr$|  | q| | | | qtj|td}|S )NF)Zdtype)r   rT   rU   object)Ztree_row_ids_array
only_sidesZshortest_pathsr   )rp   rs   rt   r   r   create_first_tree_path!  s    z1path_calculations.<locals>.create_first_tree_pathc           
      S   sX  | d d | d d  }|d d |d d  }| d d | d d  }|d d |d d  }||d d | d d   ||d d | d d    }||d d | d d   ||d d | d d    }|| d d |d d   || d d |d d    }|| d d |d d   || d d |d d    }	|| dk||	 dk@ S )Nr   r   r   )
s0s1Zdx0Zdx1Zdy0Zdy1Zp0p1p2Zp3r   r   r   
intersects-  s    8888z%path_calculations.<locals>.intersectsc              
      s  t  }|| d | d f ||d |d f |D ]}tt|}|jdddd}t|d j	j
}tj|dd}|d d }|D ]}||d |d f qtt|D ]}tt|D ]}||krt|| d || d  d || d || d  d  }	|j|| d || d f|| d || d f|	d	 qqq8 D ]@}|d |d krP|d |d krP||d |d f qP|jD ]t}|jD ]f}||krt|d |d  d |d |d  d  }	|j|d |d f|d |d f|	d	 qqg }
| D ]p}|D ]d}tt|D ]P}||| d || d f||d  d ||d  d fgr2|
| q2q"q||
 g }
| D ]}D ]}|}|d }d d d
f |k d }d d d
f |k d }||d |d f|d |d fgr|
| qq||
 t || d | d f|d |d f}|d }|}g } D ]H}|d |d kr||d |kr||d |d kr||d |d g}q|t || d | d f|d |d f}d}d}tt|d D ]P}|t|| d ||d  d  d || d ||d  d  d  7 }qtt|d D ]P}|t|| d ||d  d  d || d ||d  d  d  7 }qft|}|S )Nr   r   g?center)ZxfactZyfactoriginr   rc   r   )Zweightr<   r:   )nxZGraphZadd_nodegpdZ	GeoSeriesr   scalerT   rU   ZexteriorZcoordsfliprR   rS   r*   rZ   add_edgenodesedgesr   Zremove_edges_fromZdijkstra_path)r3   r4   	obstaclesGobstacleZgeopandas_obstacleZscaled_obstacle_pointsrW   jrY   Zedges_to_removeZedger   Zfirst_tree_idZlast_tree_idr7   r8   Zshortest_pathZfirst_pointZsecond_pointZsecond_shortest_pathZfirst_path_distanceZsecond_path_distance)r]   r_   r   r|   r"   r%   r   r   avoid_obstacles:  sr    
>>$


.0@
(
&2&NN
z*path_calculations.<locals>.avoid_obstaclesc                    s8   |d d }d d df |k d } | ||}|S )Nr   r:   r   )r^   r   Zclean_path_rowr   r\   Zpoints_to_clean_path_row)r   closest_pointsr   r   obstacle_avoidance_path  s    z2path_calculations.<locals>.obstacle_avoidance_pathc                    s  t | }|jd dkrX|d d d }||d }tt|D ]J}|| d }|| d }t || dd|ddgg}t j|||dd}qFt j|dd}	tt|	D ]R}|	| d }|	| d }t || dd|ddgg}t j|t|d |dd}q|d }
|d }d}d}|
|||ddddg}t j|d|dd}t j|t||dd}|S tt|D ]h}|| d d }||| }tt|D ]T}|| d }|| d }t || dd|ddgg}t j|| ||dd||< qt j|dd}	tt|	D ]\}|	| d }|	| d }t || dd|ddgg}t j|| t|| |dd||< q|d }
|d }d}d}|
|||ddddg}t j|| d|dd||< t j|| t|| |dd||< qd|S )Nr   r   r:   r   )rT   copyshaperR   rS   rU   insertr   )tree_row_pathr^   
clean_pathr   Zstart_of_pathr   r!   r    rA   Zreverse_start_of_pathstarting_point_xstarting_point_ystarting_point_altitudestarting_point_anglerW   )rJ   r   r   r   r   #final_paths_with_obstacle_avoidance  sZ    
&&z>path_calculations.<locals>.final_paths_with_obstacle_avoidance
   c              	   S   s&  t | }|jd dkr|d }|d }d}d}||||ddddg}t j|d|dd}|}||||ddddg}|d d d }	|d d d }
|	|
|dddddg}t j|d|dd}t j|t||dd}t j|t||dd}t j|t||dd}|S tt|D ]}|d }|d }d}d}||||ddddg}t j|| d|dd||< |}||||ddddg}|| d d }	|| d d }
|	|
|dddddg}t j|| d|dd||< t j|| t|| |dd||< t j|| t|| |dd||< t j|| t|| |dd||< q|S )Nr   r   r   )rT   r   r   r   rS   rR   )r   r^   Zobstacle_avoidance_altitudeclean_path_altituder   r   r   r   rA   Zcorner_point_xZcorner_point_yrn   rW   r   r   r   &final_paths_with_no_obstacle_avoidance  sF    
""&zApath_calculations.<locals>.final_paths_with_no_obstacle_avoidancer   c                 S   s   d}d}d}t t| d D ]}t| | d | |d  d  d | | d | |d  d  d  }|d }| | d | |d  d  }	||7 }|t|	7 }|	dkr|	| }
nt|	| }
||
7 }q||| 7 }|S )Nr   r   r   r   )rR   rS   r*   rZ   abs)Zflight_pathspeedascent_speeddescent_speedtotal_distance
total_timeZtotal_altitude_distancerW   rY   distance_altitudetimer   r   r   calculate_flight_time_altitude  s    F

z9path_calculations.<locals>.calculate_flight_time_altitudec                    sX   g }t t| D ]B} | | } || }||k rD|| |  q|||  q|S r$   )rR   rS   r   )r   r   
final_pathrW   r   Ztime_altitude)r   r   r   compare_final_paths_by_time  s    z6path_calculations.<locals>.compare_final_paths_by_time)r   r;   rb   T)F)r   )r   r   r   )r
   objectsfilterorder_by	Exceptionr   getDoesNotExistr	   r   r   polygonrT   rU   r   r   r    r!   )r   rJ   rK   obstacle_avoidancerv   ZDroneStationr   ZobstaclesDataBaser   ZobstacleGEOJSONZobstacleNoGEOJSONr#   r&   r9   rX   rw   r   r   r   r   rW   Ztree_row_pathsZpaths_with_obstacle_avoidanceZ paths_with_no_obstacle_avoidanceZfinal_pathsr   )r   r   r2   r   r]   rP   rM   rN   rp   rs   rt   r`   r_   r   rJ   r6   r|   rO   rK   r   r   rL   r^   r"   rV   r%   r   r   path_calculations   s~    

"^6
1







r   )r   r   TF)Zmatplotlib.pyplotZpyplotZpltZnumpyrT   r*   Zshapely.geometryr   r   r   Z	geopandasr   Znetworkxr   ZgeopyZgeopy.distancer   gardens.modelsr   r   r   r	   r
   r   r   r   r   r   r   r   <module>   s   $