pp4duav.py

import time
import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as plt3d
from queue import PriorityQueue
from pandas import DataFrame, ExcelWriter, read_excel, read_csv, concat
from typing import Iterable, Literal, Generator, overload, final
from pathlib import Path
from argparse import ArgumentParser

__all__ = 'ID', 'PathPlanner', 'TaskAssigner'

class ID:
    
    __slots__ = (
        'src', 'dst', 'mode', 'content', 'priority', 'type', 'weight', 'mode_abbr', 
        'mode_resolve', 'time', 'consumption', 'turn_sum', 'height_variance'
    )
    
    src: tuple[int] | int | str
    dst: tuple[int] | int | str
    mode: str
    content: str
    priority: str
    type: tuple[str]
    weight : int | float
    mode_abbr: dict[str, str]
    mode_resolve: dict[str, str]
    
    time: int | float
    consumption: int | float
    turn_sum: float
    height_variance: float
    
    def __new__(
        cls, 
        src: tuple[int] | int | str, 
        dst: tuple[int] | int | str, 
        type: str | tuple[str] = ('empty', 'normal'), 
        weight: int | float = 0, 
        mode: Literal['efficiency', 'economy', 'balanced', 'default'] = 'default'):
        
        self = object.__new__(cls)
        self.mode_abbr = {'efficiency': 'E', 'economy': 'C', 'balanced': 'B', 'default': '0'}
        self.mode_resolve = {'E': 'efficiency', 'C': 'economy', 'B': 'balanced', '0': 'default'}
        
        self.src = src[:2] if isinstance(src, tuple) else src
        self.dst = dst[:2] if isinstance(dst, tuple) else dst
        self.mode = mode if mode in self.mode_abbr else self.mode_resolve.get(mode, 'default')
        self.content = type[0]  # empty, regular, valueable, special
        self.priority = type[1] # normal, express, urgent
        self.type = (self.content.upper()[0], self.priority.upper()[0])
        self.weight = weight
        self.time = self.consumption = self.turn_sum = self.height_variance = 0
        return self
    
    @classmethod
    def from_str(cls, __str: str):
        """
        Decode string id generated by PathPlanner, retrun cls
        """
        content = {'E': 'empty', 'R': 'regular', 'V': 'valueable', 'S': 'special'}
        priority = {'N': 'normal', 'E': 'express', 'U': 'urgent'}
        tw, src, dst, mode = __str.split('-')
        args = (content[tw[0]], priority[tw[1]]), int(tw[2:]) / 100, mode
        if len(src) == 6 and len(dst) == 6:
            src, dst = (int(src[:3]), int(src[3:])), (int(dst[:3]), int(dst[3:]))
        else:
            src = int(src) if src.isnumeric() else src
            dst = int(dst) if dst.isnumeric() else dst
        return cls.__new__(ID, src, dst, *args)
    
    @classmethod
    def reverse(cls, __id):
        return cls.__new__(ID, __id.dst, __id.src, __id.t, __id.weight, __id.mode)
    
    @property
    def t(self):
        return self.content, self.priority
    
    def merge_data(self, *args):
        '''merge flight data: `turn_sum`, `height_variance`, `time`, `consumption`'''
        self.turn_sum, self.height_variance, self.time, self.consumption = args
        return self
    
    def __str__(self) -> str:
        s = "{:03}{:03}".format(*self.src) if isinstance(self.src, tuple) else self.src
        d = "{:03}{:03}".format(*self.dst) if isinstance(self.dst, tuple) else self.dst
        return "{}{}{:04d}-{}-{}-{}".format(
            *self.type, round(self.weight*100), s, d, self.mode_abbr[self.mode])
    
    def __repr__(self):
        return f"{self.__class__.__module__}.{self.__class__.__qualname__}({self})"
    
    def __eq__(self, other):
        return str(other) == self.__str__() \
            if isinstance(other, ID) else str(other) == self.__str__()
    
    def __lt__(self, other):
        return str(other)[:-2] == self.__str__()[:-2] if isinstance(other, ID) \
            else str(other)[:-2] == self.__str__()[:-2]
    
    def __hash__(self):
        return hash(repr(self).encode())
    
class PathPlanner(object):
    '''
    Parameters
    ----------
    environment: `DataFrame`, original DEM map data
    danger_area: `DataFrame`, a mapping of DEM map defining danger area with a ratio
    resolution: `int`, shows the accuracy of the map
    pos_map: `DataFrame`, for src and dst abbreviation lookup
    session: `str`, session name for data saving or locating
    '''
    def __init__(
        self, 
        environment: DataFrame, 
        danger_area: DataFrame | None = None,
        pos_map: DataFrame | None = None, 
        resolution: int = 5, 
        session: str = time.strftime(r'%m%d%H%M%S')):
        
        self.res = resolution
        self.env = environment
        self.da = {} if danger_area is None else \
            dict(((int(x), int(y)), da) for x, y, da in danger_area.values)
        self.session = session
        self.mode_refer = 'efficiency', 'economy', 'balanced', 'default'
        # pos_map = read_csv('TD.csv', index_col=0, header=0).fillna(0).astype('int')
        self.pos_map = {}
        if pos_map is not None:
            t, d = [], []
            for idx in pos_map.index:
                (d if str(idx).isnumeric() else t).append(idx)
            t = pos_map.loc[t, ['x', 'y']]
            d = pos_map.loc[d]
            self.terminal = dict((k, (x, y)) for k, x, y in t.reset_index().values)
            self.distributor = dict((int(k), (x, y)) for k, x, y, _, _ in d.reset_index().values)
            self.t2d = dict((k, tuple(int(v) for v in v.index)) for k, v in zip(t.index, \
                [d.loc[np.any([d.iloc[:,2] == x+1, d.iloc[:,3] == x+1], axis=0)] for x in range(len(t))]))
            self.d2t = dict((int(kv[0]), str().join([chr(64 + v) if isinstance(v, int) else v if \
                isinstance(v, str) else '' for v in kv[1:]]) ) for kv in d.iloc[:,2:].reset_index().values)
            self.pos_map.update(self.terminal)
            self.pos_map.update(self.distributor)
        
        self.__partition = {}
        self.__children = {}
        self.maxx, self.maxy = self.env.shape
        
        # flight dist, cargo weight, danger ratio
        gw0 = lambda w: 1 if w < 0.5 else (w - 0.5) ** 2 + 1
        gw1 = lambda w: 0.1 if w < 0.125 else 0.8 * w if w < 1.5 else (w - 1.1) ** 2 + 1.04
        self.g_kwargs = {
            'efficiency': {'ratio': (lambda w: 1.6 / gw0(w), lambda w: 1.4 * gw1(w) + 1, 3)}, 
            'economy':    {'ratio': (lambda w: 1.2 / gw0(w), lambda w: 1.8 * gw1(w) + 1, 5)}, 
            'balanced':   {'ratio': (lambda w: 1.5 / gw0(w), lambda w: 1.5 * gw1(w) + 1, 4)}}
        # horizontal ratio, vertical ratio
        self.h_kwargs = {
            'efficiency': {'r1': 6, 'r2': 12}, 
            'economy':    {'r1': 4, 'r2': 16}, 
            'balanced':   {'r1': 5, 'r2': 14}}
        self.cost_ratio = {'efficiency': 0.5, 'economy': 0.7,'balanced': 0.6, 'default': 0.5}
        
        self.path_analysis_refer = (
            'id', 'srcX', 'srcY', 'dstX', 'dstY', 'time_start', 'time_elapse', 'program_loop', 
            'program_closed', 'cost_adj', 'cost_ratio', 'path_length', 'path_nodes', 'flight_turn_sum', 
            'flight_height_variance', 'flight_time', 'flight_consumption')
        self.path_nodes_refer = ('x','y','z','dist')
        self.returns = {-2: 'skipped', -1: 'unreach', 0: 'completed', 1: 'toplimit', 2: 'timeout'}
        self.path_analysis = DataFrame(columns=self.path_analysis_refer[1:])
        self.path_nodes = {}
        self.success = []
        self.failed = set()
        
        '''lambda function predef'''
        self.mid_pos = lambda x, y: (round((x[0] + y[0]) / 2), round((x[1] + y[1]) / 2), round((x[2] + y[2]) / 2))
        self.not_near = lambda x, y, z_step: \
            abs(x[0] - y[0]) > 1 or abs(x[1] - y[1]) > 1 or abs(x[2] - y[2]) > z_step
        
    def __len__(self):
        return len(self.path_analysis)
    
    def save(self, save: Path | str = Path(), only_new: bool = False):
        file = Path(save) / Path(f'data_{self.session}.xlsx') if Path(save).is_dir() else save
        with ExcelWriter(file, engine='openpyxl', mode='w') as file:
            output = self.path_analysis.reset_index()
            if only_new:
                index = output['id'].isin(self.success)
                output.loc[index].to_excel(file, 'Summary', index=False)
                for k in self.success:
                    self.path_nodes[k].to_excel(file, str(k), index=False)
            else:
                output.to_excel(file, 'Summary', index=False)
                for k, v in self.path_nodes.items():
                    v.to_excel(file, str(k), index=False)
    
    def load(self, load: Path | str = None, as_new: bool = False):
        data = read_excel(Path(load if load else f'data_{self.session}.xlsx'), sheet_name=None)
        data['Summary'].set_index('id', inplace=True)
        data['Summary'].index = data['Summary'].index.map(ID.from_str)
        if as_new:
            self.success.extend(data['Summary'].index)
        self.path_analysis = concat([data['Summary'], self.path_analysis])
        self.path_nodes.update((k, data[str(k)]) for k in data['Summary'].index)
    
    @overload
    def plan(
        self, 
        src: tuple[int], 
        dst: tuple[int], 
        *, 
        init_alt: int | float = 10, 
        max_alt: int | float = 100, 
        min_alt: int | float = 5, 
        z_step: int = 2, 
        danger_threshold: float | None = 0.35, 
        loop_limit: int = 0, 
        time_limit: float = 0, 
        skip_planned: bool = True, 
        reverse: bool = False, 
        visualize: bool = True, 
        view_pos: bool = True, 
        show_stat: bool = True, 
        mode: Literal['efficiency', 'economy', 'balanced', 'default'] = 'default', 
        **kwargs) -> tuple[int, ID]: 
        """
        Path Planner main function
        -----
        Input plan parameters by src and dst position like (x,y,z) or (x,y) in `tuple[int]`
        """
        ...
    
    @overload
    def plan(
        self, 
        id: str | ID, 
        *, 
        init_alt: int | float = 10, 
        max_alt: int | float = 100, 
        min_alt: int | float = 5, 
        z_step: int = 2, 
        danger_threshold: float | None = 0.35, 
        loop_limit: int = 0, 
        time_limit: float = 60, 
        skip_planned: bool = True, 
        reverse: bool = False, 
        visualize: bool = False, 
        view_pos: bool = False, 
        show_stat: bool = False, 
        **kwargs) -> tuple[int, ID]:
        """
        Path Planner main function
        -----
        Input plan parameters by id in `str` or `ID`: 
        cargo type, weight  - source  - destination - plannning mode
        (`2 caps` `4d in 10g`-`3d`-`3d`-`1d`)
        
        Here's an example id of a regular, normal cargo weighing 940g 
        from (2, 12) to (35, 95), path planned in default mode (0):
        >>> example_id = r'RN0094-002012-035095-0'
        >>> ID.from_str(example_id)
        pp4duav.ID(RN0094-002012-035095-0)
        
        type, weight, mode will be overwritten by kwargs if given.
        """
        ...
    
    @final
    def plan(self, *args, **kwargs) -> tuple[int, ID]:
        
        #############################
        # coordinate, env, id check #
        #############################
        
        init_alt = kwargs.setdefault('init_alt', 10)
        max_alt = kwargs.pop('max_alt', 100)
        min_alt = kwargs.pop('min_alt', 5)
        z_step = kwargs.pop('z_step', 2)
        dt = kwargs.pop('danger_threshold', 0.35)
        loop_limit = kwargs.pop('loop_limit', 0)
        
        if len(args) == 1 or kwargs.get('id'):
            id = kwargs.pop('id') if kwargs.get('id') else args[0]
            if isinstance(id, str):
                id = ID.from_str(id)
            elif not isinstance(id, ID):
                raise TypeError
            # skip planned path
            src, dst = id.src, id.dst
            if 'mode' in kwargs:
                id.mode = kwargs.pop('mode')
            if 'type' in kwargs:
                id.content, id.priority = kwargs.pop('type')[:2]
                id.type = (id.content.upper()[0], id.priority.upper()[0])
            if 'weight' in kwargs:
                id.weight = kwargs.pop('weight')
            
            time_limit = kwargs.pop('time_limit', 300)
            visualize = kwargs.pop('visualize', False)
            view_pos = kwargs.pop('view_pos', False)
            show_stat = kwargs.pop('show_stat', False)
        
        elif len(args) == 2 or (kwargs.get('src') and kwargs.get('dst')):
            src, dst = args if len(args) == 2 else (kwargs.pop('src'), kwargs.pop('dst'))
            mode = kwargs.pop('mode', 'default')
            type = kwargs.pop('type', ('regular','normal') if 'weight' in kwargs else ('empty','normal'))
            weight = kwargs.pop('weight', 0)
            id = ID(src, dst, type, weight, mode)
            # skip planned path
            
            time_limit = kwargs.pop('time_limit', 0)
            visualize = kwargs.pop('visualize', True)
            view_pos = kwargs.pop('view_pos', True)
            show_stat = kwargs.pop('show_stat', True)
        
        else:
            raise ValueError('init parameter error')
        
        reverse = kwargs.pop('reverse', False)
        if kwargs.pop('skip_planned', True):
            if id in self.path_nodes:
                return 0, id.merge_data(*self.path_analysis.loc[id].iloc[-4:])
            elif kwargs.pop('get', False):
                if not isinstance(src, tuple):
                    try:
                        src = self.pos_map[src]
                    except KeyError:
                        ValueError("src pos invalid")
                if not isinstance(dst, tuple):
                    try:
                        dst = self.pos_map[dst]
                    except KeyError:
                        ValueError("dst pos invalid")
                group = (src[0], src[1], dst[0], dst[1]), (dst[0], dst[1], src[0], src[1])
                for p in range(2):
                    try:
                        match_id = self.path_analysis.groupby(
                            ['srcX', 'srcY', 'dstX', 'dstY']).get_group(group[p])
                        match_id = match_id.loc[match_id.index.map(lambda x: x.mode) == id.mode]
                        result = self.path_nodes[(match_id.index.to_series().map(
                            lambda x: x.weight) - id.weight).abs().idxmin()].to_numpy()
                        if p:
                            result = result[::-1]
                            t = result[-1,3]
                            for p in range(len(result)):
                                result[p,3], t = t, result[p,3]
                        flag = 0
                        match_id = True
                        break
                    except KeyError:
                        flag = -2
                        match_id = False
                
                if flag:
                    self.failed.add(id.reverse(id) if reverse else id)
                else:
                    flight_data = self.flight_analysis(result, id, **kwargs)
                    id.merge_data(*flight_data)
                    if reverse:
                        src, dst = dst, src
                    self.path_analysis.loc[id] = [
                        *src[:2], *dst[:2], 0, 0, 0, 0, 0, 0, result[:,3].sum(), len(result), *flight_data]
                    self.path_nodes[id] = DataFrame(result, columns=self.path_nodes_refer)
                    self.success.append(id)
                return flag, id
            
            if reverse:
                if id.reverse(id) in self.failed:
                    return -2, id
            elif id in self.failed:
                return -2, id
        
        if id.type[0] == 'E' or id.weight == 0:
            if not (id.type[0] == 'E' and id.weight == 0):
                raise ValueError('empty cargo weight conflict')
        if not isinstance(src, tuple):
            try:
                src = self.pos_map[src]
            except KeyError:
                ValueError("src pos invalid")
        if len(src) == 2:
            src = (*src, round(self.env.iloc[src[0], src[1]]+init_alt))
        elif len(src) == 3:
            src = (*src[:2], max(round(self.env.iloc[src[0], src[1]]+init_alt), src[2]))
        else:
            raise ValueError("src is not an (x, y) or (x, y, z) pos")
        
        if not isinstance(dst, tuple):
            try:
                dst = self.pos_map[dst]
            except KeyError:
                ValueError("dst pos invalid")
        if len(dst) == 2:
            p = round(self.env.iloc[dst[0], dst[1]]+init_alt)
        elif len(dst) == 3:
            p = max(round(self.env.iloc[dst[0], dst[1]]+init_alt), dst[2])
        else:
            raise ValueError("dst is not an (x, y) or (x, y, z) pos")
        dst = (*dst[:2], p + (p - src[2]) % z_step)
        if reverse:
            src, dst = dst, src
        
        ###################
        # init parameters #
        ###################
        
        skw = {'min_alt': min_alt, 'max_alt': max_alt, 'z_step': z_step}
        gkw = self.g_kwargs.get(id.mode, {'default': True}).copy()
        hkw = self.h_kwargs.get(id.mode, {'default': True}).copy()
        dh = gkw['dh'] = hkw['dh'] = self.dist2d(src, dst, self.res)
        if not (gkw.get('default') and hkw.get('default')):
            gkw['rf'] = gkw['ratio'][0](id.weight)
            gkw['rw'] = gkw['ratio'][1](id.weight)
            gkw['rd'] = gkw['ratio'][2]
        self.__partition = {0: [1], 1: (1, 0), 'pos': [src]}
        self.__partition['k'] = np.inf if (dst[0]-src[0])==0 else (dst[1]-src[1])/(dst[0]-src[0])
        
        time_limit = float(time_limit) if time_limit > 0 else np.inf
        loop_limit = int(loop_limit) if loop_limit > 0 else np.inf
        g_ratio = cost_ratio = kwargs.pop('cost_ratio', self.cost_ratio[id.mode])
        cost_adj = kwargs.pop('cost_adj', 0.05)
        if g_ratio >= 1 or g_ratio <= 0:
            raise ValueError('valid cost ratio range is (0, 1), recommend [0.5, 0.6]')
        h_ratio = 1 - g_ratio
        
        openlist = PriorityQueue()
        closed = {src: (np.inf, np.inf)}  # init close list (dict to store g): {pos: g(n))}
        h_value = self.h(src, src, src, dst, **hkw)
        openlist.put((h_ratio * h_value, 0, h_value, src))  # init open list: (f(n), g(n), h(n), pos)
        g = {src: 0}   #init min g(n) dict
        path = {src: src}   # dict for pos to find its parent of lowest cost
        time_s = time_e = time.time()   # timer for efficiency analysis
        count = g_adj = 0    # process loop and cost adj counter
        
        ###############
        ## main func ##
        ###############
        
        while count < loop_limit:
            count += 1
            try:
                _, g_value, h_value, pos = openlist.get(timeout=0.01)
            except:
                time_e = time.time()
                flag = -1
                break
            
            '''break conditions, restrictions check every 500 loop'''
            if self.__partition[0][0] < 0.2:
                dr, _da = self.sight(pos, dst, **skw)
                if dr <= dt and 1 not in _da:
                    path[dst] = path[pos] if pos == dst else pos
                    time_e = time.time()
                    flag = 0
                    break   # reach destination
            
            if count % 500 == 0:
                time_e = time.time()
                t = time_e - time_s
                p = self.__partition[0][0]
                if show_stat:
                    print("\r{4} {0:03.0f}% |{1}{2}|  Elapsed {3:.0f}s".format(
                        (1-p)*100, str().join(['>' for _ in range(10-round(p*10))]), 
                        str().join([' ' for _ in range(round(p*10))]), t, id), end='   ')
                if t >= time_limit:
                    flag = 2
                    break   # timeout
                
                '''dynamic cost ratio adjust'''
                while cost_adj and t > self.__partition[1][1] + 60 * cost_ratio:
                    p = self.__partition[1][0] - self.__partition[0][0]
                    if p <= 0.01:
                        if g_ratio > 0.45:
                            g_ratio -= cost_adj
                            h_ratio += cost_adj
                            g_adj += 1
                            time_limit += 60 * cost_ratio
                    elif g_ratio != cost_ratio and p > cost_adj:
                        g_ratio = cost_ratio
                        h_ratio = 1 - g_ratio
                    else:
                        self.__partition[1] = (p, int(t))
                        self.__partition[0] = self.__partition[0][:1]
                        self.__partition['pos'] = self.__partition['pos'][:1]
                        break
                    newlist = PriorityQueue()
                    while True:
                        try:
                            _, _g, _h, child = openlist.get(timeout=0.01)
                            newlist.put((g_ratio * _g + h_ratio * _h, _g, _h, child))
                        except:
                            openlist = newlist
                            break
                    self.__partition[1] = (self.__partition[1][0], int(t))
                    self.__partition[0] = self.__partition[0][:1]
                    while self.__partition['pos']:
                        child = self.__partition['pos'].pop()
                        _g, _h = closed.get(child)
                        openlist.put((g_ratio * _g + h_ratio * _h, _g, _h, child))
                    self.__partition['pos'].append(child)
                    break
            
            '''g check and add to closed list'''
            if closed.get(pos, (np.inf, 0))[0] <= g_value:
                continue    # searched pos in open or closed list
            closed[pos] = (g_value, h_value)
            
            '''get child nodes'''
            children = self.children(pos, **skw)
            if children[0] > dt or children[1] == 1:
                continue
            
            '''theta-star sight check'''
            dr, da = self.sight(path[pos], pos, **skw)
            if dr > dt or 1 in da:
                parents = PriorityQueue()   # sight blocked (collison or restricted area)
                
                # check sight between pos and parent's parent
                parent, depth = path[path[pos]], 0
                while parent != src and depth <= 2:
                    dr, da = self.sight(parent, pos, **skw)
                    depth += 1
                    if dr <= dt and 1 not in da and self.not_near(parent, pos, z_step):
                        parents.put((closed[parent][0] + \
                            self.g(src, parent, pos, dst, da, path[parent], **gkw, **skw), parent, da))
                    else:
                        parent = path[parent]
                
                # check sight between pos and parent path mid if not near
                mid = self.mid_pos(path[path[pos]], path[pos])
                dr, da = self.sight(mid, pos, **skw)
                if dr <= dt and 1 not in da and self.not_near(mid, pos, z_step) \
                    and self.not_near(mid, path[pos], z_step) and self.not_near(mid, path[path[pos]], z_step):
                    parent = path.setdefault(mid, path[path[pos]])
                    if mid in closed:
                        g_value = closed[mid][0]
                    else:
                        g_value = closed[parent][0] + self.g(src, parent, mid, dst, \
                            self.sight(parent, mid, **skw)[1], path[parent], **gkw, **skw)
                        closed[mid] = (g_value, self.h(src, path[mid], mid, dst, **hkw))
                    parents.put((g_value + self.g(src, mid, pos, dst, da, path[mid], **gkw, **skw), mid, da))
                else:
                    da = {children[1]: {pos}} if children[1] else dict()
                    for child in children[2]:   # Priority Queue of (g(n), pos)
                        if child in closed:     # search closed nearby min g(n) pos as new parent
                            parents.put((closed[child][0] + \
                                self.g(src, child, pos, dst, da, path[child], **gkw, **skw), child, da))
                
                g_value, parent, da = parents.get(timeout=0.01) # min g(n) pos
                try:
                    while True:
                        p = parent
                        while p != src:
                            if p != pos:
                                p = path[p]
                            else:
                                break
                        else:
                            break
                        g_value, parent, da = parents.get(timeout=0.01)
                except:
                    continue
                g[pos] = g_value
                closed[pos] = (g_value, self.h(src, path[pos], pos, dst, **hkw))
                path[pos] = parent
            else:
                parent = path[pos]  # parent remain the same if sighted
            
            '''search progress'''
            p = sum(self.partition(src, path[pos], pos, dst)) / dh
            if p < self.__partition[0][0]:
                self.__partition[0].insert(0, p)
                if pos in self.__partition['pos']:
                    self.__partition['pos'].remove(pos)
                self.__partition['pos'].insert(0, pos)
            else:
                self.__partition[0].append(p)
                if pos not in self.__partition['pos']:
                    self.__partition['pos'].append(pos)
            
            '''child nodes search'''
            rt = self.dist(path[parent], parent, self.res)
            for child in children[2]:
                arg = src, parent, child, dst
                g_value = closed[parent][0] + self.g(*arg, da, path[parent], rt = rt, **gkw, **skw)
                if g.get(child, np.inf) < g_value:
                    continue
                g[child] = g_value
                h_value = self.h(*arg, **hkw)
                openlist.put((g_ratio * g_value + h_ratio * h_value, g_value, h_value, child))
                p = parent
                while p != src:
                    if child == p:
                        break
                    p = path[p]
                else:
                    path[child] = parent
        else:
            flag = 1    # search loop limit
        
        #############
        ## results ##
        #############
        
        match_id = kwargs.get('match_id')
        # path found, retrace
        if flag == 0:   
            result = [(*dst, self.dist(pos, dst, self.res))]
            while pos != src:
                dist = self.dist(path[pos], pos, self.res)
                result.append((*pos, dist))
                pos = path[pos]
            result.append((*pos, 0))
            result = np.array(result)
            if reverse:
                t = result[-1,3]
                for p in range(len(result)):
                    result[p,3], t = t, result[p,3]
            else:
                result = result[::-1]
        # match another planned id & path as if succeed
        elif match_id:
            group = (src[0], src[1], dst[0], dst[1]), (dst[0], dst[1], src[0], src[1])
            for p in range(2):
                try:
                    match_id = self.path_analysis.groupby(
                        ['srcX', 'srcY', 'dstX', 'dstY']).get_group(group[p])
                    match_id = match_id.loc[match_id.index.map(lambda x: x.mode) == id.mode]
                    result = self.path_nodes[(match_id.index.to_series().map(
                        lambda x: x.weight) - id.weight).abs().idxmin()].to_numpy()
                    if p:
                        result = result[::-1]
                        t = result[-1,3]
                        for p in range(len(result)):
                            result[p,3], t = t, result[p,3]
                    flag = 0
                    match_id = True
                    break
                except KeyError:
                    match_id = False
        
        if show_stat:
            print("\r>> {} {}, plan time {:.02f}s {}".format(
                id, 'matched' if match_id else self.returns[flag], time_e-time_s, 
                f"cost adj * {g_adj}" if g_adj else "      "))
        
        if flag:
            self.failed.add(id.reverse(id) if reverse else id)
        # analysis, stat & visualize
        else:
            flight_data = self.flight_analysis(result, id, **kwargs)
            id.merge_data(*flight_data)
            
            # id srcX srcY dstX dstY time_start time_elapse program_loop program_closed 
            # cost_adj cost_ratio path_length path_nodes flight_turn_sum 
            # flight_height_variance flight_time flight_consumption
            
            if reverse:
                src, dst = dst, src
            stat = [
                *src[:2], *dst[:2], time.ctime(time_s)[11:19], time_e-time_s, count, 
                len(closed), g_adj, cost_ratio, result[:,3].sum(), len(result), *flight_data]
            self.path_analysis.loc[id] = stat
            self.path_nodes[id] = DataFrame(result, columns=self.path_nodes_refer)
            self.success.append(id)
            
            if visualize:
                self.visualize(result, np.array(list(closed.keys())) if view_pos else [])
        
        return flag, id
    
    def flight_analysis(self, path: np.array, id: ID, **kwargs):
        '''
        kwargs
        ------
        speed_limit: `float`, max cruise speed in m/s
        avg_climb: `float`, average climb in m/s
        avg_descent: `float`, average descent in m/s
        avg_acc: `float`, average accelerate in m/s^2
        avg_dec: `float`, average decelerate in m/s^2
        power_v: `int`, vertical power in J/m
        power_h: `int`, horizontal power in J/m
        
        returns
        -------
        flight_turn_sum: `float`, sum of turn rad
        flight_height_variance: `float`, standard deviation of path height (z)
        flight_time: `float`, time elapsed during flight including airborn and landing
        flight_consumption: `float`, power consumed during flight
        '''
        
        '''flight route preprocess'''
        turn = [np.pi/2]
        slope = [np.pi/2]
        for i in range(1, len(path)-1):
            a = (path[i] - path[i-1])[:3]
            b = (path[i+1] - path[i])[:3]
            t = np.dot(a[:2], b[:2]) / (np.linalg.norm(a[:2]) * np.linalg.norm(b[:2])) \
                if a[:2].any() and b[:2].any() else 1
            turn.append(round(np.arccos(t if t < 1 else 1), 4))
            slope.append(np.arctan2(b[2]-a[2], np.sqrt((b[0]-a[0])**2 + (b[1]-a[1])**2)))
        turn.append(np.pi/2)
        slope.append(np.pi/2)
        
        '''mode and type modificator'''
        # type[0]: empty, regular, valueable, special
        # type[1]: normal, express, urgent
        m1 = {'E': 1.5, 'R': 1, 'V': 0.9, 'S': 0.8}
        m2 = {'N': -1, 'E': 0, 'U': 2}
        a_ratio, v_offset = m1[id.type[0]], m2[id.type[1]]
        
        
        '''initialize parameters'''
        max_speed = kwargs.get('speed_limit', 10) + v_offset
        acc = kwargs.get('avg_acc', 5 if id.weight <= 0.5 else (5 - (0.4 * (id.weight - 0.5)) ** 2)) * a_ratio
        dec = kwargs.get('avg_dec', - 1.1 * acc) * a_ratio
        climb = kwargs.get('avg_climb', round(3 + np.sqrt(1 / (id.weight + 0.5)), 4)) + v_offset
        descent = kwargs.get('avg_descent', - climb - 0.5) + v_offset
        power_v = kwargs.get('power_v', lambda v: (- 18 + 2 * id.weight) * v + 370 + 35 * id.weight)
        power_h = kwargs.get('power_h', lambda v: (- 8.8 + 2 * id.weight) * v + 156 + 2 * id.weight)
        
        '''time and consumption caculation'''
        v_turn = lambda angle: np.sqrt(9.80665 * kwargs.get('turn_radius', self.res) / np.tan(angle)) \
            if -np.pi / 2 < angle < np.pi / 2 else 0
        
        v, d = [0], []
        for idx in range(1, len(path)):
            d.append((path[idx - 1, 3] * abs(np.cos(slope[idx])), path[idx, 2] - path[idx - 1, 2]))
            vt = round(v_turn(turn[idx]) / (id.weight - 1 if id.weight > 2 else 1), 6) if turn[idx] else max_speed
            v.append(max_speed if vt > max_speed else vt)
        d.append((path[-1, 3] * abs(np.cos(slope[-2])), path[-2, 3] - path[-1, 3]))
        v.append(0)
        
        stat = {'acc': [], 'con': [], 'dec': [], 'total': [], 'consume': []}
        idx = 1
        while idx < len(v):
            flag = True
            dist, height = d[idx-1]
            v1, v0 = v[idx], v[idx-1]
            dv = v1 - v0
            
            #vertical only
            if dist < self.res:
                ta = tc = td = va = vd = 0
            # same speed
            elif dv == 0:
                va = vd = (max_speed + v1) / 2
                ddv = max_speed - v1
                ta = ddv / acc
                td = - ddv / dec
                dd = (ta + td) * (ddv / 2 + v1)
                if dd <= dist:
                    tc = (dist - dd) / max_speed
                else:
                    tc = 0
                    ddv = np.sqrt(v1**2+2*dist/(1/acc+1/-dec))
                    va = vd = (ddv + v1) / 2
                    ddv -= v1
                    ta = ddv / acc
                    td = ddv / dec
            # acc
            elif dv > 0:
                vf = 2*acc*dist+v0**2
                vf = round(np.sqrt(vf if vf > 0 else 0), 6)
                vd = 0
                if vf > v1:
                    va = (max_speed + v0) / 2
                    if v1 == max_speed:
                        ta = (max_speed - v0) / acc
                        tc = (dist - (max_speed**2 - v0**2) / 2 / acc) / max_speed
                        td = 0
                    else:
                        ta = (max_speed - v0) / acc
                        td = (v1 - max_speed) / dec
                        dd = v0 * ta + 0.5 * acc * ta**2 + v1 * td + 0.5 * dec * td**2
                        if dd <= dist:
                            vd = (v1 + max_speed) / 2
                            tc = (dist - dd) / max_speed
                        else:
                            tc = 0
                            vf = round(np.sqrt((2*acc*dec*dist-acc*v1**2+dec*v0**2)/(dec-acc)), 6)
                            ta = vf - v0 / acc
                            td = v1 - vf / dec
                            va = (vf + v0) / 2
                            vd = (v1 + vf) / 2
                elif vf == v1:
                    va = (vf + v0) / 2
                    ta = vf - v0 / acc
                    td = tc = 0
                else:
                    va = (vf + v0) / 2
                    ta = vf - v0 / acc
                    td = tc = 0
                    v[idx] = vf
            # dec
            else:
                vf = 2*dec*dist+v0**2
                vf = round(np.sqrt(vf if vf > 0 else 0), 6)
                if vf < v1:
                    ta = (max_speed - v0) / acc
                    td = (v1 - max_speed) / dec
                    dd = v0 * ta + 0.5 * acc * ta**2 + v1 * td + 0.5 * dec * td**2
                    if dd <= dist:
                        va = (v0 + max_speed) / 2
                        vd = (max_speed + v1) / 2
                        tc = (dist - dd) / max_speed
                    else:
                        tc = 0
                        vf = round(np.sqrt((2*acc*dec*dist-acc*v1**2+dec*v0**2)/(dec-acc)), 6)
                        ta = vf - v0 / acc
                        td = v1 - vf / dec
                        va = (v0 + vf) / 2
                        vd = (vf + v1) / 2
                elif abs(vf - v1) <= 0.01:
                    td = v0 - vf / dec
                    vd = (vf + v0) / 2
                    ta = tc = va = 0
                else:
                    v[idx-1] = round(np.sqrt(v1**2-2*dec*dist), 6)
                    flag = False
            
            if height > 0:
                _td = 0
                _tc = height / climb
            elif height < 0:
                _tc = 0
                _td = height / descent
            else:
                _tc = _td = 0
            
            if flag:
                stat['acc'].append((ta, va))
                stat['con'].append((tc, max_speed))
                stat['dec'].append((td, vd))
                stat['total'].append(max([ta + tc + td, _tc, _td]))
                stat['consume'].append(-height*power_v(-descent) if _td else height*power_v(climb) if _tc else 0)
                idx += 1
            else:
                for k in stat.keys():
                    stat[k].pop()
                idx -= 1
        
        for k in ['acc', 'con', 'dec']:
            stat['consume'].append(sum(t*v*power_h(v) for t, v in stat[k]))
        
        return np.sum(turn[1:-1]), np.std(path[:,2]), sum(stat['total']), sum(stat['consume'])
    
    def partition(self, *args) -> tuple[int, int]:
        '''args: src, parent, pos, dst in env coordinates `tuple[int]`'''
        (x1, y1, _), _, (x0, y0, _), (x2, y2, _) = args
        args = ((x1, y1), (x0, y0), (x2, y2))
        if args not in self.__partition:
            k = self.__partition['k']
            if k == 0:
                d = abs(x2 - x0) * self.res, abs(y2 - y0) * self.res
            elif k == np.inf:
                d = abs(y2 - y0) * self.res, abs(x2 - x0) * self.res
            else:
                if y0 == k * (x0 - x1) + y1:
                    x, y = x0, y0
                else:
                    x = (y0 - y1 + x0 / k + k * x1) / (1 / k + k)
                    y = k * (x - x1) + y1
                d = self.dist2d((x,y), args[2], self.res), self.dist2d((x,y), args[1], self.res)
            self.__partition[args] = d
        return self.__partition[args]
    
    @staticmethod
    def ray(src: tuple[int], dst: tuple[int]):
        return ([src, tuple((d - s) for s, d in zip(src, dst))])
    
    @staticmethod
    def dist2d(src: tuple[int], dst: tuple[int], res):
        return np.sqrt((res*(dst[0]-src[0]))**2 + (res*(dst[1]-src[1]))**2)
    
    @staticmethod
    def dist(src: tuple[int], dst: tuple[int], res):
        return np.sqrt((res*(dst[0]-src[0]))**2 + (res*(dst[1]-src[1]))**2 + (dst[2]-src[2])**2)
    
    @staticmethod
    def mandist(src: tuple[int], dst: tuple[int], res):
        return res * abs(src[0]-dst[0]) + res * abs(src[1]-dst[1]) + abs(src[2]-dst[2])
    
    @staticmethod
    def opt_mandist(src: tuple[int], parent: tuple[int], pos: tuple[int], dst: tuple[int], res, r1, r2):
        
        dx = abs(src[0]-dst[0])
        dy = abs(src[1]-dst[1])
        x = abs(pos[0]-dst[0]) / dx if dx else abs(pos[1]-dst[1]) / dy + 0.1
        y = abs(pos[1]-dst[1]) / dy if dy else abs(pos[0]-dst[0]) / dx + 0.1
        xy = x+y
        z1, z2 = abs(pos[2]-dst[2]), abs(pos[2]-parent[2])
        z = (z1 if z1<z2 else z2) if xy>0.2 else z1
        return (r1*res*(x*abs(pos[0]-dst[0]) + y*abs(pos[1]-dst[1]))/xy if xy else 0) + r2*z
    
    def g(self, *args, **kwargs):
        '''
        Cost Function
        -------------
        *args: src, parent, pos, dst in env coordinates `tuple[int]`
        **kwargs: g function parameters
        '''
        if kwargs.get('default'):
            return self.dist(*args[1:3], self.res)
        
        _, parent, pos, _, da, t = args
        
        d = self.dist(parent, pos, self.res)
        h = abs(parent[2] - pos[2])
        rf, rw, rd = kwargs['rf'], kwargs['rw'], kwargs['rd']
        
        a, b = (parent[0] - t[0], parent[1] - t[1]), (pos[0] - parent[0], pos[1] - parent[1])
        if np.any(a) and np.any(b):
            rt = kwargs.get('rt', self.dist(parent, t, self.res))
            t = np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))
            t = round(np.arccos(t if t < 1 else 1), 4) if t >= 0 else np.inf
        else:
            rt = 0
        
        return rf * d + rw * h + (kwargs['dh'] / (rt + d) * t if rt else 0) + \
            (sum((k*len(v)**rd) for k, v in da.items()) if da else 0)
    
    def h(self, *args, **kwargs):
        '''
        Heuristic Function
        ------------------
        *args: src, parent, pos, dst in env coordinates `tuple[int]`
        **kwargs: h function parameters
        '''    
        if kwargs.get('default'):
            return self.mandist(*args[2:4], self.res)
        return self.opt_mandist(*args, self.res, kwargs['r1'], kwargs['r2'],)
    
    def children(self, pos: tuple[int], min_alt: int | float, max_alt: int | float, z_step: int):
        '''Check collision and max altitude of nearby 26 child cells'''
        if pos in self.__children:
            return self.__children[pos]
        __children = []
        dr = 0
        da = [0]
        for x in range(pos[0]-1, pos[0]+2):
            for y in range(pos[1]-1, pos[1]+2):
                if (x, y) in self.da:
                    da.append(self.da[x, y])
                for z in range(pos[2]-z_step, pos[2]+z_step*2, z_step):
                    if 0 <= x < self.maxx and 0 <= y < self.maxy:
                        ground = self.env.iloc[x, y]
                        if min_alt < z - ground < max_alt:
                            __children.append((x, y, z))
                            continue
                    dr += 1
        try:
            __children.remove(pos)
            self.__children[pos] = dr/26, max(da), __children
        except:
            self.__children[pos] = 1, max(da), []
        return self.__children[pos]
    
    def sight(self, src: tuple, dst: tuple, **kwargs) -> tuple[float, dict[set]]:
        '''
        A method to check collision between source and destination: 
        src, dst: env coordinates in `tuple[int]`
        kwargs: min_alt, max_alt, z_step for self.children()
        returns danger ratio and danger area
        '''
        if self.dist(src, dst, self.res) <= self.res/2:
            pos = round(src[0]), round(src[1]), round(src[2])
            dr, da = self.children(pos, **kwargs)[:2]
            return dr, {da: {pos}} if da else dict()
        else:
            pos = ((src[0] + dst[0]) / 2, (src[1] + dst[1]) / 2, (src[2] + dst[2]) / 2)
            dr1, da1 = self.sight(dst, pos, **kwargs)
            dr2, da2 = self.sight(src, pos, **kwargs)
            for k, v in da2.items():
                da1.setdefault(k, set()).union(v)
            return dr1 if dr1 > dr2 else dr2, da1
    
    @overload
    def visualize(self): ...
    
    @overload
    def visualize(self, __path: np.ndarray, __pos: np.ndarray): ...
    
    @overload
    def visualize(self, __start: int, __stop: int): ...
    
    def visualize(self, *args):
        
        black = (1,1,1,0)
        z = self.env.to_numpy().T / self.res
        maxz = z.max().max()
        radius = 0, max([self.maxx, self.maxy, ])
        
        '''empty plt initialize'''
        
        ax = plt.subplot(111, projection='3d')
        ax.view_init(elev=90., azim=0.)
        ax.clear()
        
        ax.set_xlim3d(radius)
        ax.set_ylim3d(radius)
        ax.set_zlim3d(radius)
        
        ax.xaxis.set_pane_color(black)
        ax.yaxis.set_pane_color(black)
        ax.zaxis.set_pane_color(black)
        
        ax.xaxis._axinfo["grid"]['color'] = black
        ax.yaxis._axinfo["grid"]['color'] = black
        ax.zaxis._axinfo["grid"]['color'] = black

        '''env map as surface plot'''
        
        ax.plot_surface(
            *np.meshgrid(np.arange(0, self.maxx, 1), np.arange(0, self.maxy, 1)), 
            z, edgecolor='none', rstride=1, 
            cstride=1, cmap=plt.cm.viridis, alpha=0.5)
        
        '''danger areas'''
        
        if self.da:
            for (x, y), v in self.da.items():
                if x < self.maxx and y < self.maxy:
                    z = self.env.iloc[x, y] / self.res
                    ax.add_line(plt3d.art3d.Line3D(
                        (x, x), (y, y), (z, maxz + 2), alpha=0.5, 
                        color='y' if v <= 0.5 else 'r' if v == 1 else 'm'))
        
        '''add path and pos'''
        
        path = list(self.path_nodes.values())
        if len(args) == 2:
            if isinstance(args[0], np.ndarray) and isinstance(args[1], np.ndarray):
                path, pos = args
                path = [DataFrame(path)]
                if len(pos):
                    ax.scatter3D(pos[:, 0], pos[:, 1], pos[:, 2]/self.res, s=1, alpha=0.5, color='g')
            elif isinstance(args[0], int) and isinstance(args[1], int):
                path = path[args[0]: args[1]]
        
        for p in path:
            for s, d in zip(p.iloc[1:].values, p.iloc[:-1].values):
                ax.add_line(plt3d.art3d.Line3D(
                    (s[0], d[0]), (s[1], d[1]), (s[2]/self.res, d[2]/self.res), alpha=1, color='r'))
        
        srcs = np.unique([p.iloc[0, :3] for p in path], axis=0)
        dsts = np.unique([p.iloc[-1, :3] for p in path], axis=0)
        merged = srcs.tolist() + dsts.tolist()
        sds = {'bo': [], 'go': [], 'ro': []}
        for s in srcs:
            sds['go' if merged.count(s.tolist()) == 1 else 'bo'].append(s)
        for d in dsts:
            sds['ro' if merged.count(d.tolist()) == 1 else 'bo'].append(d)
        
        for k, v in sds.items():
            for v in np.unique(v, axis=0):
                ax.plot(v[0], v[1], v[2]/self.res, k, markersize=6, markeredgecolor='k')
        
        plt.ioff()
        plt.pause(0.01)
        plt.show()

class TaskAssigner(object):
    
    def __init__(
        self,
        planner: PathPlanner, 
        tasks: DataFrame, 
        load: str | Path | Iterable[str] | Iterable[Path] = None, 
        **kwargs) -> None:
        r'''
        Parameters
        -----
        planner: `PathPlanner`, plan path to get flight consumption and time
        tasks: `DataFrame`, defines tasks' / cargo's index(starting with 1), 
        src(source), dst(destination), content, priority, weight, 
        expect delivery time, expire time
        load: `str | Path | Iterable[str] | Iterable[Path]`, calling planner.load
        kwargs: plan kwargs
        '''
        
        if isinstance(load, str) or isinstance(load, Path):
            planner.load(load)
        elif isinstance(load, Iterable):
            for load in load:
                planner.load(load)
        self.planner = planner
        self.plan_kwargs = kwargs
        
        '''tasks initialize'''
        # tasks = read_csv('tasks.csv', index=0, header=0)
        self.tasks = tasks
        if 0 in tasks.index:
            raise KeyError('Tasks index should be 1-based')
        
        self.sort = {
            'empty': 0, 'regular': 2, 'valuable': 3, 'special': 4,
            'normal': -1, 'express': 0, 'urgent': 1,
            'E': 0, 'R': 1, 'V': 2, 'S': 3, 'N': -1, 'U': 1}
        self.laconsume = lambda w: 1500 * w + 3000
        self.hover = lambda w: 60 * w + 320
        self.__chromo2id = {}
    
    def merge_type(self, *args: tuple[str]):
        l = len(args)
        if l == 2:
            return tuple(x if self.sort[x] > self.sort[y] else y for x, y in zip(*args))
        elif l == 1:
            return tuple(*args)
        elif l % 2:
            return self.merge_type(self.merge_type(*args[1:]), args[0])
        else:
            return self.merge_type(self.merge_type(*args[:l//2]), self.merge_type(*args[l//2:]))
    
    def plan(self, __index: Iterable[int] | int = None, 
             mode: Literal['efficiency', 'economy', 'balanced', 'default'] = 'balanced', 
             save: Path | str = Path(), only_new: bool = False, **kwargs):
        output = []
        if __index is None:
            __index = self.tasks.index
        elif not isinstance(__index, Iterable):
            __index = [__index]
        for k, v in self.plan_kwargs.items():
            kwargs.setdefault(k, v)
        for idx in __index:
            output.append(self.planner.plan(
                self.tasks.loc[idx, 'src'], self.tasks.loc[idx, 'dst'], 
                type=tuple(self.tasks.loc[idx, ['content', 'priority']]), 
                weight=self.tasks.loc[idx, 'weight'], mode=mode, **kwargs))
        if len(output):
            self.planner.save(save, only_new)
        return output
    
    def initialize(self, population: int, uavs: DataFrame) -> list[DataFrame]:
        '''
        Results
        -----
        generation in list of individuals,
        individual in `DataFrame`, example:
        - index          chromo
        - int, uav index list[int], task index
        - 0              [1,3,2,0]
        - 1              [5,4,0]
        
        '''
        expire_mean = dict((key, group.expire.mean()) \
            for key, group in self.tasks.groupby('src'))
        generation = []
        for _ in range(population):
            permuted_index = []
            for index in np.random.permutation(np.array(
                [group.index for _, group in uavs.groupby('pos')], dtype=object)):
                permuted_index.extend(index)
            individual = DataFrame(index=permuted_index)
            DataFrame(index=permuted_index).sort_index()
            individual['chromo'] = [[] for _ in permuted_index]
            individual['weight'] = 0
            for i in np.random.permutation(self.tasks.index):
                task = self.tasks.loc[i]
                uav = uavs.loc[task.src == uavs.pos]
                individuala = individual.loc[uav.index]
                uava = individuala.loc[individuala.weight + task.weight < uav.capacity].index
                if uava.size:
                    p = individual.loc[uava, 'chromo'].map(len).values
                    ps = p.sum()
                    if ps:
                        if task.expire < expire_mean[task.src]:
                            p = ps - p
                            ps = p.sum()
                        j = np.random.choice(uava, p=p/ps if ps else None)
                    else:
                        j = np.random.choice(uava)
                    individual.loc[j, 'chromo'].append(i)
                    individual.loc[j, 'weight'] += task.weight
                else:
                    j = np.random.choice(uav.index)
                    individual.loc[j, 'chromo'].extend([0, i])
                    individual.loc[i, 'weight'] = task.weight
            generation.append(individual.drop(columns='weight'))
        return generation
    
    @final
    def optimize(
        self, 
        population: int, 
        max_iteration: int, 
        uavs: DataFrame, 
        target_mode: Literal['efficiency', 'economy', 'balanced', 'default'], 
        select: tuple[int, float, float] = (10, 0.2, 0.1),
        crossover: tuple[float, float] = (0.75, 0.4),
        mutate: tuple[float, float] = (0.5, 0.2),
        fitness: tuple[int, int, int] = (1, 1e-3, 10), 
        visualize: bool = True, 
        output: Path | str | None = None,
        **kwargs):
        r'''
        Genetic Optimize main function
        -----
        population: `int`, Genetic Optimize population size
        max_iteration: `int`, maximum iteration for Genetic Optimize
        uavs: `DataFrame`, defines UAV information including terminal (initial pos), 
        capacity, endurance (in J), load time, unload time, rate of charge, change time.
        their initial pos in (x, y) and time before available, or total number of UAVs
        target_mode: `str`, target system running mode
        select: `tuple[int, float, float]`, selected group `count`, `top`, `bottom` probability
        crossover: `tuple[float, float]`, crossover `top`, `bottom` probability
        mutate: `tuple[float, float]`, mutate `top`, `bottom` probability
        fitness: `tuple[int, int, int]`, `time`, `consumption` and `delay` ratio for fitness calculation
        visualize: `bool`, show plan results
        '''
        
        ################
        ## initialize ##
        ################
        
        s_count, s_top, s_bottom = select
        if population % s_count:
            raise ValueError('select group count invalid')
        else:
            s_size = population // s_count
        c1, c2 = crossover
        m1, m2 = mutate
        self.plan_kwargs['mode'] = target_mode
        
        generation = self.fits(self.initialize(population, uavs), uavs, fitness, **kwargs)
        col = ['times', 'consumes', 'delays']
        stat = {'min_fit': [], 'mean_fit': []}
        
        for iter in range(max_iteration):
            
            p0 = iter / max_iteration
            f_mean = generation.fitness.mean()
            f_min = generation.fitness.min()
            f1, f2 = np.percentile(generation.fitness, (75, 25), axis=0)
            pc = lambda f: ((c1-c2)*np.exp(-5*p0)+c2)*(1 if f < f2 else (f_mean-f)/(f_mean-f2))
            pm = lambda f: ((m1-m2)*(1-np.exp(-10*p0))+m2)*(1 if f > f1 else (f-f_mean)/(f1-f_mean))
            
            try:
                
                #############
                # SELECTION #
                #############
                
                p = round((s_top - p0 * (s_top - s_bottom)) * s_size)
                next_generation, selected = [], []
                for select in np.random.permutation(generation.index).reshape((s_count, s_size)):
                    select = generation.loc[select].sort_values(by='fitness')
                    next_generation.extend(select.iloc[:p].individual.values)
                    selected.append(select.iloc[p:])
                
                #############
                # CROSSOVER #
                #############
                
                selected = concat(selected)
                individual = generation.loc[np.random.choice(select.iloc[:p].index), 'individual'].copy()
                individual['chromo'] = [i.copy() for i in individual.chromo]
                selected.at[selected.fitness.idxmax(), 'individual'] = individual
                idx = selected.loc[selected.fitness.map(
                    lambda f: pc(f) if pc(f) > c2 else c2) > np.random.rand(len(selected))]
                crossovers = np.random.permutation(idx.index)
                crossovers = crossovers[1:] if crossovers.size % 2 else crossovers
                crossover = []
                
                for i, j in crossovers.reshape(len(crossovers)//2, 2):
                    individual = {i: selected.individual[i].copy(), j: selected.individual[j].copy()}
                    individual[i]['src'] = individual[j]['src'] = uavs.pos
                    for k in i, j:
                        grouped = individual[k].groupby('src')
                        individual[k]['times'] = grouped['time'].transform('max')
                        individual[k]['consumes'] = grouped['consume'].transform('mean')
                        individual[k]['delays'] = grouped['delay'].transform('sum')
                        individual[k]['fitness'] = (individual[k][col] * fitness).sum(axis=1)
                    
                    idx = (individual[i].fitness - individual[j].fitness).astype(float)
                    # j better than i
                    if idx.max() > 0:
                        src = individual[i].src[idx.idxmax()]
                        crossover.append(i)
                    # i better than j
                    else:
                        src = individual[i].src[idx.idxmin()]
                        crossover.append(j)
                    idx = individual[i].loc[individual[i].src == src].index
                    
                    ii, jj = individual[i].loc[idx, 'chromo'], individual[j].loc[idx, 'chromo']
                    for k, ii, jj in zip(idx, jj, ii):
                        individual[i].at[k, 'chromo'] = ii
                        individual[j].at[k, 'chromo'] = jj
                    for k in i, j:
                        selected.at[k, 'individual'] = individual[k].drop(columns=['src', 'fitness']+col)
                        
                
                ############
                # MUTATION #
                ############
                
                mutates = selected.loc[np.setdiff1d(selected.index, crossover, True)]
                mutates = mutates.loc[mutates.fitness.map(
                    lambda f: pm(f) if pm(f) > m2 else m2) > np.random.rand(len(mutates))]
                fm = sm = tm = 0
                
                for k in mutates.index:
                    individual = selected.individual[k].copy()
                    if np.random.random() < (p0 if p0 > 0.5 else 0.5):
                        permuted_index = []
                        for index in np.random.permutation(np.array(
                            [group.index for _, group in uavs.groupby('pos')], dtype=object)):
                            permuted_index.extend(index)
                        individual.reindex(permuted_index)
                    
                    individual['src'] = uavs.pos
                    grouped = individual.groupby('src')
                    individual['times'] = grouped['time'].transform('max')
                    individual['consumes'] = grouped['consume'].transform('mean')
                    individual['delays'] = grouped['delay'].transform('sum')
                    individual['fitness'] = (individual[col] * fitness).sum(axis=1)
                    for src, group in individual.groupby('src'):
                        tasks = self.tasks.loc[self.tasks.src == src]
                        capacity = uavs.capacity[group.index[0]]
                        if individual.delay.sum():
                            if np.random.rand() <= group.delay.sum() / individual.delay.sum():
                                tm += 1
                                for i, j in zip(group.index, self.targeted_mutate(group, capacity, tasks)):
                                    individual.at[i, 'chromo'] = j
                                continue
                        
                        i = np.random.choice(group.index)
                        group.at[i, 'chromo'] = individual.at[i, 'chromo'] = self.flip_mutate(group.chromo[i])
                        fm += 1
                        j = np.random.choice(group.index)
                        if i == j:
                            tm += 1
                            for i, j in zip(group.index, self.targeted_mutate(group, capacity, tasks)):
                                individual.at[i, 'chromo'] = j
                        else:
                            group.at[j, 'chromo'] = individual.at[j, 'chromo'] = self.flip_mutate(group.chromo[j])
                            fm += 1
                            individual.at[i, 'chromo'], individual.at[j, 'chromo'] = \
                                self.swap_mutate(*group.chromo[[i, j]], capacity, tasks)
                            sm += 1
                    
                    selected.at[k, 'individual'] = individual.drop(columns=['src', 'fitness'] + col)
                
                ###########
                # FITNESS #
                ###########
                
                print(" Progress: {}/{}, Fitness: {:.0f}/{:.0f}, CO: {}, FM: {}, SM: {}, TM: {}".format(
                    iter+1, max_iteration, f_min, f_mean, len(crossovers), fm, sm, tm))
                stat['min_fit'].append(f_min)
                stat['mean_fit'].append(f_mean)
                
                next_generation.extend(selected.individual.values)
                if iter + 1 < max_iteration:
                    generation = self.fits(next_generation, uavs, fitness, **kwargs)
            except Exception as i:
                print(f"\rException: {i}..", end='')
                with open('GAE.txt', 'a') as file:
                    file.write(f"{population}\t{iter}/{max_iteration}\t{time.ctime()[11:19]}\t{i}\n")
        
        self.plan_kwargs.pop('mode')
        
        if isinstance(output, Path) or isinstance(output, str):
            sequence, planned = self.plan_individual(
                generation.individual[generation.fitness.idxmin()], uavs, True)
            planned['fitness'] = (planned[['time', 'consume', 'delay']] * fitness).sum(axis=1)
            planned['load_rate'] = 0
            for idx in planned.index:
                s = self.planner.path_analysis.loc[planned.id[idx], 'path_length']
                planned.at[idx, 'load_rate'] = (s * [
                    id.weight for id in planned.id[idx]]).sum() / s.sum() / uavs.capacity[idx]
            iteration = DataFrame(stat)
            with ExcelWriter(Path(output), engine='openpyxl', mode='w') as file:
                iteration.to_excel(file, 'Iteration')
                sequence = concat([planned, sequence], axis=1)
                sequence['chromo'] = sequence.chromo.map(lambda x: str(x)[1:-1].replace(' ',''))
                sequence['id'] = sequence.id.map(
                    lambda x: str([x.dst for x in x])[1:-1].replace(' ','').replace("'",''))
                sequence.reset_index().to_excel(file, 'Individual', index=False)
                ids = []
                for idx in sequence.index:
                    analysis = DataFrame(columns=['task']+list(self.planner.path_analysis_refer))
                    chromo, id = planned.loc[idx, ['chromo', 'id']]
                    ids += id
                    for i in range(len(id)):
                        analysis.loc[i] = [chromo[i], id[i]] + \
                            self.planner.path_analysis.loc[id[i]].tolist()
                    analysis.to_excel(file, f'UAV{idx}', index=False)
                for id in ids:
                    self.planner.path_nodes[id].to_excel(file, str(id), index=False)
        
        if visualize:
            font = {'fontproperties': 'Times New Roman', 'fontsize': 15}
            plt.xticks(**font)
            plt.yticks(**font)
            plt.xlabel('iteration', **font)
            plt.ylabel('fitness', **font)
            x = np.arange(max_iteration) + 1
            plt.plot(x, stat['fit_min'])
            plt.plot(x, stat['fit_mean'])
            plt.ioff()
            plt.show()
    
    @staticmethod
    def capacity(__c: list[int], capacity: int, tasks: DataFrame):
        for i in range(1, len(__c)):
            if tasks.loc[__c[:i], 'weight'].sum() > capacity:
                return i-1, 0
        return 0, capacity-tasks.loc[__c, 'weight'].sum()
    
    @staticmethod
    def flip_mutate(__c: list[int]):
        if 0 in __c:
            __c.remove(0)
        l = len(__c)
        if l == 2:
            __c[0], __c[1] = __c[1], __c[0]
        elif l > 2:
            c = __c.copy()
            while c == __c:
                i = sorted(np.random.choice(l+1, 2, False))
                j = np.random.randint(i[0], i[1]+1), np.random.randint(i[1], l+1)
                c = __c[:i[0]] + __c[i[1]:j[1]] + __c[j[0]:i[1]] + __c[i[0]:j[0]] +  __c[j[1]:]
            __c = c
        return __c + [0]
    
    @staticmethod
    def swap_mutate(__c1: list[int], __c2: list[int], capacity: int, tasks: DataFrame):
        if 0 in __c1:
            __c1.remove(0)
        if 0 in __c2:
            __c2.remove(0)
        l1, l2 = len(__c1), len(__c2)
        if l1 == 1 and l2 == 1:
            c1, c2 = __c2, __c1
        else:
            for _ in range(100):
                ci = np.random.choice(__c1, np.random.randint(l1+1), False)
                cj = np.random.choice(__c2, np.random.randint(l2+1), False)
                c1 = np.setdiff1d(__c1, ci, True).tolist() + cj.tolist()
                c2 = np.setdiff1d(__c2, cj, True).tolist() + ci.tolist()
                if c1 != __c1 and c2 != __c2:
                    if tasks.loc[c1, 'weight'].sum() < capacity and \
                        tasks.loc[c2, 'weight'].sum() < capacity:
                            break
            else:
                c1, c2 = __c2, __c1
        return c1 + [0], c2 + [0]
    
    @staticmethod
    def targeted_mutate(individual: DataFrame, capacity: int, tasks: DataFrame):
        i = 1
        individual['weight'] = capacity - individual.chromo.map(
            lambda x: tasks.loc[x[:-1], 'weight'].sum())
        i, j = individual.weight.idxmin(), individual.weight.idxmax()
        src, dst = individual.loc[[i, j], 'chromo']
        for k in range(1, len(src)-1):
            if tasks.loc[src[k:-1], 'weight'].sum() <= individual.weight[j]:
                k = np.random.randint(k, len(src)-1)
                individual.at[i, 'chromo'] = src[:k] + [0]
                individual.at[j, 'chromo'] = dst[:-1] + src[k:]
                break
        else:
            task = tasks.loc[src[:-1]]
            task = task.loc[task.weight < individual.weight[j]].index
            if task.size:
                k = np.random.choice(task)
                src.remove(k)
                dst.insert(np.random.randint(len(dst)-1) if dst[0] else 0, k)
                individual.at[i, 'chromo'] = src
                individual.at[j, 'chromo'] = dst
        return individual.chromo
    
    def fits(self, generation: list[DataFrame], uavs: DataFrame, fit, **kwargs):
        
        fitness = []
        population = len(generation)
        for idx in range(population):
            i = self.plan_individual(generation[idx], uavs)
            fitness.append(self.fit(i, *fit))
            generation[idx] = i
            p = (idx + 1) / population
            print("\rFitted: {:03.0f}% |{}{}|".format(
                p*100, str().join(['>' for _ in range(round(p*10))]), 
                str().join([' ' for _ in range(10-round(p*10))])), end='')
        if self.planner.success and kwargs:
            self.planner.save(**kwargs)
        return DataFrame({'individual': generation, 'fitness': fitness})
    
    @staticmethod
    def fit(__i: DataFrame, __r1, __r2, __r3):
        return __i.time.max()*__r1 + __i.consume.mean()*__r2 + __i.delay.sum()*__r3
    
    def plan_individual(
        self, individual: DataFrame, uavs: DataFrame, final: bool = False):
        
        inf = 60
        planned_col = ['chromo', 'id', 'time', 'consume', 'delay']
        planned = DataFrame(index=individual.index, columns=planned_col)
        planned.chromo = individual.chromo.copy()
        # chromo integrity check
        for idx in planned.index:   
            if planned.chromo[idx]:
                if planned.chromo[idx][-1]:
                    planned.at[idx, 'chromo'].append(0)
            else:
                planned.at[idx, 'chromo'] = [0]
            
        for src, idx in uavs.groupby('pos').groups.items():
            k = []
            for chromo in planned.loc[idx, 'chromo']:
                k += chromo[:-1]
            k = np.unique(k, return_counts=True)
            # repeated gene
            if np.any(k[1] > 1):
                chromo = []
                for i, j in zip(*k):
                    for _ in range(j-1):
                        chromo.append(i)
                for i in idx:
                    while chromo[0] in planned.loc[i, 'chromo']:
                        planned.at[i, 'chromo'].remove(chromo.pop(0))
                        if not chromo:
                            break
                    else:
                        continue
                    break
            
            # missing gene
            k = np.setdiff1d(self.tasks.loc[self.tasks.src == src].index, k[0])
            if k.size:
                avail = uavs.capacity[idx] - planned.chromo[idx].map(lambda x: self.tasks.weight[x[:-1]].sum())
                for k in k:
                    i = np.random.choice(planned.loc[idx].loc[self.tasks.weight[k] < avail].index)
                    planned.at[i, 'chromo'].insert(-1, k)
                    avail[i] -= self.tasks.weight[k]
        
        for idx in planned.index:
            planned.at[idx, 'id'] = self.chromo2id(planned.chromo[idx])
        
        stat_index = ['time_in', 'time_out', 'power_in', 'power_out']
        node_stat = dict((k, DataFrame(index=stat_index)) for k in \
            (*self.tasks.dst.unique(), *self.tasks.src.unique()))
        
        for idx in planned.index:
            uav = uavs.loc[idx]
            la = uav.time_land + uav.time_airborn
            # init airborn
            stop, uav_power = uav.time_airborn + uav.load, uav.endurance
            ids = planned.id[idx]
            ids: list[ID]
            
            # time: flight ~ land ~ unload ~ (idle) ~ load ~ airborn ~ flight
            # power: airborn + flight + land ~ recharge / change ~ airborn + flight + land
            consumption = delay = 0
            
            for i in range(len(ids)):
                id = ids[i]
                # landing, time
                stop += round(id.time)
                # landed, power
                consume = id.consumption + self.laconsume(id.weight)
                uav_power -= consume
                # time in use starts
                start = stop
                # time in use stops
                
                # last flight returns terminal
                if i+1 == len(ids):
                    stop += uav.time_land
                    break
                # cargo to be unloaded
                elif id.weight:
                    stop += uav.unload + la
                    power_out, idle = self.power(ids[i+1], uav, uav_power, uav.unload)
                # empty flight return to load cargo
                else:
                    stop += uav.load + la
                    power_out, idle = self.power(ids[i+1], uav, uav_power, uav.load)
                
                consumption += consume
                
                # may conflict with the others
                while node_stat[id.dst].columns.size and id.dst in self.planner.distributor:
                    # check conflict
                    for ti, to, _, _ in node_stat[id.dst].T.values:
                        if ti <= start < to or ti < stop+idle <= to or (ti >= start and to <= stop+idle):
                            # conflict detects
                            break
                    else:
                        # fully check and conflict-free
                        break
                    
                    avail = {'time_in': [0], 'time_out': []}
                    for ti, to, _, _ in node_stat[id.dst].T.sort_values(by='time_in').values:
                        avail['time_out'].append(ti)
                        avail['time_in'].append(to)
                    avail['time_out'].append(np.inf)
                    # avail: time available for uav to land and airborn at dst
                    avail = DataFrame(avail)
                    avail = avail.loc[avail.time_out > stop+idle].loc[
                        avail.time_out-avail.time_in > stop+idle-start]
                    # pending `start` time
                    offset = avail.iloc[0,0] - start
                    
                    # extend holding / idle time at last dst
                    if ids[i].src in self.planner.distributor:
                        dst = ids[i-1].dst
                        ns = node_stat[dst].T
                        stopi = ns.time_in[ns.time_in > ns.time_out[idx]].min()
                        stopt = ns.time_out[idx] + offset
                        if stopi >= stopt or stopi is np.nan:
                            node_stat[dst].loc['time_out', idx] = stopt
                            uav_power, _ = self.power(id, uav, ns.power_in[idx], stopt-start-la)
                            node_stat[dst].loc['power_out', idx] = uav_power
                            uav_power -= consume
                        else:
                            # idle and hover
                            node_stat[dst].loc['time_out', idx] = stopi
                            hover = self.hover(id.weight) * (stopt - stopi)
                            uav_power, _ = self.power(id, uav, ns.power_in[idx], stopi-start-la)
                            node_stat[dst].loc['power_out', idx] = uav_power
                            uav_power -= consume + hover
                            # failure
                            if uav_power < 0:
                                uav_power = 0
                                delay += inf
                    # from terminal
                    else:
                        try:
                            ti, to, pi = node_stat[ids[i].src].loc[
                                ['time_in','time_out','power_in'], idx]
                            uav_power = self.power(id, uav, pi, to-ti-la+offset)[0] - consume
                        except KeyError:
                            uav_power = uav.endurance - consume
                    
                    start = avail.iloc[0,0]
                    stop += offset
                    power_out, idle = self.power(ids[i+1], uav, uav_power, stop-start-la)
                    break
                
                # check delay
                i = planned.chromo[idx][i]
                if i:
                    delayi, expire = self.tasks.loc[i, ['expect', 'expire']]
                    unloaded = start + uav.time_land + uav.unload
                    delayi -= unloaded
                    if delayi < 0:
                        if expire >= unloaded:
                            delay -= delayi
                        else:
                            delay = inf
                
                # add node stat and set next loop
                stop += idle
                node_stat[id.dst][idx] = [start, stop, uav_power, power_out]
                uav_power = power_out
            
            if planned.chromo[idx][0]:
                planned.time[idx] = start
                planned.consume[idx] = round(consumption)
                planned.delay[idx] = delay
            else:
                planned.time[idx] = planned.consume[idx] = planned.delay[idx] = 0
        
        if final:
            col = ["node0", "flight0"]
            for i in range(1, planned.id.map(len).max()):
                col.extend([f'node{i}', f'flight{i}'])
            output = DataFrame(index=individual.index, columns=col+[f'node{i+1}'])
            for idx in individual.index:
                if planned.id[idx]:
                    t = round(planned.id[idx][0].time)
                    output.at[idx, 'node0'] = node_stat[planned.id[idx][0].dst].loc['time_in', idx] - t
                    output.at[idx, 'flight0'] = t
                    for i in range(1, len(planned.id[idx])):
                        id = planned.id[idx][i]
                        ti, to = node_stat[id.src].loc[['time_in', 'time_out'], idx]
                        output.at[idx, f'node{i}'] = to - ti
                        output.at[idx, f'flight{i}'] = round(id.time)
                    output.at[idx, f'node{i+1}'] = uavs.time_land[idx]
            return output.fillna(0).sort_index(), planned.sort_index()
        else:
            return planned.drop(columns='id')
    
    def power(self, id: ID, uav: dict, power_in: int, time_period: int) -> tuple[int, int]:
        '''returns power out `int` and idle time `int`'''
        # consider power required for the next itinerary
        power_out = power_in + uav['roc'] * time_period
        power_out = uav['endurance'] if power_out >= uav['endurance'] else power_out
        power = id.consumption + self.laconsume(id.weight)
        # extra recharge time required
        if power_out < power:
            idle = round((power - power_out) / uav['roc'])
            # distributor recharge only, terminal recharge or change battery
            if idle >= uav['change'] and id.dst in self.planner.terminal:
                idle = uav['change']
                power_out = uav['endurance']
            else:
                power_out = power
        # meets power requirement
        else:
            idle = 0
        return power_out, idle
    
    def chromo2id(self, chromo: list[int], **kwargs) -> list[ID]:
        key = tuple(chromo)
        if key not in self.__chromo2id:
            start = 0
            ids = []
            while start + 1 < len(chromo):
                stop = chromo.index(0, start)
                for idx in range(start, stop):
                    task = self.tasks.loc[chromo[idx:stop]]
                    src = self.tasks.loc[chromo[idx], 'src'] if idx == start else \
                        self.tasks.loc[chromo[idx-1], 'dst']
                    dst = self.tasks.loc[chromo[idx], 'dst']
                    kwargs['type'] = self.merge_type(*task[['content','priority']].values)
                    kwargs['weight'] = task['weight'].sum()
                    flag, id = self.planner.plan(ID(src, dst, **kwargs), **self.plan_kwargs)
                    if flag:
                        raise Exception(f'Path plan {id} {self.planner.returns[flag]}')
                    else:
                        ids.append(id)
                flag, id = self.planner.plan(ID(dst, self.tasks.src[chromo[idx]]), **self.plan_kwargs)
                if flag:
                    raise Exception(f'Path plan {id} {self.planner.returns[flag]}')
                else:
                    ids.append(id)
                start = stop + 1
            self.__chromo2id[key] = ids
        return self.__chromo2id[key]

if __name__ == '__main__':
    
    parser = ArgumentParser()
    parser.add_argument("--population", type = int, default = 200)
    parser.add_argument("--max_iter", type = int, default = 200)
    parser.add_argument("-visualize", action = 'store_true')
    args = parser.parse_args()
    
    hk = read_excel('HKKowloon5.xlsx')
    da = read_csv('HKKowloonDA.csv')
    tasks = read_csv('HKKowloonTasks.csv', index_col=0)
    pos_map = read_csv('HKKowloonTD.csv', index_col=0).fillna(0).astype('int')
    uavs = read_csv('HKKowloonUAVs.csv')
    plan_kwargs = {'max_alt': 60, 'visualize': False, 'show_stat': False, 'get': True}
    opt_kwargs = {'visualize': args.visualize, 
                  'output': Path(f'P{args.population}I{args.max_iter}') / Path(time.strftime(r'%m%d%H%M%S')+'.xlsx')}
    
    if not Path(f'P{args.population}I{args.max_iter}').exists():
        Path(f'P{args.population}I{args.max_iter}').mkdir()
    planner = PathPlanner(hk, da, pos_map)
    assigner = TaskAssigner(planner, tasks, 'HKKowloonPlanned.xlsx', **plan_kwargs)
    assigner.optimize(args.population, args.max_iter, uavs, 'balanced', **opt_kwargs)