From 8d31e74b016cb8cefe50ef9d5d4d18d81879315f Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 14:44:01 +0200
Subject: [PATCH 01/44] added new test cases

---
 tests/main_test.py | 44 ++++++++++++++++++++++++++++++++------------
 1 file changed, 32 insertions(+), 12 deletions(-)

diff --git a/tests/main_test.py b/tests/main_test.py
index d86edcf..07415e3 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -144,6 +144,21 @@
         ((6.5, 4), (7, 9)),
         ([(6.5, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.889979937555021),
     ),
+    # symmetric around the lower boundary obstacle
+    (
+        ((0.5, 0.5), (0.5, 2.5)),
+        ([(0.5, 0.5), (4, 1), (4, 2), (0.5, 2.5)], 8.071067811865476),
+    ),
+    # symmetric around the lower right boundary obstacle
+    (
+        ((16.5, 0.5), (18.5, 0.5)),
+        ([(16.5, 0.5), (17, 6), (18, 6), (18.5, 0.5)], 12.045361017187261),
+    ),
+    # symmetric around the top right boundary obstacle
+    (
+        ((16.5, 9.5), (18.5, 9.5)),
+        ([(16.5, 9.5), (17, 7), (18, 7), (18.5, 9.5)], 6.0990195135927845),
+    ),
 ]
 
 POLY_ENV_PARAMS = (
@@ -191,6 +206,12 @@
     # using a* graph search:
     # directly reachable through a single vertex (does not change distance!)
     (((9, 4), (9, 6)), ([(9, 4), (9, 5), (9, 6)], 2)),
+    # slightly indented, path must go through right boundary extremity
+    (((9.1, 4), (9.1, 6)), ([(9.1, 4.0), (9.0, 5.0), (9.1, 6.0)], 2.009975124224178)),
+    # path must go through lower hole extremity
+    (((4, 4.5), (6, 4.5)), ([(4.0, 4.5), (5.0, 4.0), (6.0, 4.5)], 2.23606797749979)),
+    # path must go through top hole extremity
+    (((4, 8.5), (6, 8.5)), ([(4.0, 8.5), (5.0, 9.0), (6.0, 8.5)], 2.23606797749979)),
 ]
 
 OVERLAP_POLY_ENV_PARAMS = (
@@ -296,8 +317,7 @@ def validate(start_coordinates, goal_coordinates, expected_output):
         else:
             status_str = "XX"
         print(f"{status_str} input: {(start_coordinates, goal_coordinates)} ")
-        if PLOT_TEST_RESULTS:
-            assert correct_result, f"unexpected result (path, length): got {output} instead of {expected_output} "
+        assert correct_result, f"unexpected result (path, length): got {output} instead of {expected_output} "
 
     print("testing if path and distance are correct:")
     for ((start_coordinates, goal_coordinates), expected_output) in test_cases:
@@ -309,13 +329,12 @@ def validate(start_coordinates, goal_coordinates, expected_output):
 
 
 class MainTest(unittest.TestCase):
-    def test_fct(self):
+    def test_grid_env(self):
         grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
 
         grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False)
         assert len(list(grid_env.all_extremities)) == 17, "extremities do not get detected correctly!"
         grid_env.prepare()
-        # raise ValueError
         assert len(grid_env.graph.all_nodes) == 16, "identical nodes should get joined in the graph!"
 
         # test if points outside the map are being rejected
@@ -333,6 +352,7 @@ def test_fct(self):
 
         nr_nodes_env1_old = len(grid_env.graph.all_nodes)
 
+    def test_poly_env(self):
         poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
         poly_env.store(*POLY_ENV_PARAMS, validate=True)
         NR_EXTR_POLY_ENV = 4
@@ -346,14 +366,14 @@ def test_fct(self):
             f"\n found: {poly_env.graph.all_nodes}"
         )
 
-        nr_nodes_env1_new = len(grid_env.graph.all_nodes)
-        assert (
-            nr_nodes_env1_new == nr_nodes_env1_old
-        ), "node amount of an grid_env should not change by creating another grid_env!"
-        assert grid_env.graph is not poly_env.graph, "different environments share the same graph object"
-        assert (
-            grid_env.graph.all_nodes is not poly_env.graph.all_nodes
-        ), "different environments share the same set of nodes"
+        # nr_nodes_env1_new = len(grid_env.graph.all_nodes)
+        # assert (
+        #     nr_nodes_env1_new == nr_nodes_env1_old
+        # ), "node amount of an grid_env should not change by creating another grid_env!"
+        # assert grid_env.graph is not poly_env.graph, "different environments share the same graph object"
+        # assert (
+        #     grid_env.graph.all_nodes is not poly_env.graph.all_nodes
+        # ), "different environments share the same set of nodes"
 
         print("\ntesting polygon environment")
         try_test_cases(poly_env, TEST_DATA_POLY_ENV)

From 1bb77b6b0601dbb6c81da9a9f6190fa8b6db05fd Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 14:44:56 +0200
Subject: [PATCH 02/44] Update extremitypathfinder.py

---
 extremitypathfinder/extremitypathfinder.py | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 52b5226..76be497 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -62,10 +62,13 @@ def all_vertices(self) -> List[PolygonVertex]:
     @property
     def all_extremities(self) -> Set[PolygonVertex]:
         if self._all_extremities is None:
-            self._all_extremities = set()
+            extremities = set()
             for p in self.polygons:
-                # only extremities that are actually within the map should be considered
-                self._all_extremities |= set(filter(lambda e: self.within_map(e.coordinates), p.extremities))
+                extremities |= set(p.extremities)
+
+            # only consider extremities that are actually within the map
+            extremities = set(filter(lambda e: self.within_map(e.coordinates), extremities))
+            self._all_extremities = extremities
         return self._all_extremities
 
     @property

From a7ea81ddb96d7d2eb04a8cfa23723d9a771c038a Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 15:01:03 +0200
Subject: [PATCH 03/44] add test for close but obstructed extremities

---
 tests/main_test.py | 37 ++++++++++++++++++++++++-------------
 1 file changed, 24 insertions(+), 13 deletions(-)

diff --git a/tests/main_test.py b/tests/main_test.py
index 07415e3..25f53e4 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -226,8 +226,8 @@
         (0.5, 16.5),
         (9.5, 16.5),
         (9.5, 45.5),
-        (15.5, 45.5),
-        (15.5, 30.5),
+        (10.0, 45.5),
+        (10.0, 30.5),
         (35.5, 30.5),
         (35.5, 14.5),
         (0.5, 14.5),
@@ -243,16 +243,26 @@
             (40.5, 15.0),
         ],
         [
-            (45.40990195143968, 14.5),
-            (44.59009804856032, 14.5),
-            (43.39009804883972, 20.5),
-            (46.60990195116028, 20.5),
+            (45.4, 14.5),
+            (44.6, 14.5),
+            (43.4, 20.5),
+            (46.6, 20.5),
         ],
+        # slightly right of the top boundary obstacle
+        # goal: create an obstacle that obstructs two very close extremities
+        # to check if visibility is correctly blocked in such cases
         [
-            (40.5, 34.5),
-            (24.5, 34.5),
-            (24.5, 40.5),
-            (40.5, 40.5),
+            (40, 34),
+            (10.5, 34),
+            (10.5, 40),
+            (40, 40),
+        ],
+        # on the opposite site close to top boundary obstacle
+        [
+            (9, 34),
+            (5, 34),
+            (5, 40),
+            (9, 40),
         ],
         [
             (31.5, 5.390098048839718),
@@ -277,12 +287,13 @@
                 (42.5, 7.590098048560321),
                 (42.5, 13.109901951160282),
                 (35.5, 30.5),
-                (24.5, 34.5),
-                (15.5, 45.5),
+                (10.5, 34.0),
+                (10.0, 45.5),
                 (9.5, 45.5),
+                (9, 34),
                 (5, 20),
             ],
-            132.71677685197986,
+            138.23115155299263,
         ),
     ),
 ]

From 985cd526539457b2d6427e264666abf4bba30c4b Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 15:01:54 +0200
Subject: [PATCH 04/44] Update .pre-commit-config.yaml

---
 .pre-commit-config.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 65565ed..97ca922 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -59,7 +59,7 @@ repos:
         additional_dependencies: [ numpy, poetry==1.1.11 ]
 
   - repo: https://github.com/asottile/pyupgrade
-    rev: v2.36.0
+    rev: v2.37.3
     hooks:
       - id: pyupgrade
 

From b9727e9351028bab064f5ac1d57729b7942108ea Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 15:18:44 +0200
Subject: [PATCH 05/44] all extremities as list

---
 extremitypathfinder/extremitypathfinder.py | 20 +++++++++++---------
 extremitypathfinder/helper_classes.py      |  6 +++---
 2 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 76be497..11a8716 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,6 +1,7 @@
+import itertools
 import pickle
 from copy import deepcopy
-from typing import Iterable, List, Optional, Set, Tuple
+from typing import Iterable, List, Optional, Tuple
 
 import numpy as np
 
@@ -47,7 +48,7 @@ class PolygonEnvironment:
     prepared: bool = False
     graph: DirectedHeuristicGraph = None
     temp_graph: DirectedHeuristicGraph = None  # for storing and plotting the graph during a query
-    _all_extremities: Optional[Set[PolygonVertex]] = None
+    _all_extremities: Optional[List[PolygonVertex]] = None
 
     @property
     def polygons(self) -> Iterable[Polygon]:
@@ -60,14 +61,12 @@ def all_vertices(self) -> List[PolygonVertex]:
             yield from p.vertices
 
     @property
-    def all_extremities(self) -> Set[PolygonVertex]:
+    def all_extremities(self) -> List[PolygonVertex]:
         if self._all_extremities is None:
-            extremities = set()
-            for p in self.polygons:
-                extremities |= set(p.extremities)
-
+            poly_extremities = iter(p.extremities for p in self.polygons)
+            extremities_chained = itertools.chain(*poly_extremities)
             # only consider extremities that are actually within the map
-            extremities = set(filter(lambda e: self.within_map(e.coordinates), extremities))
+            extremities = list(filter(lambda e: self.within_map(e.coordinates), extremities_chained))
             self._all_extremities = extremities
         return self._all_extremities
 
@@ -175,7 +174,10 @@ def prepare(self):  # TODO include in storing functions?
         #   even if a node has no edges (visibility to other extremities), it should still be included!
         self.graph = DirectedHeuristicGraph(self.all_extremities)
 
-        extremities_to_check = self.all_extremities.copy()
+        extremities_to_check = set(self.all_extremities)
+
+        # nr_extremities = len(self.all_extremities)
+        # angle_representations = np.full((nr_extremities, nr_extremities), np.nan)
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
         while len(extremities_to_check) > 0:
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index a25fdc0..aab4a69 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -1,5 +1,5 @@
 import heapq
-from typing import Dict, List, Optional, Set
+from typing import Dict, Iterable, List, Optional, Set
 
 import numpy as np
 
@@ -302,13 +302,13 @@ def get(self):
 class DirectedHeuristicGraph(object):
     __slots__ = ["all_nodes", "distances", "goal_node", "heuristic", "neighbours"]
 
-    def __init__(self, all_nodes: Optional[Set[Vertex]] = None):
+    def __init__(self, all_nodes: Optional[Iterable[Vertex]] = None):
         self.distances: Dict = {}
         self.neighbours: Dict = {}
 
         if all_nodes is None:
             all_nodes = set()
-        self.all_nodes: Set[Vertex] = all_nodes.copy()  # independent copy required!
+        self.all_nodes: Set[Vertex] = set(all_nodes)  # independent copy required!
 
         # TODO use same set as extremities of env, but different for copy!
 

From e0b96d51b56ae834507477af84574e48582690a0 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 17:28:00 +0200
Subject: [PATCH 06/44] replace angle representation class start

---
 extremitypathfinder/extremitypathfinder.py | 58 ++++++++++++----
 extremitypathfinder/helper_classes.py      | 77 ++++++++++++++--------
 extremitypathfinder/helper_fcts.py         |  6 +-
 3 files changed, 101 insertions(+), 40 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 11a8716..58b1ed0 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,11 +1,13 @@
 import itertools
 import pickle
 from copy import deepcopy
-from typing import Iterable, List, Optional, Tuple
+from typing import Iterable, Iterator, List, Optional, Tuple
 
 import numpy as np
 
 # TODO possible to allow polygon consisting of 2 vertices only(=barrier)? lots of functions need at least 3 vertices atm
+import pytest
+
 from extremitypathfinder.global_settings import (
     DEFAULT_PICKLE_NAME,
     INPUT_COORD_LIST_TYPE,
@@ -14,7 +16,15 @@
     OBSTACLE_ITER_TYPE,
     PATH_TYPE,
 )
-from extremitypathfinder.helper_classes import DirectedHeuristicGraph, Edge, Polygon, PolygonVertex, Vertex
+from extremitypathfinder.helper_classes import (
+    DirectedHeuristicGraph,
+    Edge,
+    Polygon,
+    PolygonVertex,
+    Vertex,
+    angle_rep_inverse,
+    compute_angle_repr,
+)
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
     convert_gridworld,
@@ -56,7 +66,7 @@ def polygons(self) -> Iterable[Polygon]:
         yield from self.holes
 
     @property
-    def all_vertices(self) -> List[PolygonVertex]:
+    def all_vertices(self) -> Iterator[PolygonVertex]:
         for p in self.polygons:
             yield from p.vertices
 
@@ -172,12 +182,34 @@ def prepare(self):  # TODO include in storing functions?
         # and optimize graph further at construction time
         # NOTE: initialise the graph with all extremities.
         #   even if a node has no edges (visibility to other extremities), it should still be included!
-        self.graph = DirectedHeuristicGraph(self.all_extremities)
+        extremities = self.all_extremities
+        self.graph = DirectedHeuristicGraph(extremities)
+
+        extremities_to_check = set(extremities)
+
+        vertices = list(self.all_vertices)
+        nr_vertices = len(vertices)
+
+        # TODO sparse matrix
+        angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
+
+        def get_angle_representation(v1: PolygonVertex, v2: PolygonVertex) -> float:
+            i1 = vertices.index(v1)
+            i2 = vertices.index(v2)
+            repr = angle_representations[i1, i2]
+
+            # lazy initalisation: compute on demand only
+            if np.isnan(repr):  # attention: repr == np.nan does not match!
+                repr = compute_angle_repr(v1, v2)
+                angle_representations[i1, i2] = repr
+                # make use of symmetry: rotate 180 deg
+                angle_representations[i2, i1] = angle_rep_inverse(repr)
 
-        extremities_to_check = set(self.all_extremities)
+            assert repr is None or not np.isnan(repr)
+            return repr
 
-        # nr_extremities = len(self.all_extremities)
-        # angle_representations = np.full((nr_extremities, nr_extremities), np.nan)
+        def angle_repr_is_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
+            return get_angle_representation(v1, v2) is None
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
         while len(extremities_to_check) > 0:
@@ -206,8 +238,11 @@ def prepare(self):  # TODO include in storing functions?
             # all vertices between the angle of the two neighbouring edges ('outer side')
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            repr1 = n1.get_angle_representation()
-            repr2 = n2.get_angle_representation()
+            repr1 = get_angle_representation(query_extremity, n1)
+            repr1_ = n1.get_angle_representation()
+            if repr1 != pytest.approx(repr1_):
+                raise ValueError
+            repr2 = get_angle_representation(query_extremity, n2)
             repr_diff = abs(repr1 - repr2)
             candidate_extremities.difference_update(
                 find_within_range(
@@ -232,8 +267,8 @@ def prepare(self):  # TODO include in storing functions?
             # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
             # MUST be added to the graph!
             # Find extremities which fulfill this condition for the given query extremity
-            repr1 = (repr1 + 2.0) % 4.0  # rotate 180 deg
-            repr2 = (repr2 + 2.0) % 4.0
+            repr1 = angle_rep_inverse(repr1)
+            repr2 = angle_rep_inverse(repr2)
             # IMPORTANT: the true angle diff does not change, but the repr diff does! compute again
             repr_diff = abs(repr1 - repr2)
             # IMPORTANT: check all extremities here, not just current candidates
@@ -245,6 +280,7 @@ def prepare(self):  # TODO include in storing functions?
                     self.all_extremities,
                 )
             )
+            # temp_candidates = {e for e in extremities if not angle_repr_is_none(query_extremity, e)}
             lie_in_front = find_within_range(
                 repr1,
                 repr2,
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index aab4a69..9b6c37c 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -6,12 +6,13 @@
 # TODO find a way to avoid global variable, wrap all in a different kind of 'coordinate system environment'?
 # problem: lazy evaluation, passing the origin every time is not an option
 # placeholder for temporarily storing the origin of the current coordinate system
+
 origin = None
 
 
-class AngleRepresentation(object):
-    """
-    a class automatically computing a representation for the angle from the origin to a given vector
+def compute_angle_repr_inner(np_vector: np.ndarray) -> float:
+    """computing representation for the angle from the origin to a given vector
+
     value in [0.0 : 4.0[
     every quadrant contains angle measures from 0.0 to 1.0
     there are 4 quadrants (counter clockwise numbering)
@@ -28,39 +29,46 @@ class AngleRepresentation(object):
     angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
     with (0,0)' being the vector representing the origin
 
+    :param np_vector:
+    :return:
     """
+    # 2D vector: (dx, dy) = np_vector
+    dx, dy = np_vector
+    dx_positive = dx >= 0
+    dy_positive = dy >= 0
 
-    # prevent dynamic attribute assignment (-> safe memory)
-    # __slots__ = ['quadrant', 'angle_measure', 'value']
-    __slots__ = ["value"]
+    if dx_positive and dy_positive:
+        quadrant = 0.0
+        angle_measure = dy
 
-    def __init__(self, np_vector):
-        # 2D vector: (dx, dy) = np_vector
-        norm = np.linalg.norm(np_vector)
-        if norm == 0.0:
-            # make sure norm is not 0!
-            raise ValueError("received null vector:", np_vector, norm)
+    elif not dx_positive and dy_positive:
+        quadrant = 1.0
+        angle_measure = -dx
 
-        dx_positive = np_vector[0] >= 0
-        dy_positive = np_vector[1] >= 0
+    elif not dx_positive and not dy_positive:
+        quadrant = 2.0
+        angle_measure = -dy
 
-        if dx_positive and dy_positive:
-            quadrant = 0.0
-            angle_measure = np_vector[1] / norm
+    else:
+        quadrant = 3.0
+        angle_measure = dx
 
-        elif not dx_positive and dy_positive:
-            quadrant = 1.0
-            angle_measure = -np_vector[0] / norm
+    norm = np.linalg.norm(np_vector, ord=2)
+    if norm == 0.0:
+        # make sure norm is not 0!
+        raise ValueError("received null vector:", np_vector, norm)
+    # normalise angle measure to [0; 1]
+    angle_measure /= norm
+    return quadrant + angle_measure
 
-        elif not dx_positive and not dy_positive:
-            quadrant = 2.0
-            angle_measure = -np_vector[1] / norm
 
-        else:
-            quadrant = 3.0
-            angle_measure = np_vector[0] / norm
+class AngleRepresentation(object):
+    # prevent dynamic attribute assignment (-> safe memory)
+    # __slots__ = ['quadrant', 'angle_measure', 'value']
+    __slots__ = ["value"]
 
-        self.value = quadrant + angle_measure
+    def __init__(self, np_vector):
+        self.value = compute_angle_repr_inner(np_vector)
 
     def __str__(self):
         return str(self.value)
@@ -138,6 +146,21 @@ def mark_outdated(self):
         self.is_outdated = True
 
 
+def compute_angle_repr(v1: Vertex, v2: Vertex) -> Optional[float]:
+    diff_vect = v2.coordinates - v1.coordinates
+    if np.all(diff_vect == 0.0):
+        return None
+    return compute_angle_repr_inner(diff_vect)
+
+
+def angle_rep_inverse(repr: Optional[float]) -> Optional[float]:
+    if repr is None:
+        repr_inv = None
+    else:
+        repr_inv = (repr + 2.0) % 4.0
+    return repr_inv
+
+
 class PolygonVertex(Vertex):
     # __slots__ declared in parents are available in child classes. However, child subclasses will get a __dict__
     # and __weakref__ unless they also define __slots__ (which should only contain names of any additional slots).
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 193e2ce..3bf21bf 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -6,7 +6,7 @@
 
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import AngleRepresentation, PolygonVertex
+from extremitypathfinder.helper_classes import PolygonVertex, compute_angle_repr_inner
 
 
 def inside_polygon(x, y, coords, border_value):
@@ -20,7 +20,9 @@ def inside_polygon(x, y, coords, border_value):
     p = np.array([x, y])
     p1 = coords[-1, :]
     for p2 in coords[:]:
-        if abs(AngleRepresentation(p1 - p).value - AngleRepresentation(p2 - p).value) == 2.0:
+        rep_p1_p = compute_angle_repr_inner(p1 - p)
+        rep_p2_p = compute_angle_repr_inner(p2 - p)
+        if abs(rep_p1_p - rep_p2_p) == 2.0:
             return border_value
         p1 = p2
 

From 1bf343cce0ae4af045bd9ce50d0615cc302ca5ee Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 20:22:18 +0200
Subject: [PATCH 07/44] refactor find_within_range

---
 extremitypathfinder/extremitypathfinder.py |  54 ++++-------
 extremitypathfinder/helper_fcts.py         | 101 ++++++++-------------
 2 files changed, 58 insertions(+), 97 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 58b1ed0..a572f7c 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -190,7 +190,7 @@ def prepare(self):  # TODO include in storing functions?
         vertices = list(self.all_vertices)
         nr_vertices = len(vertices)
 
-        # TODO sparse matrix
+        # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
 
         def get_angle_representation(v1: PolygonVertex, v2: PolygonVertex) -> float:
@@ -208,8 +208,8 @@ def get_angle_representation(v1: PolygonVertex, v2: PolygonVertex) -> float:
             assert repr is None or not np.isnan(repr)
             return repr
 
-        def angle_repr_is_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
-            return get_angle_representation(v1, v2) is None
+        def angle_repr_not_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
+            return get_angle_representation(v1, v2) is not None
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
         while len(extremities_to_check) > 0:
@@ -238,22 +238,19 @@ def angle_repr_is_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
             # all vertices between the angle of the two neighbouring edges ('outer side')
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            repr1 = get_angle_representation(query_extremity, n1)
+            n1_repr = get_angle_representation(query_extremity, n1)
             repr1_ = n1.get_angle_representation()
-            if repr1 != pytest.approx(repr1_):
+            if n1_repr != pytest.approx(repr1_):
                 raise ValueError
-            repr2 = get_angle_representation(query_extremity, n2)
-            repr_diff = abs(repr1 - repr2)
-            candidate_extremities.difference_update(
-                find_within_range(
-                    repr1,
-                    repr2,
-                    repr_diff,
-                    candidate_extremities,
-                    angle_range_less_180=True,
-                    equal_repr_allowed=False,
-                )
+            n2_repr = get_angle_representation(query_extremity, n2)
+            candidates_behind_edge = find_within_range(
+                n1_repr,
+                n2_repr,
+                candidate_extremities,
+                angle_range_less_180=True,
+                equal_repr_allowed=False,
             )
+            candidate_extremities.difference_update(candidates_behind_edge)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -267,24 +264,15 @@ def angle_repr_is_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
             # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
             # MUST be added to the graph!
             # Find extremities which fulfill this condition for the given query extremity
-            repr1 = angle_rep_inverse(repr1)
-            repr2 = angle_rep_inverse(repr2)
-            # IMPORTANT: the true angle diff does not change, but the repr diff does! compute again
-            repr_diff = abs(repr1 - repr2)
+            n1_repr = angle_rep_inverse(n1_repr)
+            n2_repr = angle_rep_inverse(n2_repr)
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_candidates = set(
-                filter(
-                    lambda e: e.get_angle_representation() is not None,
-                    self.all_extremities,
-                )
-            )
-            # temp_candidates = {e for e in extremities if not angle_repr_is_none(query_extremity, e)}
+            temp_candidates = {e for e in extremities if angle_repr_not_none(query_extremity, e)}
             lie_in_front = find_within_range(
-                repr1,
-                repr2,
-                repr_diff,
+                n1_repr,
+                n2_repr,
                 temp_candidates,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
@@ -449,16 +437,14 @@ def find_shortest_path(
                 goal_vertex.mark_outdated()
 
                 n1, n2 = vertex.get_neighbours()
-                repr1 = (n1.get_angle_representation() + 2.0) % 4.0  # rotated 180 deg
-                repr2 = (n2.get_angle_representation() + 2.0) % 4.0
-                repr_diff = abs(repr1 - repr2)
+                repr1 = angle_rep_inverse(n1.get_angle_representation())  # rotated 180 deg
+                repr2 = angle_rep_inverse(n2.get_angle_representation())
 
                 # IMPORTANT: special case:
                 # here the nodes must stay connected if they have the same angle representation!
                 lie_in_front = find_within_range(
                     repr1,
                     repr2,
-                    repr_diff,
                     temp_candidates,
                     angle_range_less_180=True,
                     equal_repr_allowed=False,
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 3bf21bf..1392309 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -211,7 +211,7 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
     # TODO data rectification
 
 
-def find_within_range(repr1, repr2, repr_diff, vertex_set, angle_range_less_180, equal_repr_allowed):
+def find_within_range(repr1: float, repr2: float, vertex_set, angle_range_less_180, equal_repr_allowed):
     """
     filters out all vertices whose representation lies within the range between
       the two given angle representations
@@ -219,75 +219,52 @@ def find_within_range(repr1, repr2, repr_diff, vertex_set, angle_range_less_180,
       - query angle (range) is < 180deg or not (>= 180deg)
     :param repr1:
     :param repr2:
-    :param repr_diff: abs(repr1-repr2)
     :param vertex_set:
     :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg
     :param equal_repr_allowed: whether vertices with the same representation should also be returned
     :return:
     """
-
     if len(vertex_set) == 0:
         return vertex_set
 
+    repr_diff = abs(repr1 - repr2)
     if repr_diff == 0.0:
         return set()
 
-    min_repr_val = min(repr1, repr2)
-    max_repr_val = max(repr1, repr2)  # = min_angle + angle_diff
-
-    def lies_within(vertex):
-        # vertices with the same representation will not NOT be returned!
-        return min_repr_val < vertex.get_angle_representation() < max_repr_val
+    min_repr = min(repr1, repr2)
+    max_repr = max(repr1, repr2)  # = min_angle + angle_diff
 
-    def lies_within_eq(vertex):
-        # vertices with the same representation will be returned!
-        return min_repr_val <= vertex.get_angle_representation() <= max_repr_val
+    def repr_within(r):
+        # Note: vertices with the same representation will not NOT be returned!
+        return min_repr < r < max_repr
 
+    # depending on the angle the included range is clockwise or anti-clockwise
+    # (from min_repr to max_val or the other way around)
     # when the range contains the 0.0 value (transition from 3.99... -> 0.0)
     # it is easier to check if a representation does NOT lie within this range
-    # -> filter_fct = not_within
-    def not_within(vertex):
-        # vertices with the same representation will NOT be returned!
-        return not (min_repr_val <= vertex.get_angle_representation() <= max_repr_val)
-
-    def not_within_eq(vertex):
-        # vertices with the same representation will be returned!
-        return not (min_repr_val < vertex.get_angle_representation() < max_repr_val)
-
-    if equal_repr_allowed:
-        lies_within_fct = lies_within_eq
-        not_within_fct = not_within_eq
-    else:
-        lies_within_fct = lies_within
-        not_within_fct = not_within
-
-    if repr_diff < 2.0:
-        # angle < 180 deg
-        if angle_range_less_180:
-            filter_fct = lies_within_fct
-        else:
-            # the actual range to search is from min_val to max_val, but clockwise!
-            filter_fct = not_within_fct
-
-    elif repr_diff == 2.0:
-        # angle == 180deg
-        # which range to filter is determined by the order of the points
-        # since the polygons follow a numbering convention,
-        # the 'left' side of p1-p2 always lies inside the map
-        # -> filter out everything on the right side (='outside')
-        if repr1 < repr2:
-            filter_fct = lies_within_fct
-        else:
-            filter_fct = not_within_fct
+    # -> invert filter condition
+    # special case: angle == 180deg
+    on_line_inv = repr_diff == 2.0 and repr1 >= repr2
+    # which range to filter is determined by the order of the points
+    # since the polygons follow a numbering convention,
+    # the 'left' side of p1-p2 always lies inside the map
+    # -> filter out everything on the right side (='outside')
+    inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180)
+
+    def within_filter_func(r: float) -> bool:
+        repr_eq = r == min_repr or r == max_repr
+        if repr_eq and equal_repr_allowed:
+            return True
+        if repr_eq and not equal_repr_allowed:
+            return False
 
-    else:
-        # angle > 180deg
-        if angle_range_less_180:
-            filter_fct = not_within_fct
-        else:
-            filter_fct = lies_within_fct
+        res = repr_within(r)
+        if inversion_condition:
+            res = not res
+        return res
 
-    return set(filter(filter_fct, vertex_set))
+    vertices_within = {v for v in vertex_set if within_filter_func(v.get_angle_representation())}
+    return vertices_within
 
 
 def convert_gridworld(size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True) -> (list, list):
@@ -516,6 +493,7 @@ def find_visible(vertex_candidates, edges_to_check):
         if lies_on_edge:
             # when the query vertex lies on an edge (or vertex) no behind/in front checks must be performed!
             # the neighbouring edges are visible for sure
+            # attention: only add to visible set if vertex was a candidate!
             try:
                 vertex_candidates.remove(v1)
                 visible_vertices.add(v1)
@@ -529,16 +507,14 @@ def find_visible(vertex_candidates, edges_to_check):
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation should not be deleted, because they can be visible!
-            vertex_candidates.difference_update(
-                find_within_range(
-                    repr1,
-                    repr2,
-                    repr_diff,
-                    vertex_candidates,
-                    angle_range_less_180=range_less_180,
-                    equal_repr_allowed=False,
-                )
+            invisible_candidates = find_within_range(
+                repr1,
+                repr2,
+                vertex_candidates,
+                angle_range_less_180=range_less_180,
+                equal_repr_allowed=False,
             )
+            vertex_candidates.difference_update(invisible_candidates)
             continue
 
         # case: a 'regular' edge
@@ -564,7 +540,6 @@ def find_visible(vertex_candidates, edges_to_check):
         vertices_to_check = find_within_range(
             repr1,
             repr2,
-            repr_diff,
             vertices_to_check,
             angle_range_less_180=True,
             equal_repr_allowed=True,

From 7a67e90a7081928baa29cbc6f3b4dfd163febbe9 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 23:16:13 +0200
Subject: [PATCH 08/44] transition to index based computation

---
 extremitypathfinder/extremitypathfinder.py | 88 ++++++++++++++--------
 extremitypathfinder/helper_classes.py      | 17 +++--
 tests/main_test.py                         |  6 +-
 3 files changed, 74 insertions(+), 37 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index a572f7c..7577b33 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,7 +1,7 @@
 import itertools
 import pickle
 from copy import deepcopy
-from typing import Iterable, Iterator, List, Optional, Tuple
+from typing import Iterable, List, Optional, Set, Tuple
 
 import numpy as np
 
@@ -59,6 +59,7 @@ class PolygonEnvironment:
     graph: DirectedHeuristicGraph = None
     temp_graph: DirectedHeuristicGraph = None  # for storing and plotting the graph during a query
     _all_extremities: Optional[List[PolygonVertex]] = None
+    _all_vertices: Optional[List[PolygonVertex]] = None
 
     @property
     def polygons(self) -> Iterable[Polygon]:
@@ -66,19 +67,26 @@ def polygons(self) -> Iterable[Polygon]:
         yield from self.holes
 
     @property
-    def all_vertices(self) -> Iterator[PolygonVertex]:
+    def all_vertices(self) -> List[PolygonVertex]:
+        if self._all_vertices is None:
+            self._all_vertices = list(itertools.chain(*iter(p.vertices for p in self.polygons)))
+        return self._all_vertices
+
+    @property
+    def extremity_indices(self) -> Set[int]:
+        # TODO refactor
         for p in self.polygons:
-            yield from p.vertices
+            p._find_extremities()
+        # Attention: only consider extremities that are actually within the map
+        return {idx for idx, v in enumerate(self.all_vertices) if v.is_extremity and self.within_map(v.coordinates)}
 
     @property
     def all_extremities(self) -> List[PolygonVertex]:
-        if self._all_extremities is None:
-            poly_extremities = iter(p.extremities for p in self.polygons)
-            extremities_chained = itertools.chain(*poly_extremities)
-            # only consider extremities that are actually within the map
-            extremities = list(filter(lambda e: self.within_map(e.coordinates), extremities_chained))
-            self._all_extremities = extremities
-        return self._all_extremities
+        return [self.all_vertices[i] for i in self.extremity_indices]
+
+        # TODO
+        # if self._all_extremities is None:
+        # return self._all_extremities
 
     @property
     def all_edges(self) -> Iterable[Edge]:
@@ -185,51 +193,65 @@ def prepare(self):  # TODO include in storing functions?
         extremities = self.all_extremities
         self.graph = DirectedHeuristicGraph(extremities)
 
-        extremities_to_check = set(extremities)
+        # extremities_to_check = set(extremities)
 
-        vertices = list(self.all_vertices)
+        vertices = self.all_vertices
         nr_vertices = len(vertices)
+        extremity_indices = self.extremity_indices
+
+        if len(extremity_indices) != len(extremities):
+            raise ValueError
 
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
 
-        def get_angle_representation(v1: PolygonVertex, v2: PolygonVertex) -> float:
-            i1 = vertices.index(v1)
-            i2 = vertices.index(v2)
+        def get_angle_representation(i1: int, i2: int) -> float:
             repr = angle_representations[i1, i2]
 
             # lazy initalisation: compute on demand only
             if np.isnan(repr):  # attention: repr == np.nan does not match!
+                v1 = vertices[i1]
+                v2 = vertices[i2]
                 repr = compute_angle_repr(v1, v2)
                 angle_representations[i1, i2] = repr
+
+                # TODO not required. only triangle required?!
                 # make use of symmetry: rotate 180 deg
                 angle_representations[i2, i1] = angle_rep_inverse(repr)
 
+            # TODO
             assert repr is None or not np.isnan(repr)
             return repr
 
-        def angle_repr_not_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
-            return get_angle_representation(v1, v2) is not None
+        def angle_repr_is_none(i1: int, i2: int) -> bool:
+            return get_angle_representation(i1, i2) is None
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
-        while len(extremities_to_check) > 0:
+        while len(extremity_indices) > 0:
+            # while len(extremities_to_check) > 0:
             # extremities are always visible to each other (bi-directional relation -> undirected graph)
             #  -> do not check extremities which have been checked already
             #  (would only give the same result when algorithms are correct)
             # the extremity itself must not be checked when looking for visible neighbours
-            query_extremity: PolygonVertex = extremities_to_check.pop()
+            # query_extremity: PolygonVertex = extremities_to_check.pop()
+            idx = extremity_indices.pop()
+            query_extremity = vertices[idx]
 
             self.translate(new_origin=query_extremity)
 
             visible_vertices = set()
-            candidate_extremities = extremities_to_check.copy()
+
+            candidate_idxs = extremity_indices.copy()  # independent copy
             # remove the extremities with the same coordinates as the query extremity
-            candidate_extremities.difference_update(
-                {c for c in candidate_extremities if c.get_angle_representation() is None}
-            )
+            # candidates.difference_update(
+            #     {c for c in candidates if get_angle_representation(idx, c) is None}
+            # )
+            candidate_idxs.difference_update({i for i in candidate_idxs if angle_repr_is_none(idx, i)})
 
             # these vertices all belong to a polygon
             n1, n2 = query_extremity.get_neighbours()
+            idx_n1 = vertices.index(n1)
+            idx_n2 = vertices.index(n2)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
 
             # eliminate all vertices 'behind' the query point from the candidate set
@@ -238,19 +260,25 @@ def angle_repr_not_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
             # all vertices between the angle of the two neighbouring edges ('outer side')
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            n1_repr = get_angle_representation(query_extremity, n1)
+            n1_repr = get_angle_representation(idx, idx_n1)
+
+            # TODO
             repr1_ = n1.get_angle_representation()
             if n1_repr != pytest.approx(repr1_):
                 raise ValueError
-            n2_repr = get_angle_representation(query_extremity, n2)
+
+            n2_repr = get_angle_representation(idx, idx_n2)
+
+            # TODO
+            candidates = {vertices[i] for i in candidate_idxs}
             candidates_behind_edge = find_within_range(
                 n1_repr,
                 n2_repr,
-                candidate_extremities,
+                candidates,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            candidate_extremities.difference_update(candidates_behind_edge)
+            candidates.difference_update(candidates_behind_edge)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -269,7 +297,7 @@ def angle_repr_not_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_candidates = {e for e in extremities if angle_repr_not_none(query_extremity, e)}
+            temp_candidates = {vertices[i] for i in extremity_indices if not angle_repr_is_none(idx, i)}
             lie_in_front = find_within_range(
                 n1_repr,
                 n2_repr,
@@ -281,14 +309,14 @@ def angle_repr_not_none(v1: PolygonVertex, v2: PolygonVertex) -> bool:
             # already existing edges in the graph to the extremities in front have to be removed
             self.graph.remove_multiple_undirected_edges(query_extremity, lie_in_front)
             # do not consider when looking for visible extremities (NOTE: they might actually be visible!)
-            candidate_extremities.difference_update(lie_in_front)
+            candidates.difference_update(lie_in_front)
 
             # all edges except the neighbouring edges (handled above!) have to be checked
             edges_to_check = set(self.all_edges)
             edges_to_check.remove(query_extremity.edge1)
             edges_to_check.remove(query_extremity.edge2)
 
-            visible_vertices.update(find_visible(candidate_extremities, edges_to_check))
+            visible_vertices.update(find_visible(candidates, edges_to_check))
             self.graph.add_multiple_undirected_edges(query_extremity, visible_vertices)
 
         self.graph.make_clean()  # join all nodes with the same coordinates
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 9b6c37c..e11af1f 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -243,7 +243,7 @@ def _find_extremities(self):
             holes: clockwise
         :return:
         """
-        self._extremities = []
+        extremities = []
         # extremity_indices = []
         # extremity_index = -1
         v1 = self.vertices[-2]
@@ -252,11 +252,14 @@ def _find_extremities(self):
         p2 = v2.coordinates
 
         for v3 in self.vertices:
-
             p3 = v3.coordinates
             # since consequent vertices are not permitted to be equal,
             #   the angle representation of the difference is well defined
-            if (AngleRepresentation(p3 - p2).value - AngleRepresentation(p1 - p2).value) % 4 < 2.0:
+            diff_p3_p2 = p3 - p2
+            # TODO optimise
+            diff_p1_p2 = p1 - p2
+
+            if (AngleRepresentation(diff_p3_p2).value - AngleRepresentation(diff_p1_p2).value) % 4 < 2.0:
                 # basic idea:
                 #   - translate the coordinate system to have p2 as origin
                 #   - compute the angle representations of both vectors representing the edges
@@ -267,14 +270,18 @@ def _find_extremities(self):
                 #   (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
                 # then p2 = extremity
                 v2.declare_extremity()
-                self._extremities.append(v2)
+                extremities.append(v2)
 
             # move to the next point
             # vertex1=vertex2
-            v2 = v3
+            # TODO optimise
+            diff_p1_p2 = diff_p3_p2
             p1 = p2
+            v2 = v3
             p2 = p3
 
+        self._extremities = extremities
+
     @property
     def extremities(self) -> List[PolygonVertex]:
         if self._extremities is None:
diff --git a/tests/main_test.py b/tests/main_test.py
index 25f53e4..43c4fcf 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -344,9 +344,11 @@ def test_grid_env(self):
         grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
 
         grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False)
-        assert len(list(grid_env.all_extremities)) == 17, "extremities do not get detected correctly!"
+        nr_extremities = len(list(grid_env.all_extremities))
+        assert nr_extremities == 17, "extremities do not get detected correctly!"
         grid_env.prepare()
-        assert len(grid_env.graph.all_nodes) == 16, "identical nodes should get joined in the graph!"
+        nr_graph_nodes = len(grid_env.graph.all_nodes)
+        assert nr_graph_nodes == 16, "identical nodes should get joined in the graph!"
 
         # test if points outside the map are being rejected
         for start_coordinates, goal_coordinates in INVALID_DESTINATION_DATA:

From ae92a151c3b5b11aec5d21f9ba09da8eba1044dd Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Tue, 16 Aug 2022 23:34:54 +0200
Subject: [PATCH 09/44] factor out get repr func

---
 extremitypathfinder/extremitypathfinder.py | 36 ++++++----------------
 extremitypathfinder/helper_fcts.py         | 26 +++++++++++++++-
 2 files changed, 34 insertions(+), 28 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 7577b33..2cfc221 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -23,13 +23,13 @@
     PolygonVertex,
     Vertex,
     angle_rep_inverse,
-    compute_angle_repr,
 )
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
     convert_gridworld,
     find_visible,
     find_within_range,
+    get_angle_representation,
     inside_polygon,
 )
 
@@ -205,26 +205,8 @@ def prepare(self):  # TODO include in storing functions?
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
 
-        def get_angle_representation(i1: int, i2: int) -> float:
-            repr = angle_representations[i1, i2]
-
-            # lazy initalisation: compute on demand only
-            if np.isnan(repr):  # attention: repr == np.nan does not match!
-                v1 = vertices[i1]
-                v2 = vertices[i2]
-                repr = compute_angle_repr(v1, v2)
-                angle_representations[i1, i2] = repr
-
-                # TODO not required. only triangle required?!
-                # make use of symmetry: rotate 180 deg
-                angle_representations[i2, i1] = angle_rep_inverse(repr)
-
-            # TODO
-            assert repr is None or not np.isnan(repr)
-            return repr
-
-        def angle_repr_is_none(i1: int, i2: int) -> bool:
-            return get_angle_representation(i1, i2) is None
+        def get_repr(i1, i2):
+            return get_angle_representation(i1, i2, angle_representations, vertices)
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
         while len(extremity_indices) > 0:
@@ -241,12 +223,12 @@ def angle_repr_is_none(i1: int, i2: int) -> bool:
 
             visible_vertices = set()
 
-            candidate_idxs = extremity_indices.copy()  # independent copy
-            # remove the extremities with the same coordinates as the query extremity
+            # only consider extremities with coordinates different from the query extremity
+            # (angle representation not None)
+            candidate_idxs = {i for i in extremity_indices if get_repr(idx, i) is not None}
             # candidates.difference_update(
             #     {c for c in candidates if get_angle_representation(idx, c) is None}
             # )
-            candidate_idxs.difference_update({i for i in candidate_idxs if angle_repr_is_none(idx, i)})
 
             # these vertices all belong to a polygon
             n1, n2 = query_extremity.get_neighbours()
@@ -260,14 +242,14 @@ def angle_repr_is_none(i1: int, i2: int) -> bool:
             # all vertices between the angle of the two neighbouring edges ('outer side')
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            n1_repr = get_angle_representation(idx, idx_n1)
+            n1_repr = get_repr(idx, idx_n1)
 
             # TODO
             repr1_ = n1.get_angle_representation()
             if n1_repr != pytest.approx(repr1_):
                 raise ValueError
 
-            n2_repr = get_angle_representation(idx, idx_n2)
+            n2_repr = get_repr(idx, idx_n2)
 
             # TODO
             candidates = {vertices[i] for i in candidate_idxs}
@@ -297,7 +279,7 @@ def angle_repr_is_none(i1: int, i2: int) -> bool:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_candidates = {vertices[i] for i in extremity_indices if not angle_repr_is_none(idx, i)}
+            temp_candidates = {vertices[i] for i in extremity_indices if get_repr(idx, i) is not None}
             lie_in_front = find_within_range(
                 n1_repr,
                 n2_repr,
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 1392309..8ce6b5b 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -6,7 +6,12 @@
 
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import PolygonVertex, compute_angle_repr_inner
+from extremitypathfinder.helper_classes import (
+    PolygonVertex,
+    angle_rep_inverse,
+    compute_angle_repr,
+    compute_angle_repr_inner,
+)
 
 
 def inside_polygon(x, y, coords, border_value):
@@ -211,6 +216,25 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
     # TODO data rectification
 
 
+def get_angle_representation(i1: int, i2: int, repr_matrix: np.ndarray, vertices: List["PolygonVertex"]) -> float:
+    repr = repr_matrix[i1, i2]
+
+    # lazy initalisation: compute on demand only
+    if np.isnan(repr):  # attention: repr == np.nan does not match!
+        v1 = vertices[i1]
+        v2 = vertices[i2]
+        repr = compute_angle_repr(v1, v2)
+        repr_matrix[i1, i2] = repr
+
+        # TODO not required. only triangle required?!
+        # make use of symmetry: rotate 180 deg
+        repr_matrix[i2, i1] = angle_rep_inverse(repr)
+
+    # TODO
+    assert repr is None or not np.isnan(repr)
+    return repr
+
+
 def find_within_range(repr1: float, repr2: float, vertex_set, angle_range_less_180, equal_repr_allowed):
     """
     filters out all vertices whose representation lies within the range between

From a89f161721496dad43c89d3c417aceed9249028e Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 00:00:09 +0200
Subject: [PATCH 10/44] transition to idx 2

---
 extremitypathfinder/extremitypathfinder.py | 40 +++++++++----
 extremitypathfinder/helper_fcts.py         | 70 +++++++++++++++++++++-
 2 files changed, 99 insertions(+), 11 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 2cfc221..f36b307 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -29,6 +29,7 @@
     convert_gridworld,
     find_visible,
     find_within_range,
+    find_within_range2,
     get_angle_representation,
     inside_polygon,
 )
@@ -252,15 +253,27 @@ def get_repr(i1, i2):
             n2_repr = get_repr(idx, idx_n2)
 
             # TODO
-            candidates = {vertices[i] for i in candidate_idxs}
-            candidates_behind_edge = find_within_range(
+            idx_behind = find_within_range2(
                 n1_repr,
                 n2_repr,
-                candidates,
+                candidate_idxs,
+                angle_representations,
+                vertices,
+                idx,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            candidates.difference_update(candidates_behind_edge)
+            candidate_idxs.difference_update(idx_behind)
+
+            candidates = {vertices[i] for i in candidate_idxs}
+            # candidates_behind_edge = find_within_range(
+            #     n1_repr,
+            #     n2_repr,
+            #     candidates,
+            #     angle_range_less_180=True,
+            #     equal_repr_allowed=False,
+            # )
+            # candidates.difference_update(candidates_behind_edge)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -279,25 +292,32 @@ def get_repr(i1, i2):
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_candidates = {vertices[i] for i in extremity_indices if get_repr(idx, i) is not None}
-            lie_in_front = find_within_range(
+            temp_idxs = {i for i in extremity_indices if get_repr(idx, i) is not None}
+            lie_in_front_idx = find_within_range2(
                 n1_repr,
                 n2_repr,
-                temp_candidates,
+                temp_idxs,
+                angle_representations,
+                vertices,
+                idx,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
             # "thin out" the graph -> optimisation
             # already existing edges in the graph to the extremities in front have to be removed
+            # TODO graph: do not use vertices
+            lie_in_front = {vertices[i] for i in lie_in_front_idx}
             self.graph.remove_multiple_undirected_edges(query_extremity, lie_in_front)
-            # do not consider when looking for visible extremities (NOTE: they might actually be visible!)
-            candidates.difference_update(lie_in_front)
+            # do not consider when looking for visible extremities, even if they are actually be visible
+            candidate_idxs.difference_update(idx_behind)
 
             # all edges except the neighbouring edges (handled above!) have to be checked
             edges_to_check = set(self.all_edges)
             edges_to_check.remove(query_extremity.edge1)
             edges_to_check.remove(query_extremity.edge2)
 
+            # TODO graph: do not use vertices
+            candidates = {vertices[i] for i in candidate_idxs}
             visible_vertices.update(find_visible(candidates, edges_to_check))
             self.graph.add_multiple_undirected_edges(query_extremity, visible_vertices)
 
@@ -310,7 +330,7 @@ def within_map(self, coords: INPUT_COORD_TYPE):
         :param coords: numerical tuple representing coordinates
         :return: whether the given coordinate is a valid query point
         """
-        #
+
         x, y = coords
         if not inside_polygon(x, y, self.boundary_polygon.coordinates, border_value=True):
             return False
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 8ce6b5b..b756e66 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,6 +1,6 @@
 import json
 from itertools import combinations
-from typing import List
+from typing import List, Set
 
 import numpy as np
 
@@ -291,6 +291,74 @@ def within_filter_func(r: float) -> bool:
     return vertices_within
 
 
+def find_within_range2(
+    repr1: float,
+    repr2: float,
+    candidate_idx: Set[int],
+    angle_representations: np.ndarray,
+    vertices: List["PolygonVertex"],
+    origin_idx: int,
+    angle_range_less_180: bool,
+    equal_repr_allowed: bool,
+) -> Set[int]:
+    """
+    filters out all vertices whose representation lies within the range between
+      the two given angle representations
+    which range ('clockwise' or 'counter-clockwise') should be checked is determined by:
+      - query angle (range) is < 180deg or not (>= 180deg)
+    :param repr1:
+    :param repr2:
+    :param representations:
+    :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg
+    :param equal_repr_allowed: whether vertices with the same representation should also be returned
+    :return:
+    """
+    if len(candidate_idx) == 0:
+        return set()
+
+    repr_diff = abs(repr1 - repr2)
+    if repr_diff == 0.0:
+        return set()
+
+    min_repr = min(repr1, repr2)
+    max_repr = max(repr1, repr2)  # = min_angle + angle_diff
+
+    def repr_within(r):
+        # Note: vertices with the same representation will not NOT be returned!
+        return min_repr < r < max_repr
+
+    # depending on the angle the included range is clockwise or anti-clockwise
+    # (from min_repr to max_val or the other way around)
+    # when the range contains the 0.0 value (transition from 3.99... -> 0.0)
+    # it is easier to check if a representation does NOT lie within this range
+    # -> invert filter condition
+    # special case: angle == 180deg
+    on_line_inv = repr_diff == 2.0 and repr1 >= repr2
+    # which range to filter is determined by the order of the points
+    # since the polygons follow a numbering convention,
+    # the 'left' side of p1-p2 always lies inside the map
+    # -> filter out everything on the right side (='outside')
+    inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180)
+
+    def within_filter_func(r: float) -> bool:
+        repr_eq = r == min_repr or r == max_repr
+        if repr_eq and equal_repr_allowed:
+            return True
+        if repr_eq and not equal_repr_allowed:
+            return False
+
+        res = repr_within(r)
+        if inversion_condition:
+            res = not res
+        return res
+
+    def get_repr(i1, i2):
+        return get_angle_representation(i1, i2, angle_representations, vertices)
+
+    idx_within = {idx for idx in candidate_idx if within_filter_func(get_repr(origin_idx, idx))}
+    return idx_within
+
+
 def convert_gridworld(size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True) -> (list, list):
     """
     prerequisites: grid world must not have non-obstacle cells which are surrounded by obstacles

From 59bca7965cf95a3fd3d236de40a0ec1f3cc6e3cc Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 01:41:20 +0200
Subject: [PATCH 11/44] index based find_visible

---
 extremitypathfinder/extremitypathfinder.py |  34 ++---
 extremitypathfinder/helper_fcts.py         | 169 +++++++++++++++++++++
 2 files changed, 180 insertions(+), 23 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index f36b307..0627e2b 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -28,6 +28,7 @@
     check_data_requirements,
     convert_gridworld,
     find_visible,
+    find_visible2,
     find_within_range,
     find_within_range2,
     get_angle_representation,
@@ -90,9 +91,8 @@ def all_extremities(self) -> List[PolygonVertex]:
         # return self._all_extremities
 
     @property
-    def all_edges(self) -> Iterable[Edge]:
-        for p in self.polygons:
-            yield from p.edges
+    def all_edges(self) -> Set[Edge]:
+        return set(itertools.chain(*iter(p.edges for p in self.polygons)))
 
     def store(
         self,
@@ -222,8 +222,6 @@ def get_repr(i1, i2):
 
             self.translate(new_origin=query_extremity)
 
-            visible_vertices = set()
-
             # only consider extremities with coordinates different from the query extremity
             # (angle representation not None)
             candidate_idxs = {i for i in extremity_indices if get_repr(idx, i) is not None}
@@ -265,16 +263,6 @@ def get_repr(i1, i2):
             )
             candidate_idxs.difference_update(idx_behind)
 
-            candidates = {vertices[i] for i in candidate_idxs}
-            # candidates_behind_edge = find_within_range(
-            #     n1_repr,
-            #     n2_repr,
-            #     candidates,
-            #     angle_range_less_180=True,
-            #     equal_repr_allowed=False,
-            # )
-            # candidates.difference_update(candidates_behind_edge)
-
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
             # reachable on a shorter path without e (except e itself).
@@ -305,20 +293,17 @@ def get_repr(i1, i2):
             )
             # "thin out" the graph -> optimisation
             # already existing edges in the graph to the extremities in front have to be removed
-            # TODO graph: do not use vertices
+            # TODO graph: also use indices instead of vertices
             lie_in_front = {vertices[i] for i in lie_in_front_idx}
             self.graph.remove_multiple_undirected_edges(query_extremity, lie_in_front)
             # do not consider when looking for visible extremities, even if they are actually be visible
             candidate_idxs.difference_update(idx_behind)
 
             # all edges except the neighbouring edges (handled above!) have to be checked
-            edges_to_check = set(self.all_edges)
+            edges_to_check = self.all_edges
             edges_to_check.remove(query_extremity.edge1)
             edges_to_check.remove(query_extremity.edge2)
-
-            # TODO graph: do not use vertices
-            candidates = {vertices[i] for i in candidate_idxs}
-            visible_vertices.update(find_visible(candidates, edges_to_check))
+            visible_vertices = find_visible2(candidate_idxs, angle_representations, vertices, idx, edges_to_check)
             self.graph.add_multiple_undirected_edges(query_extremity, visible_vertices)
 
         self.graph.make_clean()  # join all nodes with the same coordinates
@@ -399,7 +384,8 @@ def find_shortest_path(
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
         candidates.add(start_vertex)
 
-        visibles_n_distances_goal = find_visible(candidates, edges_to_check=set(self.all_edges))
+        # TODO use new variant
+        visibles_n_distances_goal = find_visible(candidates, edges_to_check=self.all_edges)
         if len(visibles_n_distances_goal) == 0:
             # The goal node does not have any neighbours. Hence there is not possible path to the goal.
             return [], None
@@ -430,7 +416,9 @@ def find_shortest_path(
                 self.graph.get_all_nodes(),
             )
         )
-        visibles_n_distances_start = find_visible(candidates, edges_to_check=set(self.all_edges))
+
+        # TODO use new variant
+        visibles_n_distances_start = find_visible(candidates, edges_to_check=self.all_edges)
         if len(visibles_n_distances_start) == 0:
             # The start node does not have any neighbours. Hence there is not possible path to the goal.
             return [], None
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index b756e66..acdf8ec 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -690,6 +690,175 @@ def find_visible(vertex_candidates, edges_to_check):
     return {(e, e.get_distance_to_origin()) for e in visible_vertices}
 
 
+def find_visible2(
+    candidate_idxs: Set[int],
+    angle_representations: np.ndarray,
+    vertices: List["PolygonVertex"],
+    origin_idx: int,
+    edges_to_check: Set,
+):
+    """
+    query_vertex: a vertex for which the visibility to the vertices should be checked.
+        also non extremity vertices, polygon vertices and vertices with the same coordinates are allowed.
+        query point also might lie directly on an edge! (angle = 180deg)
+    :param candidate_idxs: the set of all vertex ids which should be checked for visibility.
+        IMPORTANT: is being manipulated, so has to be a copy!
+        IMPORTANT: must not contain the query vertex!
+    :param edges_to_check: the set of edges which determine visibility
+    :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
+    """
+    visible_idxs = set()
+    if len(candidate_idxs) == 0:
+        return visible_idxs
+
+    origin_vertex = vertices[origin_idx]
+
+    # TODO use
+    def get_distance_to_origin(v: "PolygonVertex") -> float:
+        return np.linalg.norm(v.coordinates - origin_vertex.coordinates, ord=2)
+
+    # goal: eliminating all vertices lying 'behind' any edge
+    while len(edges_to_check) > 0 and len(candidate_idxs) > 0:
+        edge = edges_to_check.pop()
+
+        lies_on_edge = False
+        range_less_180 = False
+
+        # TODO use indices only
+        v1, v2 = edge.vertex1, edge.vertex2
+        idx_v1 = vertices.index(v1)
+        idx_v2 = vertices.index(v2)
+
+        if get_distance_to_origin(v1) == 0.0:
+            # vertex1 has the same coordinates as the query vertex -> on the edge
+            lies_on_edge = True
+            # (but does not belong to the same polygon, not identical!)
+            # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
+            candidate_idxs.discard(idx_v1)
+
+            # its angle representation is not defined (no line segment from vertex1 to query vertex!)
+            range_less_180 = v1.is_extremity
+            # do not check the other neighbouring edge of vertex1 in the future
+            e1 = v1.edge1
+            edges_to_check.discard(e1)
+            # everything between its two neighbouring edges is not visible for sure
+            v1, v2 = v1.get_neighbours()
+            idx_v1 = vertices.index(v1)
+            idx_v2 = vertices.index(v2)
+
+        elif get_distance_to_origin(v2) == 0.0:
+            lies_on_edge = True
+            candidate_idxs.discard(idx_v2)
+            range_less_180 = v2.is_extremity
+            e1 = v2.edge2
+            edges_to_check.discard(e1)
+            v1, v2 = v2.get_neighbours()
+            idx_v1 = vertices.index(v1)
+            idx_v2 = vertices.index(v2)
+
+        def get_repr(i2):
+            return get_angle_representation(origin_idx, i2, angle_representations, vertices)
+
+        repr1 = get_repr(idx_v1)
+        repr2 = get_repr(idx_v2)
+
+        repr_diff = abs(repr1 - repr2)
+
+        if repr_diff == 2.0:
+            # angle == 180deg -> on the edge
+            lies_on_edge = True
+
+        if lies_on_edge:
+            # when the query vertex lies on an edge (or vertex) no behind/in front checks must be performed!
+            # the neighbouring edges are visible for sure
+            # attention: only add to visible set if vertex was a candidate!
+            try:
+                candidate_idxs.remove(idx_v1)
+                visible_idxs.add(idx_v1)
+            except KeyError:
+                pass
+            try:
+                candidate_idxs.remove(idx_v2)
+                visible_idxs.add(idx_v2)
+            except KeyError:
+                pass
+
+            # all the candidates between the two vertices v1 v2 are not visible for sure
+            # candidates with the same representation should not be deleted, because they can be visible!
+            invisible_candidate_idxs = find_within_range2(
+                repr1,
+                repr2,
+                candidate_idxs,
+                angle_representations,
+                vertices,
+                origin_idx,
+                angle_range_less_180=range_less_180,
+                equal_repr_allowed=False,
+            )
+            candidate_idxs.difference_update(invisible_candidate_idxs)
+            continue
+
+        # case: a 'regular' edge
+        # eliminate all candidates which are blocked by the edge
+        # that means inside the angle range spanned by the edge and actually behind it
+        idxs2check = candidate_idxs.copy()
+        # the vertices belonging to the edge itself (its vertices) must not be checked.
+        # use discard() instead of remove() to not raise an error (they might not be candidates)
+        idxs2check.discard(idx_v1)
+        idxs2check.discard(idx_v2)
+
+        # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge)
+        #   within this angle range
+        # the "view range" of an edge from a query point (spanned by the two vertices of the edge)
+        #   is always < 180deg when the edge is not running through the query point (=180 deg)
+        #  candidates with the same representation as v1 or v2 should be considered.
+        #   they can be visible, but should be ruled out if they lie behind any edge!
+        idxs2check = find_within_range2(
+            repr1,
+            repr2,
+            idxs2check,
+            angle_representations,
+            vertices,
+            origin_idx,
+            angle_range_less_180=True,
+            equal_repr_allowed=True,
+        )
+
+        # if a candidate is farther away from the query point than both vertices of the edge,
+        #   it surely lies behind the edge
+        # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
+        #   it still needs to be checked!
+        # TODO use only indices
+        v1_dist = get_distance_to_origin(v1)
+        v2_dist = get_distance_to_origin(v2)
+        max_distance = max(v1_dist, v2_dist)
+        idxs_behind = set()
+        # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
+        #    has an intersection with the current edge p1---p2
+        p1 = v1.get_coordinates_translated()
+        p2 = v2.get_coordinates_translated()
+        for idx in idxs2check:
+            v = vertices[idx]
+            idx = vertices.index(v)
+            further_away = get_distance_to_origin(v) > max_distance
+            v_coords = v.get_coordinates_translated()
+            if further_away or lies_behind(p1, p2, v_coords):
+                idxs_behind.add(idx)
+            # vertex lies in front of this edge
+
+        # vertices behind any edge are not visible
+        candidate_idxs.difference_update(idxs_behind)
+
+    # all edges have been checked
+    # all remaining vertices were not concealed behind any edge and hence are visible
+
+    visible_idxs.update(candidate_idxs)
+    visible_vertices = {vertices[i] for i in visible_idxs}
+
+    # return a set of tuples: (vertex, distance)
+    return {(e, e.get_distance_to_origin()) for e in visible_vertices}
+
+
 def try_extraction(json_data, key):
     try:
         extracted_data = json_data[key]

From c8a2c827f2134b7e539452b85359508077669552 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 02:25:46 +0200
Subject: [PATCH 12/44] coords and edge indices

---
 extremitypathfinder/extremitypathfinder.py | 60 ++++++++++-------
 extremitypathfinder/helper_classes.py      |  4 +-
 extremitypathfinder/helper_fcts.py         | 75 +++++++++++++---------
 3 files changed, 81 insertions(+), 58 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 0627e2b..ea36d12 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -5,9 +5,6 @@
 
 import numpy as np
 
-# TODO possible to allow polygon consisting of 2 vertices only(=barrier)? lots of functions need at least 3 vertices atm
-import pytest
-
 from extremitypathfinder.global_settings import (
     DEFAULT_PICKLE_NAME,
     INPUT_COORD_LIST_TYPE,
@@ -35,6 +32,8 @@
     inside_polygon,
 )
 
+# TODO possible to allow polygon consisting of 2 vertices only(=barrier)? lots of functions need at least 3 vertices atm
+
 
 # is not a helper function to make it an importable part of the package
 def load_pickle(path=DEFAULT_PICKLE_NAME):
@@ -182,7 +181,6 @@ def prepare(self):  # TODO include in storing functions?
             and storing them in the graph would not be an advantage, because then the graph is fully connected.
             A star would visit every node in the graph at least once (-> disadvantage!).
         """
-
         if self.prepared:
             raise ValueError("this environment is already prepared. load new polygons first.")
 
@@ -193,21 +191,29 @@ def prepare(self):  # TODO include in storing functions?
         #   even if a node has no edges (visibility to other extremities), it should still be included!
         extremities = self.all_extremities
         self.graph = DirectedHeuristicGraph(extremities)
+        nr_extremities = len(extremities)
+        if nr_extremities == 0:
+            return
 
-        # extremities_to_check = set(extremities)
+            # extremities_to_check = set(extremities)
 
         vertices = self.all_vertices
         nr_vertices = len(vertices)
         extremity_indices = self.extremity_indices
+        coordinates = np.stack([v.coordinates for v in vertices])
 
-        if len(extremity_indices) != len(extremities):
-            raise ValueError
-
+        # TODO more performant way of computing
+        edges = list(self.all_edges)
+        edge_idxs = np.stack([(edges.index(e.edge1), edges.index(e.edge2)) for e in extremities])
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
+        neighbour_idxs = np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in edges])
+
+        if len(extremity_indices) != len(extremities):
+            raise ValueError
 
         def get_repr(i1, i2):
-            return get_angle_representation(i1, i2, angle_representations, vertices)
+            return get_angle_representation(i1, i2, angle_representations, coordinates)
 
         # have to run for all (also last one!), because existing edges might get deleted every loop
         while len(extremity_indices) > 0:
@@ -217,14 +223,14 @@ def get_repr(i1, i2):
             #  (would only give the same result when algorithms are correct)
             # the extremity itself must not be checked when looking for visible neighbours
             # query_extremity: PolygonVertex = extremities_to_check.pop()
-            idx = extremity_indices.pop()
-            query_extremity = vertices[idx]
+            origin_idx = extremity_indices.pop()
+            query_extremity = vertices[origin_idx]
 
-            self.translate(new_origin=query_extremity)
+            # self.translate(new_origin=query_extremity)
 
             # only consider extremities with coordinates different from the query extremity
             # (angle representation not None)
-            candidate_idxs = {i for i in extremity_indices if get_repr(idx, i) is not None}
+            candidate_idxs = {i for i in extremity_indices if get_repr(origin_idx, i) is not None}
             # candidates.difference_update(
             #     {c for c in candidates if get_angle_representation(idx, c) is None}
             # )
@@ -241,14 +247,11 @@ def get_repr(i1, i2):
             # all vertices between the angle of the two neighbouring edges ('outer side')
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            n1_repr = get_repr(idx, idx_n1)
+            n1_repr = get_repr(origin_idx, idx_n1)
 
             # TODO
-            repr1_ = n1.get_angle_representation()
-            if n1_repr != pytest.approx(repr1_):
-                raise ValueError
 
-            n2_repr = get_repr(idx, idx_n2)
+            n2_repr = get_repr(origin_idx, idx_n2)
 
             # TODO
             idx_behind = find_within_range2(
@@ -256,8 +259,8 @@ def get_repr(i1, i2):
                 n2_repr,
                 candidate_idxs,
                 angle_representations,
-                vertices,
-                idx,
+                coordinates,
+                origin_idx,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
@@ -280,14 +283,14 @@ def get_repr(i1, i2):
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_idxs = {i for i in extremity_indices if get_repr(idx, i) is not None}
+            temp_idxs = {i for i in extremity_indices if get_repr(origin_idx, i) is not None}
             lie_in_front_idx = find_within_range2(
                 n1_repr,
                 n2_repr,
                 temp_idxs,
                 angle_representations,
-                vertices,
-                idx,
+                coordinates,
+                origin_idx,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
@@ -303,7 +306,16 @@ def get_repr(i1, i2):
             edges_to_check = self.all_edges
             edges_to_check.remove(query_extremity.edge1)
             edges_to_check.remove(query_extremity.edge2)
-            visible_vertices = find_visible2(candidate_idxs, angle_representations, vertices, idx, edges_to_check)
+            visible_vertices = find_visible2(
+                candidate_idxs,
+                angle_representations,
+                vertices,
+                coordinates,
+                edge_idxs,
+                neighbour_idxs,
+                origin_idx,
+                edges_to_check,
+            )
             self.graph.add_multiple_undirected_edges(query_extremity, visible_vertices)
 
         self.graph.make_clean()  # join all nodes with the same coordinates
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index e11af1f..f1b9ff1 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -146,8 +146,8 @@ def mark_outdated(self):
         self.is_outdated = True
 
 
-def compute_angle_repr(v1: Vertex, v2: Vertex) -> Optional[float]:
-    diff_vect = v2.coordinates - v1.coordinates
+def compute_angle_repr(coords_v1: np.ndarray, coords_v2: np.ndarray) -> Optional[float]:
+    diff_vect = coords_v2 - coords_v1
     if np.all(diff_vect == 0.0):
         return None
     return compute_angle_repr_inner(diff_vect)
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index acdf8ec..2a92e2a 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -5,6 +5,8 @@
 import numpy as np
 
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
+import numpy.linalg
+
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
 from extremitypathfinder.helper_classes import (
     PolygonVertex,
@@ -216,19 +218,19 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
     # TODO data rectification
 
 
-def get_angle_representation(i1: int, i2: int, repr_matrix: np.ndarray, vertices: List["PolygonVertex"]) -> float:
-    repr = repr_matrix[i1, i2]
+def get_angle_representation(idx_origin: int, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
+    repr = repr_matrix[idx_origin, idx_v]
 
     # lazy initalisation: compute on demand only
     if np.isnan(repr):  # attention: repr == np.nan does not match!
-        v1 = vertices[i1]
-        v2 = vertices[i2]
-        repr = compute_angle_repr(v1, v2)
-        repr_matrix[i1, i2] = repr
+        coords_origin = coordinates[idx_origin]
+        coords_v = coordinates[idx_v]
+        repr = compute_angle_repr(coords_origin, coords_v)
+        repr_matrix[idx_origin, idx_v] = repr
 
         # TODO not required. only triangle required?!
         # make use of symmetry: rotate 180 deg
-        repr_matrix[i2, i1] = angle_rep_inverse(repr)
+        repr_matrix[idx_v, idx_origin] = angle_rep_inverse(repr)
 
     # TODO
     assert repr is None or not np.isnan(repr)
@@ -296,7 +298,7 @@ def find_within_range2(
     repr2: float,
     candidate_idx: Set[int],
     angle_representations: np.ndarray,
-    vertices: List["PolygonVertex"],
+    coords: np.ndarray,
     origin_idx: int,
     angle_range_less_180: bool,
     equal_repr_allowed: bool,
@@ -353,7 +355,7 @@ def within_filter_func(r: float) -> bool:
         return res
 
     def get_repr(i1, i2):
-        return get_angle_representation(i1, i2, angle_representations, vertices)
+        return get_angle_representation(i1, i2, angle_representations, coords)
 
     idx_within = {idx for idx in candidate_idx if within_filter_func(get_repr(origin_idx, idx))}
     return idx_within
@@ -694,6 +696,9 @@ def find_visible2(
     candidate_idxs: Set[int],
     angle_representations: np.ndarray,
     vertices: List["PolygonVertex"],
+    coords: np.ndarray,
+    edge_idxs: np.ndarray,
+    neighbour_idxs: np.ndarray,
     origin_idx: int,
     edges_to_check: Set,
 ):
@@ -711,11 +716,21 @@ def find_visible2(
     if len(candidate_idxs) == 0:
         return visible_idxs
 
-    origin_vertex = vertices[origin_idx]
+    coords_origin = coords[origin_idx]
+
+    # nr_edges = neighbour_idxs.shape[0]
+    # edge_idxs2check = set(range(nr_edges))
+
+    def get_coordinates_translated(i: int) -> np.ndarray:
+        coords_v = coords[i]
+        return coords_v - coords_origin
+
+    def get_distance_to_origin(i: int) -> float:
+        coords = get_coordinates_translated(i)
+        return np.linalg.norm(coords, ord=2)
 
-    # TODO use
-    def get_distance_to_origin(v: "PolygonVertex") -> float:
-        return np.linalg.norm(v.coordinates - origin_vertex.coordinates, ord=2)
+    def get_repr(i: int) -> float:
+        return get_angle_representation(origin_idx, i, angle_representations, coords)
 
     # goal: eliminating all vertices lying 'behind' any edge
     while len(edges_to_check) > 0 and len(candidate_idxs) > 0:
@@ -729,7 +744,9 @@ def get_distance_to_origin(v: "PolygonVertex") -> float:
         idx_v1 = vertices.index(v1)
         idx_v2 = vertices.index(v2)
 
-        if get_distance_to_origin(v1) == 0.0:
+        # idx_v1, idx_v2 = neighbour_idxs[edge_idx]
+
+        if get_distance_to_origin(idx_v1) == 0.0:
             # vertex1 has the same coordinates as the query vertex -> on the edge
             lies_on_edge = True
             # (but does not belong to the same polygon, not identical!)
@@ -746,7 +763,7 @@ def get_distance_to_origin(v: "PolygonVertex") -> float:
             idx_v1 = vertices.index(v1)
             idx_v2 = vertices.index(v2)
 
-        elif get_distance_to_origin(v2) == 0.0:
+        elif get_distance_to_origin(idx_v2) == 0.0:
             lies_on_edge = True
             candidate_idxs.discard(idx_v2)
             range_less_180 = v2.is_extremity
@@ -756,9 +773,6 @@ def get_distance_to_origin(v: "PolygonVertex") -> float:
             idx_v1 = vertices.index(v1)
             idx_v2 = vertices.index(v2)
 
-        def get_repr(i2):
-            return get_angle_representation(origin_idx, i2, angle_representations, vertices)
-
         repr1 = get_repr(idx_v1)
         repr2 = get_repr(idx_v2)
 
@@ -790,7 +804,7 @@ def get_repr(i2):
                 repr2,
                 candidate_idxs,
                 angle_representations,
-                vertices,
+                coords,
                 origin_idx,
                 angle_range_less_180=range_less_180,
                 equal_repr_allowed=False,
@@ -818,7 +832,7 @@ def get_repr(i2):
             repr2,
             idxs2check,
             angle_representations,
-            vertices,
+            coords,
             origin_idx,
             angle_range_less_180=True,
             equal_repr_allowed=True,
@@ -828,21 +842,19 @@ def get_repr(i2):
         #   it surely lies behind the edge
         # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
         #   it still needs to be checked!
-        # TODO use only indices
-        v1_dist = get_distance_to_origin(v1)
-        v2_dist = get_distance_to_origin(v2)
+        v1_dist = get_distance_to_origin(idx_v1)
+        v2_dist = get_distance_to_origin(idx_v2)
         max_distance = max(v1_dist, v2_dist)
         idxs_behind = set()
         # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
         #    has an intersection with the current edge p1---p2
-        p1 = v1.get_coordinates_translated()
-        p2 = v2.get_coordinates_translated()
+        p1 = get_coordinates_translated(idx_v1)
+        p2 = get_coordinates_translated(idx_v2)
         for idx in idxs2check:
-            v = vertices[idx]
-            idx = vertices.index(v)
-            further_away = get_distance_to_origin(v) > max_distance
-            v_coords = v.get_coordinates_translated()
-            if further_away or lies_behind(p1, p2, v_coords):
+            if get_repr(idx) is None:
+                continue
+            further_away = get_distance_to_origin(idx) > max_distance
+            if further_away or lies_behind(p1, p2, get_coordinates_translated(idx)):
                 idxs_behind.add(idx)
             # vertex lies in front of this edge
 
@@ -853,10 +865,9 @@ def get_repr(i2):
     # all remaining vertices were not concealed behind any edge and hence are visible
 
     visible_idxs.update(candidate_idxs)
-    visible_vertices = {vertices[i] for i in visible_idxs}
 
     # return a set of tuples: (vertex, distance)
-    return {(e, e.get_distance_to_origin()) for e in visible_vertices}
+    return {(vertices[i], get_distance_to_origin(i)) for i in visible_idxs}
 
 
 def try_extraction(json_data, key):

From 2bd90574f115e60e0b53b56b0b3e2d966d4737ef Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 02:50:27 +0200
Subject: [PATCH 13/44] prepare edge index only

---
 extremitypathfinder/extremitypathfinder.py |  3 ++-
 extremitypathfinder/helper_fcts.py         | 19 +++++++++++++------
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index ea36d12..e37e674 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -204,7 +204,8 @@ def prepare(self):  # TODO include in storing functions?
 
         # TODO more performant way of computing
         edges = list(self.all_edges)
-        edge_idxs = np.stack([(edges.index(e.edge1), edges.index(e.edge2)) for e in extremities])
+        # TODO better name
+        edge_idxs = np.stack([(edges.index(v.edge1), edges.index(v.edge2)) for v in vertices])
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
         neighbour_idxs = np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in edges])
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 2a92e2a..58130f0 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -718,8 +718,8 @@ def find_visible2(
 
     coords_origin = coords[origin_idx]
 
-    # nr_edges = neighbour_idxs.shape[0]
-    # edge_idxs2check = set(range(nr_edges))
+    nr_edges = neighbour_idxs.shape[0]
+    edge_idxs2check = set(range(nr_edges))
 
     def get_coordinates_translated(i: int) -> np.ndarray:
         coords_v = coords[i]
@@ -734,17 +734,20 @@ def get_repr(i: int) -> float:
 
     # goal: eliminating all vertices lying 'behind' any edge
     while len(edges_to_check) > 0 and len(candidate_idxs) > 0:
-        edge = edges_to_check.pop()
-
-        lies_on_edge = False
-        range_less_180 = False
 
         # TODO use indices only
+        edge = edges_to_check.pop()
         v1, v2 = edge.vertex1, edge.vertex2
         idx_v1 = vertices.index(v1)
         idx_v2 = vertices.index(v2)
 
+        # edge_idx = edge_idxs2check.pop()
         # idx_v1, idx_v2 = neighbour_idxs[edge_idx]
+        # v1 = vertices[idx_v1]
+        # v2 = vertices[idx_v2]
+
+        lies_on_edge = False
+        range_less_180 = False
 
         if get_distance_to_origin(idx_v1) == 0.0:
             # vertex1 has the same coordinates as the query vertex -> on the edge
@@ -758,6 +761,8 @@ def get_repr(i: int) -> float:
             # do not check the other neighbouring edge of vertex1 in the future
             e1 = v1.edge1
             edges_to_check.discard(e1)
+            edge_idx1, _ = edge_idxs[idx_v1]
+            edge_idxs2check.discard(edge_idx1)
             # everything between its two neighbouring edges is not visible for sure
             v1, v2 = v1.get_neighbours()
             idx_v1 = vertices.index(v1)
@@ -769,6 +774,8 @@ def get_repr(i: int) -> float:
             range_less_180 = v2.is_extremity
             e1 = v2.edge2
             edges_to_check.discard(e1)
+            _, edge_idx2 = edge_idxs[idx_v2]
+            edge_idxs2check.discard(edge_idx2)
             v1, v2 = v2.get_neighbours()
             idx_v1 = vertices.index(v1)
             idx_v2 = vertices.index(v2)

From 194b68e7774555a0de15c6971417e573fe535264 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 11:11:30 +0200
Subject: [PATCH 14/44] extremity_mask

---
 extremitypathfinder/extremitypathfinder.py | 13 +++++++++++++
 extremitypathfinder/helper_fcts.py         |  8 ++++++--
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index e37e674..f65874d 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -73,6 +73,10 @@ def all_vertices(self) -> List[PolygonVertex]:
             self._all_vertices = list(itertools.chain(*iter(p.vertices for p in self.polygons)))
         return self._all_vertices
 
+    @property
+    def nr_vertices(self) -> int:
+        return len(self.all_vertices)
+
     @property
     def extremity_indices(self) -> Set[int]:
         # TODO refactor
@@ -81,6 +85,13 @@ def extremity_indices(self) -> Set[int]:
         # Attention: only consider extremities that are actually within the map
         return {idx for idx, v in enumerate(self.all_vertices) if v.is_extremity and self.within_map(v.coordinates)}
 
+    @property
+    def extremity_mask(self) -> np.ndarray:
+        mask = np.full(self.nr_vertices, False, dtype=bool)
+        for i in self.extremity_indices:
+            mask[i] = True
+        return mask
+
     @property
     def all_extremities(self) -> List[PolygonVertex]:
         return [self.all_vertices[i] for i in self.extremity_indices]
@@ -200,6 +211,7 @@ def prepare(self):  # TODO include in storing functions?
         vertices = self.all_vertices
         nr_vertices = len(vertices)
         extremity_indices = self.extremity_indices
+        extremity_mask = self.extremity_mask
         coordinates = np.stack([v.coordinates for v in vertices])
 
         # TODO more performant way of computing
@@ -309,6 +321,7 @@ def get_repr(i1, i2):
             edges_to_check.remove(query_extremity.edge2)
             visible_vertices = find_visible2(
                 candidate_idxs,
+                extremity_mask,
                 angle_representations,
                 vertices,
                 coordinates,
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 58130f0..2bfab00 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -694,6 +694,7 @@ def find_visible(vertex_candidates, edges_to_check):
 
 def find_visible2(
     candidate_idxs: Set[int],
+    extremity_mask: np.ndarray,
     angle_representations: np.ndarray,
     vertices: List["PolygonVertex"],
     coords: np.ndarray,
@@ -732,6 +733,9 @@ def get_distance_to_origin(i: int) -> float:
     def get_repr(i: int) -> float:
         return get_angle_representation(origin_idx, i, angle_representations, coords)
 
+    def is_extremity(i: int) -> bool:
+        return extremity_mask[i]
+
     # goal: eliminating all vertices lying 'behind' any edge
     while len(edges_to_check) > 0 and len(candidate_idxs) > 0:
 
@@ -757,7 +761,7 @@ def get_repr(i: int) -> float:
             candidate_idxs.discard(idx_v1)
 
             # its angle representation is not defined (no line segment from vertex1 to query vertex!)
-            range_less_180 = v1.is_extremity
+            range_less_180 = is_extremity(idx_v1)
             # do not check the other neighbouring edge of vertex1 in the future
             e1 = v1.edge1
             edges_to_check.discard(e1)
@@ -771,7 +775,7 @@ def get_repr(i: int) -> float:
         elif get_distance_to_origin(idx_v2) == 0.0:
             lies_on_edge = True
             candidate_idxs.discard(idx_v2)
-            range_less_180 = v2.is_extremity
+            range_less_180 = is_extremity(idx_v2)
             e1 = v2.edge2
             edges_to_check.discard(e1)
             _, edge_idx2 = edge_idxs[idx_v2]

From f4b8b43760d14a1a318bfece4e66a1ca43464596 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 11:57:34 +0200
Subject: [PATCH 15/44] find_visible edge indices

---
 extremitypathfinder/extremitypathfinder.py | 35 ++++++-----
 extremitypathfinder/helper_fcts.py         | 70 ++++++++++------------
 2 files changed, 53 insertions(+), 52 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index f65874d..10d5835 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -215,12 +215,14 @@ def prepare(self):  # TODO include in storing functions?
         coordinates = np.stack([v.coordinates for v in vertices])
 
         # TODO more performant way of computing
-        edges = list(self.all_edges)
+        all_edges = list(self.all_edges)
         # TODO better name
-        edge_idxs = np.stack([(edges.index(v.edge1), edges.index(v.edge2)) for v in vertices])
+        vertex_edge_idxs = np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in vertices])
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
-        neighbour_idxs = np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in edges])
+        edge_vertex_idxs = np.stack(
+            [(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in all_edges]
+        )
 
         if len(extremity_indices) != len(extremities):
             raise ValueError
@@ -235,11 +237,11 @@ def get_repr(i1, i2):
             #  -> do not check extremities which have been checked already
             #  (would only give the same result when algorithms are correct)
             # the extremity itself must not be checked when looking for visible neighbours
-            # query_extremity: PolygonVertex = extremities_to_check.pop()
+            # origin_extremity: PolygonVertex = extremities_to_check.pop()
             origin_idx = extremity_indices.pop()
-            query_extremity = vertices[origin_idx]
+            origin_extremity = vertices[origin_idx]
 
-            # self.translate(new_origin=query_extremity)
+            # self.translate(new_origin=origin_extremity)
 
             # only consider extremities with coordinates different from the query extremity
             # (angle representation not None)
@@ -249,7 +251,7 @@ def get_repr(i1, i2):
             # )
 
             # these vertices all belong to a polygon
-            n1, n2 = query_extremity.get_neighbours()
+            n1, n2 = origin_extremity.get_neighbours()
             idx_n1 = vertices.index(n1)
             idx_n2 = vertices.index(n2)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
@@ -311,26 +313,29 @@ def get_repr(i1, i2):
             # already existing edges in the graph to the extremities in front have to be removed
             # TODO graph: also use indices instead of vertices
             lie_in_front = {vertices[i] for i in lie_in_front_idx}
-            self.graph.remove_multiple_undirected_edges(query_extremity, lie_in_front)
+            self.graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
             # do not consider when looking for visible extremities, even if they are actually be visible
             candidate_idxs.difference_update(idx_behind)
 
             # all edges except the neighbouring edges (handled above!) have to be checked
-            edges_to_check = self.all_edges
-            edges_to_check.remove(query_extremity.edge1)
-            edges_to_check.remove(query_extremity.edge2)
+            nr_edges = len(all_edges)
+            edge_idxs2check = set(range(nr_edges))
+            edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
+            edge_idxs2check.remove(edge1_idx)
+            edge_idxs2check.remove(edge2_idx)
+
             visible_vertices = find_visible2(
                 candidate_idxs,
                 extremity_mask,
                 angle_representations,
                 vertices,
                 coordinates,
-                edge_idxs,
-                neighbour_idxs,
+                vertex_edge_idxs,
+                edge_vertex_idxs,
                 origin_idx,
-                edges_to_check,
+                edge_idxs2check,
             )
-            self.graph.add_multiple_undirected_edges(query_extremity, visible_vertices)
+            self.graph.add_multiple_undirected_edges(origin_extremity, visible_vertices)
 
         self.graph.make_clean()  # join all nodes with the same coordinates
         self.prepared = True
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 2bfab00..f0e3f90 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,6 +1,6 @@
 import json
 from itertools import combinations
-from typing import List, Set
+from typing import List, Set, Tuple
 
 import numpy as np
 
@@ -698,10 +698,10 @@ def find_visible2(
     angle_representations: np.ndarray,
     vertices: List["PolygonVertex"],
     coords: np.ndarray,
-    edge_idxs: np.ndarray,
-    neighbour_idxs: np.ndarray,
+    vertex_edge_idxs: np.ndarray,
+    edge_vertex_idxs: np.ndarray,
     origin_idx: int,
-    edges_to_check: Set,
+    edge_idxs2check: Set[int],
 ):
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
@@ -719,9 +719,6 @@ def find_visible2(
 
     coords_origin = coords[origin_idx]
 
-    nr_edges = neighbour_idxs.shape[0]
-    edge_idxs2check = set(range(nr_edges))
-
     def get_coordinates_translated(i: int) -> np.ndarray:
         coords_v = coords[i]
         return coords_v - coords_origin
@@ -733,56 +730,56 @@ def get_distance_to_origin(i: int) -> float:
     def get_repr(i: int) -> float:
         return get_angle_representation(origin_idx, i, angle_representations, coords)
 
+    def get_neighbours(i: int) -> Tuple[int, int]:
+        edge_idx1, edge_idx2 = vertex_edge_idxs[i]
+        neigh_idx1, i2_ = edge_vertex_idxs[edge_idx1]
+        i1_, neigh_idx2 = edge_vertex_idxs[edge_idx2]
+        # TODO
+        assert i2_ == i2_
+        return neigh_idx1, neigh_idx2
+
     def is_extremity(i: int) -> bool:
         return extremity_mask[i]
 
     # goal: eliminating all vertices lying 'behind' any edge
-    while len(edges_to_check) > 0 and len(candidate_idxs) > 0:
-
-        # TODO use indices only
-        edge = edges_to_check.pop()
-        v1, v2 = edge.vertex1, edge.vertex2
-        idx_v1 = vertices.index(v1)
-        idx_v2 = vertices.index(v2)
-
-        # edge_idx = edge_idxs2check.pop()
-        # idx_v1, idx_v2 = neighbour_idxs[edge_idx]
-        # v1 = vertices[idx_v1]
-        # v2 = vertices[idx_v2]
+    while len(edge_idxs2check) > 0 and len(candidate_idxs) > 0:
+        edge_idx = edge_idxs2check.pop()
+        idx_v1, idx_v2 = edge_vertex_idxs[edge_idx]
 
         lies_on_edge = False
         range_less_180 = False
 
         if get_distance_to_origin(idx_v1) == 0.0:
-            # vertex1 has the same coordinates as the query vertex -> on the edge
+            # vertex1 of the edge has the same coordinates as the query vertex
+            # -> the origin lies on the edge
             lies_on_edge = True
-            # (but does not belong to the same polygon, not identical!)
+            # (note: not identical, does not belong to the same polygon!)
             # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
             candidate_idxs.discard(idx_v1)
 
-            # its angle representation is not defined (no line segment from vertex1 to query vertex!)
+            # no points lie truly "behind" this edge as there is no "direction of sight" defined
+            # <-> angle representation/range undefined for just this single edge
+            # however if one considers the point neighbouring in the other direction (<-> two edges)
+            # these two neighbouring edges define an invisible angle range
+            # -> simply move the pointer
+            idx_v1, idx_v2 = get_neighbours(idx_v1)
             range_less_180 = is_extremity(idx_v1)
-            # do not check the other neighbouring edge of vertex1 in the future
-            e1 = v1.edge1
-            edges_to_check.discard(e1)
-            edge_idx1, _ = edge_idxs[idx_v1]
+
+            # do not check the other neighbouring edge of vertex1 in the future (has been considered already)
+            edge_idx1, _ = vertex_edge_idxs[idx_v1]
             edge_idxs2check.discard(edge_idx1)
-            # everything between its two neighbouring edges is not visible for sure
-            v1, v2 = v1.get_neighbours()
-            idx_v1 = vertices.index(v1)
-            idx_v2 = vertices.index(v2)
 
         elif get_distance_to_origin(idx_v2) == 0.0:
+            # same for vertex2 of the edge
+            # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex
+            # (edge with length 0)
             lies_on_edge = True
             candidate_idxs.discard(idx_v2)
             range_less_180 = is_extremity(idx_v2)
-            e1 = v2.edge2
-            edges_to_check.discard(e1)
-            _, edge_idx2 = edge_idxs[idx_v2]
+            _, edge_idx2 = vertex_edge_idxs[idx_v2]
             edge_idxs2check.discard(edge_idx2)
-            v1, v2 = v2.get_neighbours()
-            idx_v1 = vertices.index(v1)
-            idx_v2 = vertices.index(v2)
+
+            idx_v1, idx_v2 = get_neighbours(idx_v2)
 
         repr1 = get_repr(idx_v1)
         repr2 = get_repr(idx_v2)
@@ -874,7 +871,6 @@ def is_extremity(i: int) -> bool:
 
     # all edges have been checked
     # all remaining vertices were not concealed behind any edge and hence are visible
-
     visible_idxs.update(candidate_idxs)
 
     # return a set of tuples: (vertex, distance)

From 2ffcab40f2cb166808af3d6aaf130ef65d063d86 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 12:15:27 +0200
Subject: [PATCH 16/44] use idx2repr mapping

---
 extremitypathfinder/extremitypathfinder.py | 31 ++++++++--------------
 extremitypathfinder/helper_fcts.py         | 28 +++++++------------
 2 files changed, 20 insertions(+), 39 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 10d5835..566d420 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -263,23 +263,17 @@ def get_repr(i1, i2):
             #   are not visible (no candidates!)
             # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
             n1_repr = get_repr(origin_idx, idx_n1)
-
-            # TODO
-
             n2_repr = get_repr(origin_idx, idx_n2)
 
-            # TODO
-            idx_behind = find_within_range2(
+            idx2repr = {i: get_repr(origin_idx, i) for i in candidate_idxs}
+            idxs_behind = find_within_range2(
                 n1_repr,
                 n2_repr,
-                candidate_idxs,
-                angle_representations,
-                coordinates,
-                origin_idx,
+                idx2repr,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            candidate_idxs.difference_update(idx_behind)
+            candidate_idxs.difference_update(idxs_behind)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -293,19 +287,16 @@ def get_repr(i1, i2):
             # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
             # MUST be added to the graph!
             # Find extremities which fulfill this condition for the given query extremity
-            n1_repr = angle_rep_inverse(n1_repr)
-            n2_repr = angle_rep_inverse(n2_repr)
+            n1_repr_inv = angle_rep_inverse(n1_repr)
+            n2_repr_inv = angle_rep_inverse(n2_repr)
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            temp_idxs = {i for i in extremity_indices if get_repr(origin_idx, i) is not None}
+            idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices if get_repr(origin_idx, i) is not None}
             lie_in_front_idx = find_within_range2(
-                n1_repr,
-                n2_repr,
-                temp_idxs,
-                angle_representations,
-                coordinates,
-                origin_idx,
+                n1_repr_inv,
+                n2_repr_inv,
+                idx2repr,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
@@ -315,7 +306,7 @@ def get_repr(i1, i2):
             lie_in_front = {vertices[i] for i in lie_in_front_idx}
             self.graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
             # do not consider when looking for visible extremities, even if they are actually be visible
-            candidate_idxs.difference_update(idx_behind)
+            candidate_idxs.difference_update(idxs_behind)
 
             # all edges except the neighbouring edges (handled above!) have to be checked
             nr_edges = len(all_edges)
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index f0e3f90..da7224d 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,6 +1,6 @@
 import json
 from itertools import combinations
-from typing import List, Set, Tuple
+from typing import Dict, List, Set, Tuple
 
 import numpy as np
 
@@ -296,10 +296,7 @@ def within_filter_func(r: float) -> bool:
 def find_within_range2(
     repr1: float,
     repr2: float,
-    candidate_idx: Set[int],
-    angle_representations: np.ndarray,
-    coords: np.ndarray,
-    origin_idx: int,
+    idx2repr: Dict[int, float],
     angle_range_less_180: bool,
     equal_repr_allowed: bool,
 ) -> Set[int]:
@@ -315,7 +312,7 @@ def find_within_range2(
     :param equal_repr_allowed: whether vertices with the same representation should also be returned
     :return:
     """
-    if len(candidate_idx) == 0:
+    if len(idx2repr) == 0:
         return set()
 
     repr_diff = abs(repr1 - repr2)
@@ -354,11 +351,8 @@ def within_filter_func(r: float) -> bool:
             res = not res
         return res
 
-    def get_repr(i1, i2):
-        return get_angle_representation(i1, i2, angle_representations, coords)
-
-    idx_within = {idx for idx in candidate_idx if within_filter_func(get_repr(origin_idx, idx))}
-    return idx_within
+    idxs_within = {i for i, r in idx2repr.items() if within_filter_func(r)}
+    return idxs_within
 
 
 def convert_gridworld(size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True) -> (list, list):
@@ -807,13 +801,11 @@ def is_extremity(i: int) -> bool:
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation should not be deleted, because they can be visible!
+            idx2repr = {i: get_repr(i) for i in candidate_idxs}
             invisible_candidate_idxs = find_within_range2(
                 repr1,
                 repr2,
-                candidate_idxs,
-                angle_representations,
-                coords,
-                origin_idx,
+                idx2repr,
                 angle_range_less_180=range_less_180,
                 equal_repr_allowed=False,
             )
@@ -835,13 +827,11 @@ def is_extremity(i: int) -> bool:
         #   is always < 180deg when the edge is not running through the query point (=180 deg)
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
+        idx2repr = {i: get_repr(i) for i in idxs2check}
         idxs2check = find_within_range2(
             repr1,
             repr2,
-            idxs2check,
-            angle_representations,
-            coords,
-            origin_idx,
+            idx2repr,
             angle_range_less_180=True,
             equal_repr_allowed=True,
         )

From 72868150f69e98c9ba03bfe8a81459cae50521fc Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 13:14:49 +0200
Subject: [PATCH 17/44] idx2rep refactoring

---
 extremitypathfinder/extremitypathfinder.py | 82 +++++++++++++---------
 extremitypathfinder/helper_fcts.py         |  6 +-
 2 files changed, 52 insertions(+), 36 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 566d420..bb461d0 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -227,35 +227,23 @@ def prepare(self):  # TODO include in storing functions?
         if len(extremity_indices) != len(extremities):
             raise ValueError
 
+        # TODO reuse
         def get_repr(i1, i2):
             return get_angle_representation(i1, i2, angle_representations, coordinates)
 
-        # have to run for all (also last one!), because existing edges might get deleted every loop
-        while len(extremity_indices) > 0:
-            # while len(extremities_to_check) > 0:
-            # extremities are always visible to each other (bi-directional relation -> undirected graph)
-            #  -> do not check extremities which have been checked already
-            #  (would only give the same result when algorithms are correct)
-            # the extremity itself must not be checked when looking for visible neighbours
-            # origin_extremity: PolygonVertex = extremities_to_check.pop()
-            origin_idx = extremity_indices.pop()
-            origin_extremity = vertices[origin_idx]
-
-            # self.translate(new_origin=origin_extremity)
+        def get_neighbours(i: int) -> Tuple[int, int]:
+            edge_idx1, edge_idx2 = vertex_edge_idxs[i]
+            neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
+            neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
+            return neigh_idx1, neigh_idx2
 
-            # only consider extremities with coordinates different from the query extremity
-            # (angle representation not None)
-            candidate_idxs = {i for i in extremity_indices if get_repr(origin_idx, i) is not None}
-            # candidates.difference_update(
-            #     {c for c in candidates if get_angle_representation(idx, c) is None}
-            # )
+        # have to run for all (also last one!), because existing edges might get deleted every loop
 
-            # these vertices all belong to a polygon
-            n1, n2 = origin_extremity.get_neighbours()
-            idx_n1 = vertices.index(n1)
-            idx_n2 = vertices.index(n2)
+        extremity_indices = list(extremity_indices)
+        for _, origin_idx in enumerate(extremity_indices):
+            # vertices all belong to a polygon
+            idx_n1, idx_n2 = get_neighbours(origin_idx)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
-
             # eliminate all vertices 'behind' the query point from the candidate set
             # since the query vertex is an extremity the 'outer' angle is < 180 degree
             # then the difference between the angle representation of the two edges has to be < 2.0
@@ -265,7 +253,17 @@ def get_repr(i1, i2):
             n1_repr = get_repr(origin_idx, idx_n1)
             n2_repr = get_repr(origin_idx, idx_n2)
 
-            idx2repr = {i: get_repr(origin_idx, i) for i in candidate_idxs}
+            idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
+            # only consider extremities with coordinates different from the query extremity
+            # (angle representation not None)
+            # the extremity itself must also not be checked when looking for visible neighbours
+            idx2repr = {i: r for i, r in idx2repr.items() if r is not None}
+            # extremities are always visible to each other (bi-directional relation -> undirected graph)
+            #  -> do not check extremities which have been checked already
+            #  (must give the same result when algorithms are correct)
+            # TODO
+            # idx2repr_tmp = {i: r for i, r in idx2repr.items() if i > origin_idx}
+
             idxs_behind = find_within_range2(
                 n1_repr,
                 n2_repr,
@@ -273,7 +271,21 @@ def get_repr(i1, i2):
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            candidate_idxs.difference_update(idxs_behind)
+
+            # idxs_behind_ = find_within_range2(
+            #     n1_repr,
+            #     n2_repr,
+            #     idx2repr_tmp,
+            #     angle_range_less_180=True,
+            #     equal_repr_allowed=False,
+            # )
+            #
+            # if idxs_behind_ != idxs_behind:
+            #     x=1
+
+            # idxs_behind__ = {i for i in idxs_behind_ if i >origin_idx}
+            # if idxs_behind__ != idxs_behind:
+            #     raise ValueError
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -289,10 +301,10 @@ def get_repr(i1, i2):
             # Find extremities which fulfill this condition for the given query extremity
             n1_repr_inv = angle_rep_inverse(n1_repr)
             n2_repr_inv = angle_rep_inverse(n2_repr)
+
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coordinates (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices if get_repr(origin_idx, i) is not None}
             lie_in_front_idx = find_within_range2(
                 n1_repr_inv,
                 n2_repr_inv,
@@ -300,23 +312,28 @@ def get_repr(i1, i2):
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            # "thin out" the graph -> optimisation
-            # already existing edges in the graph to the extremities in front have to be removed
+            # optimisation: "thin out" the graph
+            # remove already existing edges in the graph to the extremities in front
             # TODO graph: also use indices instead of vertices
+            origin_extremity = vertices[origin_idx]
             lie_in_front = {vertices[i] for i in lie_in_front_idx}
             self.graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
             # do not consider when looking for visible extremities, even if they are actually be visible
-            candidate_idxs.difference_update(idxs_behind)
+            extr_candidates = {i for i in idx2repr.keys() if i not in lie_in_front_idx and i not in idxs_behind}
 
-            # all edges except the neighbouring edges (handled above!) have to be checked
+            # do not consider indices found to lie behind
+            # for i in idxs_behind:
+            #     idx2repr.pop(i)
+
+            # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
             edge_idxs2check = set(range(nr_edges))
             edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
             edge_idxs2check.remove(edge1_idx)
             edge_idxs2check.remove(edge2_idx)
-
+            # TODO reutrn only idx
             visible_vertices = find_visible2(
-                candidate_idxs,
+                extr_candidates,
                 extremity_mask,
                 angle_representations,
                 vertices,
@@ -326,6 +343,7 @@ def get_repr(i1, i2):
                 origin_idx,
                 edge_idxs2check,
             )
+            # TODO compile mapping to distance
             self.graph.add_multiple_undirected_edges(origin_extremity, visible_vertices)
 
         self.graph.make_clean()  # join all nodes with the same coordinates
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index da7224d..ff54831 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -726,10 +726,8 @@ def get_repr(i: int) -> float:
 
     def get_neighbours(i: int) -> Tuple[int, int]:
         edge_idx1, edge_idx2 = vertex_edge_idxs[i]
-        neigh_idx1, i2_ = edge_vertex_idxs[edge_idx1]
-        i1_, neigh_idx2 = edge_vertex_idxs[edge_idx2]
-        # TODO
-        assert i2_ == i2_
+        neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
+        neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
         return neigh_idx1, neigh_idx2
 
     def is_extremity(i: int) -> bool:

From de92ef87a33c733431a535ca114385198a0404f6 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 13:48:52 +0200
Subject: [PATCH 18/44] refactor add_multiple_undirected_edges signature

---
 extremitypathfinder/extremitypathfinder.py | 88 ++++++++++++----------
 extremitypathfinder/helper_classes.py      |  4 +-
 extremitypathfinder/helper_fcts.py         |  7 +-
 3 files changed, 53 insertions(+), 46 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index bb461d0..c4d559b 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -177,13 +177,13 @@ def translate(self, new_origin: Vertex):
         for p in self.polygons:
             p.translate(new_origin)
 
-    def prepare(self):  # TODO include in storing functions?
+    def prepare(self):  # TODO include in storing functions? breaking change!
         """Computes a visibility graph optimized (=reduced) for path planning and stores it
 
         Computes all directly reachable extremities based on visibility and their distance to each other
 
         .. note::
-            Multiple polygon vertices might have identical coordinates.
+            Multiple polygon vertices might have identical coords.
             They must be treated as distinct vertices here, since their attached edges determine visibility.
             In the created graph however, these nodes must be merged at the end to avoid ambiguities!
 
@@ -201,22 +201,21 @@ def prepare(self):  # TODO include in storing functions?
         # NOTE: initialise the graph with all extremities.
         #   even if a node has no edges (visibility to other extremities), it should still be included!
         extremities = self.all_extremities
-        self.graph = DirectedHeuristicGraph(extremities)
         nr_extremities = len(extremities)
         if nr_extremities == 0:
+            # TODO
+            self.graph = DirectedHeuristicGraph(extremities)
             return
 
-            # extremities_to_check = set(extremities)
-
         vertices = self.all_vertices
         nr_vertices = len(vertices)
         extremity_indices = self.extremity_indices
         extremity_mask = self.extremity_mask
-        coordinates = np.stack([v.coordinates for v in vertices])
 
         # TODO more performant way of computing
         all_edges = list(self.all_edges)
-        # TODO better name
+        graph = DirectedHeuristicGraph(extremities)
+        coords = np.stack([v.coordinates for v in vertices])
         vertex_edge_idxs = np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in vertices])
         # TODO sparse matrix. problematic: default value is 0.0
         angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
@@ -229,7 +228,7 @@ def prepare(self):  # TODO include in storing functions?
 
         # TODO reuse
         def get_repr(i1, i2):
-            return get_angle_representation(i1, i2, angle_representations, coordinates)
+            return get_angle_representation(i1, i2, angle_representations, coords)
 
         def get_neighbours(i: int) -> Tuple[int, int]:
             edge_idx1, edge_idx2 = vertex_edge_idxs[i]
@@ -237,10 +236,18 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
             return neigh_idx1, neigh_idx2
 
-        # have to run for all (also last one!), because existing edges might get deleted every loop
+        def get_coordinates_translated(orig_idx: int, i: int) -> np.ndarray:
+            coords_origin = coords[orig_idx]
+            coords_v = coords[i]
+            return coords_v - coords_origin
+
+        def get_distance_to_origin(orig_idx: int, i: int) -> float:
+            coords = get_coordinates_translated(orig_idx, i)
+            return np.linalg.norm(coords, ord=2)
 
         extremity_indices = list(extremity_indices)
-        for _, origin_idx in enumerate(extremity_indices):
+        # TODO use orig_ptr
+        for _orig_ptr, origin_idx in enumerate(extremity_indices):
             # vertices all belong to a polygon
             idx_n1, idx_n2 = get_neighbours(origin_idx)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
@@ -254,16 +261,10 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             n2_repr = get_repr(origin_idx, idx_n2)
 
             idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
-            # only consider extremities with coordinates different from the query extremity
+            # only consider extremities with coords different from the query extremity
             # (angle representation not None)
-            # the extremity itself must also not be checked when looking for visible neighbours
+            # the origin extremity itself must also not be checked when looking for visible neighbours
             idx2repr = {i: r for i, r in idx2repr.items() if r is not None}
-            # extremities are always visible to each other (bi-directional relation -> undirected graph)
-            #  -> do not check extremities which have been checked already
-            #  (must give the same result when algorithms are correct)
-            # TODO
-            # idx2repr_tmp = {i: r for i, r in idx2repr.items() if i > origin_idx}
-
             idxs_behind = find_within_range2(
                 n1_repr,
                 n2_repr,
@@ -272,7 +273,13 @@ def get_neighbours(i: int) -> Tuple[int, int]:
                 equal_repr_allowed=False,
             )
 
-            # idxs_behind_ = find_within_range2(
+            # TODO
+            # extremities are always visible to each other
+            # (bi-directional relation -> undirected edges in the graph)
+            #  -> do not check extremities which have been checked already
+            #  (must give the same result when algorithms are correct)
+            # idx2repr_tmp = {i: r for i, r in idx2repr.items() if i > origin_idx}
+            # idxs_behind = find_within_range2(
             #     n1_repr,
             #     n2_repr,
             #     idx2repr_tmp,
@@ -281,9 +288,9 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             # )
             #
             # if idxs_behind_ != idxs_behind:
-            #     x=1
-
-            # idxs_behind__ = {i for i in idxs_behind_ if i >origin_idx}
+            #     x = 1
+            #
+            # idxs_behind__ = {i for i in idxs_behind_ if i > origin_idx}
             # if idxs_behind__ != idxs_behind:
             #     raise ValueError
 
@@ -303,7 +310,7 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             n2_repr_inv = angle_rep_inverse(n2_repr)
 
             # IMPORTANT: check all extremities here, not just current candidates
-            # do not check extremities with equal coordinates (also query extremity itself!)
+            # do not check extremities with equal coords (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
             lie_in_front_idx = find_within_range2(
                 n1_repr_inv,
@@ -312,18 +319,11 @@ def get_neighbours(i: int) -> Tuple[int, int]:
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-            # optimisation: "thin out" the graph
-            # remove already existing edges in the graph to the extremities in front
-            # TODO graph: also use indices instead of vertices
-            origin_extremity = vertices[origin_idx]
-            lie_in_front = {vertices[i] for i in lie_in_front_idx}
-            self.graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
-            # do not consider when looking for visible extremities, even if they are actually be visible
-            extr_candidates = {i for i in idx2repr.keys() if i not in lie_in_front_idx and i not in idxs_behind}
 
-            # do not consider indices found to lie behind
-            # for i in idxs_behind:
-            #     idx2repr.pop(i)
+            # do not consider points lying in front when looking for visible extremities,
+            # even if they are actually be visible
+            # do not consider points found to lie behind
+            extr_candidates = {i for i in idx2repr.keys() if i not in lie_in_front_idx and i not in idxs_behind}
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
@@ -332,21 +332,29 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             edge_idxs2check.remove(edge1_idx)
             edge_idxs2check.remove(edge2_idx)
             # TODO reutrn only idx
-            visible_vertices = find_visible2(
+            visible_idxs = find_visible2(
                 extr_candidates,
                 extremity_mask,
                 angle_representations,
-                vertices,
-                coordinates,
+                coords,
                 vertex_edge_idxs,
                 edge_vertex_idxs,
                 origin_idx,
                 edge_idxs2check,
             )
-            # TODO compile mapping to distance
-            self.graph.add_multiple_undirected_edges(origin_extremity, visible_vertices)
 
-        self.graph.make_clean()  # join all nodes with the same coordinates
+            # TODO graph: also use indices instead of vertices
+            origin_extremity = vertices[origin_idx]
+            visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
+            graph.add_multiple_undirected_edges(origin_extremity, visible_vertex2dist_map)
+            # optimisation: "thin out" the graph
+            # remove already existing edges in the graph to the extremities in front
+            lie_in_front = {vertices[i] for i in lie_in_front_idx}
+            graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
+
+        graph.make_clean()  # join all nodes with the same coords
+
+        self.graph = graph
         self.prepared = True
 
     def within_map(self, coords: INPUT_COORD_TYPE):
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index f1b9ff1..0296898 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -418,8 +418,8 @@ def add_undirected_edge(self, node1, node2, distance):
         self.add_directed_edge(node1, node2, distance)
         self.add_directed_edge(node2, node1, distance)
 
-    def add_multiple_undirected_edges(self, node1, node_distance_iter):
-        for node2, distance in node_distance_iter:
+    def add_multiple_undirected_edges(self, node1, node_distance_map: Dict):
+        for node2, distance in node_distance_map.items():
             self.add_undirected_edge(node1, node2, distance)
 
     def add_multiple_directed_edges(self, node1, node_distance_iter):
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index ff54831..755c24f 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -690,13 +690,12 @@ def find_visible2(
     candidate_idxs: Set[int],
     extremity_mask: np.ndarray,
     angle_representations: np.ndarray,
-    vertices: List["PolygonVertex"],
     coords: np.ndarray,
     vertex_edge_idxs: np.ndarray,
     edge_vertex_idxs: np.ndarray,
     origin_idx: int,
     edge_idxs2check: Set[int],
-):
+) -> Set[int]:
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
         also non extremity vertices, polygon vertices and vertices with the same coordinates are allowed.
@@ -713,6 +712,7 @@ def find_visible2(
 
     coords_origin = coords[origin_idx]
 
+    # TODO reuse
     def get_coordinates_translated(i: int) -> np.ndarray:
         coords_v = coords[i]
         return coords_v - coords_origin
@@ -861,8 +861,7 @@ def is_extremity(i: int) -> bool:
     # all remaining vertices were not concealed behind any edge and hence are visible
     visible_idxs.update(candidate_idxs)
 
-    # return a set of tuples: (vertex, distance)
-    return {(vertices[i], get_distance_to_origin(i)) for i in visible_idxs}
+    return visible_idxs
 
 
 def try_extraction(json_data, key):

From 56bcecad43631ba0e3ee98e58ee6c506b340e43f Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 14:37:49 +0200
Subject: [PATCH 19/44] find visible without origin idx

---
 extremitypathfinder/extremitypathfinder.py |  19 ++--
 extremitypathfinder/helper_fcts.py         | 102 +++++++++++----------
 tests/main_test.py                         |   1 +
 3 files changed, 67 insertions(+), 55 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index c4d559b..8622fa5 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -28,7 +28,7 @@
     find_visible2,
     find_within_range,
     find_within_range2,
-    get_angle_representation,
+    get_angle_repr,
     inside_polygon,
 )
 
@@ -227,8 +227,9 @@ def prepare(self):  # TODO include in storing functions? breaking change!
             raise ValueError
 
         # TODO reuse
-        def get_repr(i1, i2):
-            return get_angle_representation(i1, i2, angle_representations, coords)
+        def get_repr(idx_origin, i):
+            coords_origin = coords[idx_origin]
+            return get_angle_repr(coords_origin, i, angle_representations, coords)
 
         def get_neighbours(i: int) -> Tuple[int, int]:
             edge_idx1, edge_idx2 = vertex_edge_idxs[i]
@@ -260,6 +261,9 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             n1_repr = get_repr(origin_idx, idx_n1)
             n2_repr = get_repr(origin_idx, idx_n2)
 
+            # TODO lazy init? same as angle repr
+            vert_idx2dist = {i: get_distance_to_origin(origin_idx, i) for i in range(nr_vertices)}
+
             idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
             # only consider extremities with coords different from the query extremity
             # (angle representation not None)
@@ -323,7 +327,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
             # do not consider points found to lie behind
-            extr_candidates = {i for i in idx2repr.keys() if i not in lie_in_front_idx and i not in idxs_behind}
+            idx2repr = {i: r for i, r in idx2repr.items() if i not in lie_in_front_idx and i not in idxs_behind}
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
@@ -331,16 +335,17 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
             edge_idxs2check.remove(edge1_idx)
             edge_idxs2check.remove(edge2_idx)
-            # TODO reutrn only idx
+            coords_origin = coords[origin_idx]
             visible_idxs = find_visible2(
-                extr_candidates,
                 extremity_mask,
                 angle_representations,
                 coords,
                 vertex_edge_idxs,
                 edge_vertex_idxs,
-                origin_idx,
                 edge_idxs2check,
+                coords_origin,
+                idx2repr,
+                vert_idx2dist,
             )
 
             # TODO graph: also use indices instead of vertices
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 755c24f..0f6931d 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -8,12 +8,7 @@
 import numpy.linalg
 
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import (
-    PolygonVertex,
-    angle_rep_inverse,
-    compute_angle_repr,
-    compute_angle_repr_inner,
-)
+from extremitypathfinder.helper_classes import PolygonVertex, compute_angle_repr, compute_angle_repr_inner
 
 
 def inside_polygon(x, y, coords, border_value):
@@ -218,20 +213,30 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
     # TODO data rectification
 
 
-def get_angle_representation(idx_origin: int, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
-    repr = repr_matrix[idx_origin, idx_v]
-
-    # lazy initalisation: compute on demand only
-    if np.isnan(repr):  # attention: repr == np.nan does not match!
-        coords_origin = coordinates[idx_origin]
-        coords_v = coordinates[idx_v]
-        repr = compute_angle_repr(coords_origin, coords_v)
-        repr_matrix[idx_origin, idx_v] = repr
-
-        # TODO not required. only triangle required?!
-        # make use of symmetry: rotate 180 deg
-        repr_matrix[idx_v, idx_origin] = angle_rep_inverse(repr)
-
+#
+# # TODO use caching variant
+# def get_angle_representation(idx_origin: int, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
+#     repr = repr_matrix[idx_origin, idx_v]
+#
+#     # lazy initalisation: compute on demand only
+#     if np.isnan(repr):  # attention: repr == np.nan does not match!
+#         coords_origin = coordinates[idx_origin]
+#         coords_v = coordinates[idx_v]
+#         repr = compute_angle_repr(coords_origin, coords_v)
+#         repr_matrix[idx_origin, idx_v] = repr
+#
+#         # TODO not required. only triangle required?!
+#         # make use of symmetry: rotate 180 deg
+#         repr_matrix[idx_v, idx_origin] = angle_rep_inverse(repr)
+#
+#     # TODO
+#     assert repr is None or not np.isnan(repr)
+#     return repr
+
+
+def get_angle_repr(coords_origin: np.ndarray, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
+    coords_v = coordinates[idx_v]
+    repr = compute_angle_repr(coords_origin, coords_v)
     # TODO
     assert repr is None or not np.isnan(repr)
     return repr
@@ -687,14 +692,15 @@ def find_visible(vertex_candidates, edges_to_check):
 
 
 def find_visible2(
-    candidate_idxs: Set[int],
     extremity_mask: np.ndarray,
     angle_representations: np.ndarray,
     coords: np.ndarray,
     vertex_edge_idxs: np.ndarray,
     edge_vertex_idxs: np.ndarray,
-    origin_idx: int,
     edge_idxs2check: Set[int],
+    coords_origin: np.ndarray,
+    cand_idx2repr: Dict[int, float],
+    vert_idx2dist: Dict[int, float],
 ) -> Set[int]:
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
@@ -707,22 +713,21 @@ def find_visible2(
     :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
     """
     visible_idxs = set()
-    if len(candidate_idxs) == 0:
+    if len(cand_idx2repr) == 0:
         return visible_idxs
 
-    coords_origin = coords[origin_idx]
-
     # TODO reuse
     def get_coordinates_translated(i: int) -> np.ndarray:
         coords_v = coords[i]
         return coords_v - coords_origin
 
     def get_distance_to_origin(i: int) -> float:
-        coords = get_coordinates_translated(i)
-        return np.linalg.norm(coords, ord=2)
+        return vert_idx2dist[i]
+        # coords = get_coordinates_translated(i)
+        # return np.linalg.norm(coords, ord=2)
 
     def get_repr(i: int) -> float:
-        return get_angle_representation(origin_idx, i, angle_representations, coords)
+        return get_angle_repr(coords_origin, i, angle_representations, coords)
 
     def get_neighbours(i: int) -> Tuple[int, int]:
         edge_idx1, edge_idx2 = vertex_edge_idxs[i]
@@ -734,7 +739,7 @@ def is_extremity(i: int) -> bool:
         return extremity_mask[i]
 
     # goal: eliminating all vertices lying 'behind' any edge
-    while len(edge_idxs2check) > 0 and len(candidate_idxs) > 0:
+    while len(edge_idxs2check) > 0 and len(cand_idx2repr) > 0:
         edge_idx = edge_idxs2check.pop()
         idx_v1, idx_v2 = edge_vertex_idxs[edge_idx]
 
@@ -747,7 +752,7 @@ def is_extremity(i: int) -> bool:
             lies_on_edge = True
             # (note: not identical, does not belong to the same polygon!)
             # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
-            candidate_idxs.discard(idx_v1)
+            cand_idx2repr.pop(idx_v1, None)
 
             # no points lie truly "behind" this edge as there is no "direction of sight" defined
             # <-> angle representation/range undefined for just this single edge
@@ -766,7 +771,8 @@ def is_extremity(i: int) -> bool:
             # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex
             # (edge with length 0)
             lies_on_edge = True
-            candidate_idxs.discard(idx_v2)
+            cand_idx2repr.pop(idx_v2, None)
+
             range_less_180 = is_extremity(idx_v2)
             _, edge_idx2 = vertex_edge_idxs[idx_v2]
             edge_idxs2check.discard(edge_idx2)
@@ -787,37 +793,37 @@ def is_extremity(i: int) -> bool:
             # the neighbouring edges are visible for sure
             # attention: only add to visible set if vertex was a candidate!
             try:
-                candidate_idxs.remove(idx_v1)
+                cand_idx2repr.pop(idx_v1)
                 visible_idxs.add(idx_v1)
             except KeyError:
                 pass
             try:
-                candidate_idxs.remove(idx_v2)
+                cand_idx2repr.pop(idx_v2)
                 visible_idxs.add(idx_v2)
             except KeyError:
                 pass
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation should not be deleted, because they can be visible!
-            idx2repr = {i: get_repr(i) for i in candidate_idxs}
             invisible_candidate_idxs = find_within_range2(
                 repr1,
                 repr2,
-                idx2repr,
+                cand_idx2repr,
                 angle_range_less_180=range_less_180,
                 equal_repr_allowed=False,
             )
-            candidate_idxs.difference_update(invisible_candidate_idxs)
+            for i in invisible_candidate_idxs:
+                cand_idx2repr.pop(i, None)
             continue
 
         # case: a 'regular' edge
         # eliminate all candidates which are blocked by the edge
         # that means inside the angle range spanned by the edge and actually behind it
-        idxs2check = candidate_idxs.copy()
+        idx2repr_tmp = cand_idx2repr.copy()
         # the vertices belonging to the edge itself (its vertices) must not be checked.
         # use discard() instead of remove() to not raise an error (they might not be candidates)
-        idxs2check.discard(idx_v1)
-        idxs2check.discard(idx_v2)
+        idx2repr_tmp.pop(idx_v1, None)
+        idx2repr_tmp.pop(idx_v2, None)
 
         # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge)
         #   within this angle range
@@ -825,19 +831,14 @@ def is_extremity(i: int) -> bool:
         #   is always < 180deg when the edge is not running through the query point (=180 deg)
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
-        idx2repr = {i: get_repr(i) for i in idxs2check}
         idxs2check = find_within_range2(
             repr1,
             repr2,
-            idx2repr,
+            idx2repr_tmp,
             angle_range_less_180=True,
             equal_repr_allowed=True,
         )
 
-        # if a candidate is farther away from the query point than both vertices of the edge,
-        #   it surely lies behind the edge
-        # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
-        #   it still needs to be checked!
         v1_dist = get_distance_to_origin(idx_v1)
         v2_dist = get_distance_to_origin(idx_v2)
         max_distance = max(v1_dist, v2_dist)
@@ -847,18 +848,23 @@ def is_extremity(i: int) -> bool:
         p1 = get_coordinates_translated(idx_v1)
         p2 = get_coordinates_translated(idx_v2)
         for idx in idxs2check:
-            if get_repr(idx) is None:
-                continue
+            # if a candidate is farther away from the query point than both vertices of the edge,
+            #   it surely lies behind the edge
+            # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
+            #   it still needs to be checked!
             further_away = get_distance_to_origin(idx) > max_distance
             if further_away or lies_behind(p1, p2, get_coordinates_translated(idx)):
                 idxs_behind.add(idx)
             # vertex lies in front of this edge
 
         # vertices behind any edge are not visible
-        candidate_idxs.difference_update(idxs_behind)
+        for i in idxs_behind:
+            # TOD Try without default value
+            cand_idx2repr.pop(i, None)
 
     # all edges have been checked
     # all remaining vertices were not concealed behind any edge and hence are visible
+    candidate_idxs = cand_idx2repr.keys()
     visible_idxs.update(candidate_idxs)
 
     return visible_idxs
diff --git a/tests/main_test.py b/tests/main_test.py
index 43c4fcf..eefd7d6 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -275,6 +275,7 @@
 
 TEST_DATA_OVERLAP_POLY_ENV = [
     # ((start,goal),(path,distance))
+    # TODO add more
     (
         ((1, 1), (5, 20)),
         (

From b971f7f6920ff6c0427ce2fa885e0f22da73d107 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 16:27:25 +0200
Subject: [PATCH 20/44] before switching graph to indices

---
 extremitypathfinder/extremitypathfinder.py | 157 +++++++++++++++++----
 extremitypathfinder/helper_fcts.py         |   8 +-
 tests/main_test.py                         |   2 +-
 3 files changed, 131 insertions(+), 36 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 8622fa5..9c44668 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,7 +1,7 @@
 import itertools
 import pickle
 from copy import deepcopy
-from typing import Iterable, List, Optional, Set, Tuple
+from typing import Iterable, List, Optional, Tuple
 
 import numpy as np
 
@@ -73,17 +73,31 @@ def all_vertices(self) -> List[PolygonVertex]:
             self._all_vertices = list(itertools.chain(*iter(p.vertices for p in self.polygons)))
         return self._all_vertices
 
+    @property
+    def coords(self) -> np.ndarray:
+        return np.stack([v.coordinates for v in self.all_vertices])
+
+    @property
+    def vertex_edge_idxs(self) -> np.ndarray:
+        all_edges = self.all_edges
+        return np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in self.all_vertices])
+
+    @property
+    def edge_vertex_idxs(self) -> np.ndarray:
+        vertices = self.all_vertices
+        return np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in self.all_edges])
+
     @property
     def nr_vertices(self) -> int:
         return len(self.all_vertices)
 
     @property
-    def extremity_indices(self) -> Set[int]:
+    def extremity_indices(self) -> List[int]:
         # TODO refactor
         for p in self.polygons:
             p._find_extremities()
         # Attention: only consider extremities that are actually within the map
-        return {idx for idx, v in enumerate(self.all_vertices) if v.is_extremity and self.within_map(v.coordinates)}
+        return [idx for idx, v in enumerate(self.all_vertices) if v.is_extremity and self.within_map(v.coordinates)]
 
     @property
     def extremity_mask(self) -> np.ndarray:
@@ -95,14 +109,13 @@ def extremity_mask(self) -> np.ndarray:
     @property
     def all_extremities(self) -> List[PolygonVertex]:
         return [self.all_vertices[i] for i in self.extremity_indices]
-
         # TODO
         # if self._all_extremities is None:
         # return self._all_extremities
 
     @property
-    def all_edges(self) -> Set[Edge]:
-        return set(itertools.chain(*iter(p.edges for p in self.polygons)))
+    def all_edges(self) -> List[Edge]:
+        return list(itertools.chain(*iter(p.edges for p in self.polygons)))
 
     def store(
         self,
@@ -183,7 +196,7 @@ def prepare(self):  # TODO include in storing functions? breaking change!
         Computes all directly reachable extremities based on visibility and their distance to each other
 
         .. note::
-            Multiple polygon vertices might have identical coords.
+            Multiple polygon vertices might have identical coords_rel.
             They must be treated as distinct vertices here, since their attached edges determine visibility.
             In the created graph however, these nodes must be merged at the end to avoid ambiguities!
 
@@ -207,21 +220,19 @@ def prepare(self):  # TODO include in storing functions? breaking change!
             self.graph = DirectedHeuristicGraph(extremities)
             return
 
+        # TODO pre computation and storage of all
         vertices = self.all_vertices
-        nr_vertices = len(vertices)
+        nr_vertices = self.nr_vertices
         extremity_indices = self.extremity_indices
         extremity_mask = self.extremity_mask
-
         # TODO more performant way of computing
-        all_edges = list(self.all_edges)
-        graph = DirectedHeuristicGraph(extremities)
-        coords = np.stack([v.coordinates for v in vertices])
-        vertex_edge_idxs = np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in vertices])
+        all_edges = self.all_edges
+        coords = self.coords
+        vertex_edge_idxs = self.vertex_edge_idxs
+        edge_vertex_idxs = self.edge_vertex_idxs
+
         # TODO sparse matrix. problematic: default value is 0.0
-        angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
-        edge_vertex_idxs = np.stack(
-            [(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in all_edges]
-        )
+        # angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
 
         if len(extremity_indices) != len(extremities):
             raise ValueError
@@ -229,7 +240,7 @@ def prepare(self):  # TODO include in storing functions? breaking change!
         # TODO reuse
         def get_repr(idx_origin, i):
             coords_origin = coords[idx_origin]
-            return get_angle_repr(coords_origin, i, angle_representations, coords)
+            return get_angle_repr(coords_origin, coords[i])
 
         def get_neighbours(i: int) -> Tuple[int, int]:
             edge_idx1, edge_idx2 = vertex_edge_idxs[i]
@@ -237,16 +248,16 @@ def get_neighbours(i: int) -> Tuple[int, int]:
             neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
             return neigh_idx1, neigh_idx2
 
-        def get_coordinates_translated(orig_idx: int, i: int) -> np.ndarray:
+        def get_relative_coords(orig_idx: int, i: int) -> np.ndarray:
             coords_origin = coords[orig_idx]
             coords_v = coords[i]
             return coords_v - coords_origin
 
         def get_distance_to_origin(orig_idx: int, i: int) -> float:
-            coords = get_coordinates_translated(orig_idx, i)
-            return np.linalg.norm(coords, ord=2)
+            coords_rel = get_relative_coords(orig_idx, i)
+            return np.linalg.norm(coords_rel, ord=2)
 
-        extremity_indices = list(extremity_indices)
+        graph = DirectedHeuristicGraph(extremities)
         # TODO use orig_ptr
         for _orig_ptr, origin_idx in enumerate(extremity_indices):
             # vertices all belong to a polygon
@@ -265,7 +276,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             vert_idx2dist = {i: get_distance_to_origin(origin_idx, i) for i in range(nr_vertices)}
 
             idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
-            # only consider extremities with coords different from the query extremity
+            # only consider extremities with coords_rel different from the query extremity
             # (angle representation not None)
             # the origin extremity itself must also not be checked when looking for visible neighbours
             idx2repr = {i: r for i, r in idx2repr.items() if r is not None}
@@ -314,7 +325,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             n2_repr_inv = angle_rep_inverse(n2_repr)
 
             # IMPORTANT: check all extremities here, not just current candidates
-            # do not check extremities with equal coords (also query extremity itself!)
+            # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
             lie_in_front_idx = find_within_range2(
                 n1_repr_inv,
@@ -338,7 +349,6 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             coords_origin = coords[origin_idx]
             visible_idxs = find_visible2(
                 extremity_mask,
-                angle_representations,
                 coords,
                 vertex_edge_idxs,
                 edge_vertex_idxs,
@@ -348,16 +358,27 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
                 vert_idx2dist,
             )
 
+            self.translate(vertices[origin_idx])
+            candidates = {vertices[i] for i in idx2repr.keys()}
+            edges2check = {all_edges[i] for i in edge_idxs2check}
+            visibles_n_distances_goal = find_visible(candidates, edges_to_check=edges2check)
+            visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_goal}
+
             # TODO graph: also use indices instead of vertices
             origin_extremity = vertices[origin_idx]
             visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
+
+            if not visible_idxs_ == visible_idxs:
+                _ = 1
+                # raise ValueError
+
             graph.add_multiple_undirected_edges(origin_extremity, visible_vertex2dist_map)
             # optimisation: "thin out" the graph
             # remove already existing edges in the graph to the extremities in front
             lie_in_front = {vertices[i] for i in lie_in_front_idx}
             graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
 
-        graph.make_clean()  # join all nodes with the same coords
+        graph.make_clean()  # join all nodes with the same coords_rel
 
         self.graph = graph
         self.prepared = True
@@ -394,6 +415,46 @@ def find_shortest_path(
          if points close to or on polygon edges should be accepted as valid input, set this to ``False``.
         :return: a tuple of shortest path and its length. ([], None) if there is no possible path.
         """
+
+        # TODO pre computation and storage of all
+        all_edges = self.all_edges
+        coords_start = np.array(start_coordinates)
+        coords_goal = np.array(goal_coordinates)
+        coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
+        # TODO more performant way of computing
+
+        # TODO reuse
+        def get_relative_coords(coords_origin: np.ndarray, i: int) -> np.ndarray:
+            coords_v = coords[i]
+            return coords_v - coords_origin
+
+        def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
+            coords_rel = get_relative_coords(coords_origin, i)
+            return np.linalg.norm(coords_rel, ord=2)
+
+        vertex_edge_idxs = self.vertex_edge_idxs
+        edge_vertex_idxs = self.edge_vertex_idxs
+        extremity_mask = np.append(self.extremity_mask, (False, False))
+
+        idx_start = self.nr_vertices
+        idx_goal = self.nr_vertices + 1
+        nr_vertices = self.nr_vertices + 2
+        # id_map_dist_to_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
+        id_map_dist_to_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
+
+        # TODO detect if coordinates match an extremity (node in the graph). extend only on demand
+        # TODO extend id_map_dist_to
+        # extemity_indices = self.extremity_indices
+        # skip_start_check = False
+        # for i in extemity_indices:
+        #     dist = id_map_dist_to_start[i]
+        #     if dist == 0:
+        #         idx_start = i
+        #         skip_start_check = True
+        #         break
+        # if not skip:
+        #     coords = np.append(coords, coords_start, axis=0)
+
         # path planning query:
         # make sure the map has been loaded and prepared
         if self.boundary_polygon is None:
@@ -405,7 +466,7 @@ def find_shortest_path(
             raise ValueError("start point does not lie within the map")
         if verify and not self.within_map(goal_coordinates):
             raise ValueError("goal point does not lie within the map")
-        if start_coordinates == goal_coordinates:
+        if np.array_equal(coords_start, coords_goal):
             # start and goal are identical and can be reached instantly
             return [start_coordinates, goal_coordinates], 0.0
 
@@ -418,6 +479,9 @@ def find_shortest_path(
         start_vertex = Vertex(start_coordinates)
         goal_vertex = Vertex(goal_coordinates)
 
+        # TODO required?!
+        vertices = self.all_vertices + [start_vertex, goal_vertex]
+
         # check the goal node first (earlier termination possible)
         self.translate(new_origin=goal_vertex)  # do before checking angle representations!
         # IMPORTANT: manually translate the start vertex, because it is not part of any polygon
@@ -433,12 +497,45 @@ def find_shortest_path(
                 self.graph.get_all_nodes(),
             )
         )
-        # IMPORTANT: check if the start node is visible from the goal node!
+        # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
         candidates.add(start_vertex)
 
-        # TODO use new variant
         visibles_n_distances_goal = find_visible(candidates, edges_to_check=self.all_edges)
+
+        # TODO visibility of start vertex
+        # TODO use new variant
+        visible_idxs = {vertices.index(v) for v, d in visibles_n_distances_goal}
+        nr_edges = len(all_edges)
+        edge_idxs2check = set(range(nr_edges))
+        idx_origin = idx_goal
+        coords_origin = coords[idx_origin]
+        # TODO
+        cand_idxs = {vertices.index(n) for n in self.graph.get_all_nodes()}
+        cand_idxs.add(idx_start)
+        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in cand_idxs}
+        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
+
+        vert_idx2dist = id_map_dist_to_goal
+
+        # TODO
+        visible_idxs_ = find_visible2(
+            extremity_mask,
+            coords,
+            vertex_edge_idxs,
+            edge_vertex_idxs,
+            edge_idxs2check,
+            coords_origin,
+            cand_idx2repr,
+            vert_idx2dist,
+        )
+        if not visible_idxs == visible_idxs_:
+            _ = 1
+            # raise ValueError
+
+        # visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(coords_origin, i) for i in visible_idxs}
+        visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
+
         if len(visibles_n_distances_goal) == 0:
             # The goal node does not have any neighbours. Hence there is not possible path to the goal.
             return [], None
@@ -473,7 +570,7 @@ def find_shortest_path(
         # TODO use new variant
         visibles_n_distances_start = find_visible(candidates, edges_to_check=self.all_edges)
         if len(visibles_n_distances_start) == 0:
-            # The start node does not have any neighbours. Hence there is not possible path to the goal.
+            # The start node does not have any neighbours. Hence there is no possible path to the goal.
             return [], None
 
         # add edges in the direction: start -> extremity
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 0f6931d..2b02aa3 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -234,8 +234,7 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
 #     return repr
 
 
-def get_angle_repr(coords_origin: np.ndarray, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
-    coords_v = coordinates[idx_v]
+def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
     repr = compute_angle_repr(coords_origin, coords_v)
     # TODO
     assert repr is None or not np.isnan(repr)
@@ -529,7 +528,7 @@ def find_visible(vertex_candidates, edges_to_check):
     :param edges_to_check: the set of edges which determine visibility
     :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
     """
-
+    edges_to_check = set(edges_to_check)
     visible_vertices = set()
     if len(vertex_candidates) == 0:
         return visible_vertices
@@ -693,7 +692,6 @@ def find_visible(vertex_candidates, edges_to_check):
 
 def find_visible2(
     extremity_mask: np.ndarray,
-    angle_representations: np.ndarray,
     coords: np.ndarray,
     vertex_edge_idxs: np.ndarray,
     edge_vertex_idxs: np.ndarray,
@@ -727,7 +725,7 @@ def get_distance_to_origin(i: int) -> float:
         # return np.linalg.norm(coords, ord=2)
 
     def get_repr(i: int) -> float:
-        return get_angle_repr(coords_origin, i, angle_representations, coords)
+        return get_angle_repr(coords_origin, coords[i])
 
     def get_neighbours(i: int) -> Tuple[int, int]:
         edge_idx1, edge_idx2 = vertex_edge_idxs[i]
diff --git a/tests/main_test.py b/tests/main_test.py
index eefd7d6..86a4736 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -345,7 +345,7 @@ def test_grid_env(self):
         grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
 
         grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False)
-        nr_extremities = len(list(grid_env.all_extremities))
+        nr_extremities = len(grid_env.all_extremities)
         assert nr_extremities == 17, "extremities do not get detected correctly!"
         grid_env.prepare()
         nr_graph_nodes = len(grid_env.graph.all_nodes)

From 54b462933600fad5ac5579be211366eab5e49b0b Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Wed, 17 Aug 2022 21:11:31 +0200
Subject: [PATCH 21/44] working idx based graph

---
 extremitypathfinder/extremitypathfinder.py | 187 ++++++++++++---------
 extremitypathfinder/helper_classes.py      |  82 ++++-----
 extremitypathfinder/plotting.py            |  26 +--
 3 files changed, 165 insertions(+), 130 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 9c44668..e5c6624 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -26,7 +26,6 @@
     convert_gridworld,
     find_visible,
     find_visible2,
-    find_within_range,
     find_within_range2,
     get_angle_repr,
     inside_polygon,
@@ -217,7 +216,7 @@ def prepare(self):  # TODO include in storing functions? breaking change!
         nr_extremities = len(extremities)
         if nr_extremities == 0:
             # TODO
-            self.graph = DirectedHeuristicGraph(extremities)
+            self.graph = DirectedHeuristicGraph()
             return
 
         # TODO pre computation and storage of all
@@ -257,7 +256,8 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             coords_rel = get_relative_coords(orig_idx, i)
             return np.linalg.norm(coords_rel, ord=2)
 
-        graph = DirectedHeuristicGraph(extremities)
+        extremity_coord_map = {i: coords[i] for i in extremity_indices}
+        graph = DirectedHeuristicGraph(extremity_coord_map)
         # TODO use orig_ptr
         for _orig_ptr, origin_idx in enumerate(extremity_indices):
             # vertices all belong to a polygon
@@ -327,7 +327,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            lie_in_front_idx = find_within_range2(
+            lie_in_front_idxs = find_within_range2(
                 n1_repr_inv,
                 n2_repr_inv,
                 idx2repr,
@@ -338,7 +338,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
             # do not consider points found to lie behind
-            idx2repr = {i: r for i, r in idx2repr.items() if i not in lie_in_front_idx and i not in idxs_behind}
+            idx2repr = {i: r for i, r in idx2repr.items() if i not in lie_in_front_idxs and i not in idxs_behind}
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
@@ -364,19 +364,16 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             visibles_n_distances_goal = find_visible(candidates, edges_to_check=edges2check)
             visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_goal}
 
-            # TODO graph: also use indices instead of vertices
-            origin_extremity = vertices[origin_idx]
-            visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
+            visible_vertex2dist_map = {i: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
 
             if not visible_idxs_ == visible_idxs:
                 _ = 1
                 # raise ValueError
 
-            graph.add_multiple_undirected_edges(origin_extremity, visible_vertex2dist_map)
+            graph.add_multiple_undirected_edges(origin_idx, visible_vertex2dist_map)
             # optimisation: "thin out" the graph
             # remove already existing edges in the graph to the extremities in front
-            lie_in_front = {vertices[i] for i in lie_in_front_idx}
-            graph.remove_multiple_undirected_edges(origin_extremity, lie_in_front)
+            graph.remove_multiple_undirected_edges(origin_idx, lie_in_front_idxs)
 
         graph.make_clean()  # join all nodes with the same coords_rel
 
@@ -409,7 +406,7 @@ def find_shortest_path(
 
         :param start_coordinates: a (x,y) coordinate tuple representing the start node
         :param goal_coordinates:  a (x,y) coordinate tuple representing the goal node
-        :param free_space_after: whether the created temporary search graph self.temp_graph
+        :param free_space_after: whether the created temporary search graph temp_graph
             should be deleted after the query
         :param verify: whether it should be checked if start and goal points really lie inside the environment.
          if points close to or on polygon edges should be accepted as valid input, set this to ``False``.
@@ -421,6 +418,7 @@ def find_shortest_path(
         coords_start = np.array(start_coordinates)
         coords_goal = np.array(goal_coordinates)
         coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
+
         # TODO more performant way of computing
 
         # TODO reuse
@@ -439,7 +437,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         idx_start = self.nr_vertices
         idx_goal = self.nr_vertices + 1
         nr_vertices = self.nr_vertices + 2
-        # id_map_dist_to_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
+        id_map_dist_to_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
         id_map_dist_to_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
 
         # TODO detect if coordinates match an extremity (node in the graph). extend only on demand
@@ -491,16 +489,17 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # the visibility of only the graphs nodes has to be checked (not all extremities!)
         # points with the same angle representation should not be considered visible
         # (they also cause errors in the algorithms, because their angle repr is not defined!)
-        candidates = set(
-            filter(
-                lambda n: n.get_angle_representation() is not None,
-                self.graph.get_all_nodes(),
-            )
-        )
+        candidate_idxs = self.graph.get_all_nodes()
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
-        candidates.add(start_vertex)
+        candidate_idxs.add(idx_start)
+        idx_origin = idx_goal
+        coords_origin = coords[idx_origin]
+        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in candidate_idxs}
+        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
+        candidate_idxs = set(cand_idx2repr.keys())
 
+        candidates = {vertices[i] for i in candidate_idxs}
         visibles_n_distances_goal = find_visible(candidates, edges_to_check=self.all_edges)
 
         # TODO visibility of start vertex
@@ -508,13 +507,10 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         visible_idxs = {vertices.index(v) for v, d in visibles_n_distances_goal}
         nr_edges = len(all_edges)
         edge_idxs2check = set(range(nr_edges))
-        idx_origin = idx_goal
-        coords_origin = coords[idx_origin]
+
         # TODO
-        cand_idxs = {vertices.index(n) for n in self.graph.get_all_nodes()}
-        cand_idxs.add(idx_start)
-        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in cand_idxs}
-        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
+        # cand_idxs = {vertices.index(n) for n in self.graph.get_all_nodes()}
+        # cand_idxs.add(idx_start)
 
         vert_idx2dist = id_map_dist_to_goal
 
@@ -529,9 +525,9 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             cand_idx2repr,
             vert_idx2dist,
         )
-        if not visible_idxs == visible_idxs_:
-            _ = 1
-            # raise ValueError
+        # if not visible_idxs == visible_idxs_:
+        #     _ = 1
+        #     raise ValueError
 
         # visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(coords_origin, i) for i in visible_idxs}
         visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
@@ -543,7 +539,9 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # create temporary graph TODO make more performant, avoid real copy
         # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
         # but to still not create real copies of vertex instances!
-        self.temp_graph = deepcopy(self.graph)
+        temp_graph = deepcopy(self.graph)
+        # ATTENTION: update to new coordinates
+        temp_graph.coords = coords
 
         # IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node
         #   instead of visiting other nodes first (there is never an advantage through reduced edge weight)
@@ -555,76 +553,111 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
 
             # add unidirectional edges to the temporary graph
             # add edges in the direction: extremity (v) -> goal
-            self.temp_graph.add_directed_edge(v, goal_vertex, d)
+            v_idx = vertices.index(v)
+            temp_graph.add_directed_edge(v_idx, idx_goal, d)
 
         self.translate(new_origin=start_vertex)  # do before checking angle representations!
         # the visibility of only the graphs nodes have to be checked
         # the goal node does not have to be considered, because of the earlier check
-        candidates = set(
-            filter(
-                lambda n: n.get_angle_representation() is not None,
-                self.graph.get_all_nodes(),
-            )
-        )
+        idx_origin = idx_start
+        coords_origin = coords[idx_origin]
+        candidate_idxs = self.graph.get_all_nodes()
+        # candidate_idxs.remove(idx_goal)
+        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in candidate_idxs}
+        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
+        candidate_idxs = set(cand_idx2repr.keys())
+        candidates = {vertices[i] for i in candidate_idxs}
 
         # TODO use new variant
         visibles_n_distances_start = find_visible(candidates, edges_to_check=self.all_edges)
-        if len(visibles_n_distances_start) == 0:
+        visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_start}
+
+        edge_idxs2check = set(range(nr_edges))  # new copy
+        vert_idx2dist = id_map_dist_to_start
+        visible_idxs = find_visible2(
+            extremity_mask,
+            coords,
+            vertex_edge_idxs,
+            edge_vertex_idxs,
+            edge_idxs2check,
+            coords_origin,
+            cand_idx2repr,
+            vert_idx2dist,
+        )
+
+        if not visible_idxs == visible_idxs_:
+            # _ = 1
+            raise ValueError
+
+        if len(visible_idxs) == 0:
             # The start node does not have any neighbours. Hence there is no possible path to the goal.
             return [], None
 
         # add edges in the direction: start -> extremity
-        self.temp_graph.add_multiple_directed_edges(start_vertex, visibles_n_distances_start)
+        visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
+        # visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
+        if idx_start in visible_idxs:
+            raise ValueError
+        temp_graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
 
+        # TODO optimisation
         # also here unnecessary edges in the graph can be deleted when start or goal lie in front of visible extremities
         # IMPORTANT: when a query point happens to coincide with an extremity, edges to the (visible) extremities
         #  in front MUST be added to the graph! Handled by always introducing new (non extremity, non polygon) vertices.
-
         # for every extremity that is visible from either goal or start
-        # NOTE: edges are undirected! self.temp_graph.get_neighbours_of(start_vertex) == set()
-        # neighbours_start = self.temp_graph.get_neighbours_of(start_vertex)
+        # NOTE: edges are undirected! temp_graph.get_neighbours_of(start_vertex) == set()
+        # neighbours_start = temp_graph.get_neighbours_of(start_vertex)
         neighbours_start = {n for n, d in visibles_n_distances_start}
         # the goal vertex might be marked visible, it is not an extremity -> skip
         neighbours_start.discard(goal_vertex)
-        neighbours_goal = self.temp_graph.get_neighbours_of(goal_vertex)
-        for vertex in neighbours_start | neighbours_goal:
-            # assert type(vertex) == PolygonVertex and vertex.is_extremity
-
-            # check only if point is visible
-            temp_candidates = set()
-            if vertex in neighbours_start:
-                temp_candidates.add(start_vertex)
-
-            if vertex in neighbours_goal:
-                temp_candidates.add(goal_vertex)
-
-            if len(temp_candidates) > 0:
-                self.translate(new_origin=vertex)
-                # IMPORTANT: manually translate the goal and start vertices
-                start_vertex.mark_outdated()
-                goal_vertex.mark_outdated()
-
-                n1, n2 = vertex.get_neighbours()
-                repr1 = angle_rep_inverse(n1.get_angle_representation())  # rotated 180 deg
-                repr2 = angle_rep_inverse(n2.get_angle_representation())
-
-                # IMPORTANT: special case:
-                # here the nodes must stay connected if they have the same angle representation!
-                lie_in_front = find_within_range(
-                    repr1,
-                    repr2,
-                    temp_candidates,
-                    angle_range_less_180=True,
-                    equal_repr_allowed=False,
-                )
-                self.temp_graph.remove_multiple_undirected_edges(vertex, lie_in_front)
+        # neighbour_idxs_goal = temp_graph.get_neighbours_of(idx_goal)
+        # neighbours_goal = {vertices[i] for i in neighbour_idxs_goal}
+        # neighbours = neighbours_start | neighbours_goal
+        # for vertex in neighbours:
+        #     # assert type(vertex) == PolygonVertex and vertex.is_extremity
+        #
+        #     # vertex = vertices[vertex_idx]
+        #     vertex_idx = vertices.index(vertex)
+        #
+        #     # check only if point is visible
+        #     temp_candidates = set()
+        #     if vertex in neighbours_start:
+        #         temp_candidates.add(start_vertex)
+        #
+        #     if vertex in neighbours_goal:
+        #         temp_candidates.add(goal_vertex)
+        #
+        #     if len(temp_candidates) == 0:
+        #         continue
+        #     self.translate(new_origin=vertex)
+        #     # IMPORTANT: manually translate the goal and start vertices
+        #     start_vertex.mark_outdated()
+        #     goal_vertex.mark_outdated()
+        #
+        #     n1, n2 = vertex.get_neighbours()
+        #     repr1 = angle_rep_inverse(n1.get_angle_representation())  # rotated 180 deg
+        #     repr2 = angle_rep_inverse(n2.get_angle_representation())
+        #
+        #     # IMPORTANT: special case:
+        #     # here the nodes must stay connected if they have the same angle representation!
+        #     lie_in_front = find_within_range(
+        #         repr1,
+        #         repr2,
+        #         temp_candidates,
+        #         angle_range_less_180=True,
+        #         equal_repr_allowed=False,
+        #     )
+        #     lie_in_front_idxs = iter(vertices.index(v) for v in lie_in_front)
+        #     temp_graph.remove_multiple_undirected_edges(vertex_idx, lie_in_front_idxs)
 
         # NOTE: exploiting property 2 from [1] here would be more expensive than beneficial
-        vertex_path, distance = self.temp_graph.modified_a_star(start_vertex, goal_vertex)
+        vertex_id_path, distance = temp_graph.modified_a_star(idx_start, idx_goal, coords_goal)
+        vertex_path = [vertices[i] for i in vertex_id_path]
 
         if free_space_after:
-            del self.temp_graph  # free the memory
-
+            del temp_graph  # free the memory
+        else:
+            self.temp_graph = temp_graph
         # extract the coordinates from the path
         return [tuple(v.coordinates) for v in vertex_path], distance
 
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 0296898..f8e4fc0 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -1,5 +1,5 @@
 import heapq
-from typing import Dict, Iterable, List, Optional, Set
+from typing import Dict, Iterable, Iterator, List, Optional, Set, Tuple
 
 import numpy as np
 
@@ -328,17 +328,29 @@ def get(self):
         return s.node, s.neighbours, s.distance, s.path, s.cost_so_far
 
 
+def get_distance_to_origin(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
+    coords_rel = coords_v - coords_origin
+    return np.linalg.norm(coords_rel, ord=2)
+
+
+NodeId = int
+
+
 # TODO often empty sets in self.neighbours
 class DirectedHeuristicGraph(object):
-    __slots__ = ["all_nodes", "distances", "goal_node", "heuristic", "neighbours"]
+    __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coords"]
 
-    def __init__(self, all_nodes: Optional[Iterable[Vertex]] = None):
-        self.distances: Dict = {}
-        self.neighbours: Dict = {}
+    def __init__(self, coords: Optional[Dict[NodeId, np.ndarray]] = None):
+        self.distances: Dict[Tuple[NodeId, NodeId], float] = {}
+        self.neighbours: Dict[NodeId, Set[NodeId]] = {}
 
-        if all_nodes is None:
+        if coords is None:
             all_nodes = set()
-        self.all_nodes: Set[Vertex] = set(all_nodes)  # independent copy required!
+        else:
+            all_nodes = set(coords.keys())
+
+        self.all_nodes: Set[NodeId] = set(all_nodes)  # independent copy required!
+        self.coords: Dict[NodeId, np.ndarray] = coords
 
         # TODO use same set as extremities of env, but different for copy!
 
@@ -346,8 +358,8 @@ def __init__(self, all_nodes: Optional[Iterable[Vertex]] = None):
         # <=> must always be lowest for node with the POSSIBLY lowest cost
         # <=> heuristic is LOWER BOUND for the cost
         # the heuristic here: distance to the goal node (is always the shortest possible distance!)
-        self.heuristic: dict = {}
-        self.goal_node: Optional[Vertex] = None
+        self.heuristic: Dict[NodeId, float] = {}
+        self.goal_coords: Optional[np.ndarray] = None
 
     def __deepcopy__(self, memodict=None):
         # returns an independent copy (nodes can be added without changing the original graph),
@@ -358,39 +370,29 @@ def __deepcopy__(self, memodict=None):
         independent_copy.all_nodes = self.all_nodes.copy()
         return independent_copy
 
-    def get_all_nodes(self):
+    def get_all_nodes(self) -> Set[NodeId]:
         return self.all_nodes
 
-    def get_neighbours(self):
-        return self.neighbours.items()
+    def get_neighbours(self) -> Iterable:
+        yield from self.neighbours.items()
 
-    def get_neighbours_of(self, node):
+    def get_neighbours_of(self, node: NodeId) -> Set[NodeId]:
         return self.neighbours.get(node, set())
 
-    def get_distance(self, node1, node2):
+    def get_distance(self, node1: NodeId, node2: NodeId) -> float:
         # directed edges: just one direction is being stored
         return self.distances[(node1, node2)]
 
-    def get_heuristic(self, node):
-        global origin  # use origin contains the current goal node
+    def get_heuristic(self, node: NodeId) -> float:
         # lazy evaluation:
         h = self.heuristic.get(node, None)
         if h is None:
             # has been reset, compute again
-            h = np.linalg.norm(node.coordinates - origin.coordinates)
+            h = get_distance_to_origin(self.goal_coords, self.coords[node])
             self.heuristic[node] = h
         return h
 
-    def set_goal_node(self, goal_node):
-        assert goal_node in self.all_nodes  # has no outgoing edges -> no neighbours
-        # IMPORTANT: while using heuristic graph (a star), do not change origin!
-        global origin
-        origin = goal_node  # use origin for temporally storing the goal node
-        self.goal_node = goal_node
-        # reset heuristic for all
-        self.heuristic.clear()
-
-    def neighbours_of(self, node1):
+    def neighbours_of(self, node1: NodeId) -> Iterator[NodeId]:
         # optimisation:
         #   return the neighbours ordered after their cost estimate: distance+ heuristic (= current-next + next-goal)
         #   -> when the goal is reachable return it first (-> a star search terminates)
@@ -406,27 +408,27 @@ def entry_generator(neighbours, distances):
         # yield node, distance, cost_estimate= distance + heuristic
         yield from out_sorted
 
-    def add_directed_edge(self, node1, node2, distance):
+    def add_directed_edge(self, node1: NodeId, node2: NodeId, distance: float):
         assert node1 != node2  # no self loops allowed!
         self.neighbours.setdefault(node1, set()).add(node2)
         self.distances[(node1, node2)] = distance
         self.all_nodes.add(node1)
         self.all_nodes.add(node2)
 
-    def add_undirected_edge(self, node1, node2, distance):
+    def add_undirected_edge(self, node1: NodeId, node2: NodeId, distance: float):
         assert node1 != node2  # no self loops allowed!
         self.add_directed_edge(node1, node2, distance)
         self.add_directed_edge(node2, node1, distance)
 
-    def add_multiple_undirected_edges(self, node1, node_distance_map: Dict):
+    def add_multiple_undirected_edges(self, node1: NodeId, node_distance_map: Dict[NodeId, float]):
         for node2, distance in node_distance_map.items():
             self.add_undirected_edge(node1, node2, distance)
 
-    def add_multiple_directed_edges(self, node1, node_distance_iter):
-        for node2, distance in node_distance_iter:
+    def add_multiple_directed_edges(self, node1: NodeId, node_distance_iter: Dict[NodeId, float]):
+        for node2, distance in node_distance_iter.items():
             self.add_directed_edge(node1, node2, distance)
 
-    def remove_directed_edge(self, n1, n2):
+    def remove_directed_edge(self, n1: NodeId, n2: NodeId):
         neighbours = self.neighbours.get(n1)
         if neighbours is not None:
             neighbours.discard(n2)
@@ -435,12 +437,12 @@ def remove_directed_edge(self, n1, n2):
             # the node must still be kept, since with the addition of start and goal nodes during a query
             # the node might become reachable!
 
-    def remove_undirected_edge(self, node1, node2):
+    def remove_undirected_edge(self, node1: NodeId, node2: NodeId):
         # should work even if edge does not exist yet
         self.remove_directed_edge(node1, node2)
         self.remove_directed_edge(node2, node1)
 
-    def remove_multiple_undirected_edges(self, node1, node2_iter):
+    def remove_multiple_undirected_edges(self, node1: NodeId, node2_iter: Iterable[NodeId]):
         for node2 in node2_iter:
             self.remove_undirected_edge(node1, node2)
 
@@ -451,11 +453,11 @@ def make_clean(self):
 
     def join_identical(self):
         # join all nodes with the same coordinates,
-        nodes_to_check = self.get_all_nodes().copy()
+        nodes_to_check = self.all_nodes.copy()
         while len(nodes_to_check) > 1:
             n1 = nodes_to_check.pop()
-            coordinates1 = n1.coordinates
-            same_nodes = {n for n in nodes_to_check if np.allclose(coordinates1, n.coordinates)}
+            coordinates1 = self.coords[n1]
+            same_nodes = {n for n in nodes_to_check if np.allclose(coordinates1, self.coords[n])}
             nodes_to_check.difference_update(same_nodes)
             for n2 in same_nodes:
                 # print('removing duplicate node', n2)
@@ -472,7 +474,7 @@ def join_identical(self):
 
                 self.all_nodes.remove(n2)
 
-    def modified_a_star(self, start: Vertex, goal: Vertex):
+    def modified_a_star(self, start: int, goal: int, goal_coords: np.ndarray) -> Tuple[List[int], Optional[float]]:
         """implementation of the popular A* algorithm with optimisations for this special use case
 
         IMPORTANT: geometrical property of this problem (and hence also the extracted graph):
@@ -515,7 +517,7 @@ def enqueue_neighbours():
             state = SearchState(next_node, distance, neighbours, path, cost_so_far, cost_estim)
             search_state_queue.put(state)
 
-        self.set_goal_node(goal)  # lazy update of the heuristic
+        self.goal_coords = goal_coords  # lazy update of the heuristic
 
         search_state_queue = SearchStateQueue()
         current_node = start
diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index da4caaf..6beec74 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -43,12 +43,8 @@ def mark_points(vertex_iter, **kwargs):
 
 
 def draw_edge(v1, v2, c, alpha, **kwargs):
-    if type(v1) == tuple:
-        x1, y1 = v1
-        x2, y2 = v2
-    else:
-        x1, y1 = v1.coordinates
-        x2, y2 = v2.coordinates
+    x1, y1 = v1
+    x2, y2 = v2
     plt.plot([x1, x2], [y1, y2], color=c, alpha=alpha, **kwargs)
 
 
@@ -70,7 +66,10 @@ def draw_boundaries(map, ax):
 
 
 def draw_internal_graph(map, ax):
-    for start, all_goals in map.graph.get_neighbours():
+    graph = map.graph
+    for start_idx, all_goal_idxs in graph.get_neighbours():
+        start = graph.coords[start_idx]
+        all_goals = [graph.coords[i] for i in all_goal_idxs]
         for goal in all_goals:
             draw_edge(start, goal, c="red", alpha=0.2, linewidth=2)
 
@@ -166,14 +165,15 @@ def draw_only_path(map, vertex_path):
 def draw_graph(map, graph):
     fig, ax = plt.subplots()
 
-    all_nodes = graph.get_all_nodes()
+    all_node_idxs = graph.get_all_nodes()
+    all_nodes = [graph.coords[i] for i in all_node_idxs]
     mark_points(all_nodes, c="black", s=30)
 
-    for n in all_nodes:
-        x, y = n.coordinates
-        neighbours = graph.get_neighbours_of(n)
-        for n2 in neighbours:
-            x2, y2 = n2.coordinates
+    for i in all_node_idxs:
+        x, y = graph.coords[i]
+        neighbour_idxs = graph.get_neighbours_of(i)
+        for n2_idx in neighbour_idxs:
+            x2, y2 = graph.coords[n2_idx]
             dx, dy = x2 - x, y2 - y
             plt.arrow(
                 x,

From 729ff9ab0f90b259db41b3f0a69919700fba7aef Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 00:48:27 +0200
Subject: [PATCH 22/44] working intermediary state

---
 CHANGELOG.rst                              |  12 ++
 Makefile                                   |   2 +-
 extremitypathfinder/extremitypathfinder.py | 167 ++++++++++++---------
 extremitypathfinder/helper_classes.py      |  64 ++++----
 extremitypathfinder/helper_fcts.py         |  95 ++++++++----
 extremitypathfinder/plotting.py            |  76 ++++++----
 tests/main_test.py                         |   4 +-
 7 files changed, 256 insertions(+), 164 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index e9e798c..91bdd6b 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -1,6 +1,18 @@
 Changelog
 =========
 
+2.2.4 (2022-xx)
+-------------------
+
+internal:
+
+* added test cases
+
+
+TODO remove separate prepare step?!
+
+
+
 2.2.3 (2022-10-11)
 -------------------
 
diff --git a/Makefile b/Makefile
index f065ce4..ad35000 100644
--- a/Makefile
+++ b/Makefile
@@ -19,7 +19,7 @@ hook:
 	@pre-commit install
 	@pre-commit run --all-files
 
-hookupdate:
+hook2:
 	@pre-commit autoupdate
 
 clean:
diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index e5c6624..f6c46a0 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -274,6 +274,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
 
             # TODO lazy init? same as angle repr
             vert_idx2dist = {i: get_distance_to_origin(origin_idx, i) for i in range(nr_vertices)}
+            vert_idx2repr = {i: get_repr(origin_idx, i) for i in range(nr_vertices)}
 
             idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
             # only consider extremities with coords_rel different from the query extremity
@@ -338,7 +339,9 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
             # do not consider points found to lie behind
+            # TODO remvoe
             idx2repr = {i: r for i, r in idx2repr.items() if i not in lie_in_front_idxs and i not in idxs_behind}
+            candidate_idxs = set(idx2repr.keys())
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
@@ -354,8 +357,9 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
                 edge_vertex_idxs,
                 edge_idxs2check,
                 coords_origin,
-                idx2repr,
+                vert_idx2repr,
                 vert_idx2dist,
+                candidate_idxs,
             )
 
             self.translate(vertices[origin_idx])
@@ -413,14 +417,6 @@ def find_shortest_path(
         :return: a tuple of shortest path and its length. ([], None) if there is no possible path.
         """
 
-        # TODO pre computation and storage of all
-        all_edges = self.all_edges
-        coords_start = np.array(start_coordinates)
-        coords_goal = np.array(goal_coordinates)
-        coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
-
-        # TODO more performant way of computing
-
         # TODO reuse
         def get_relative_coords(coords_origin: np.ndarray, i: int) -> np.ndarray:
             coords_v = coords[i]
@@ -430,29 +426,6 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             coords_rel = get_relative_coords(coords_origin, i)
             return np.linalg.norm(coords_rel, ord=2)
 
-        vertex_edge_idxs = self.vertex_edge_idxs
-        edge_vertex_idxs = self.edge_vertex_idxs
-        extremity_mask = np.append(self.extremity_mask, (False, False))
-
-        idx_start = self.nr_vertices
-        idx_goal = self.nr_vertices + 1
-        nr_vertices = self.nr_vertices + 2
-        id_map_dist_to_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
-        id_map_dist_to_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
-
-        # TODO detect if coordinates match an extremity (node in the graph). extend only on demand
-        # TODO extend id_map_dist_to
-        # extemity_indices = self.extremity_indices
-        # skip_start_check = False
-        # for i in extemity_indices:
-        #     dist = id_map_dist_to_start[i]
-        #     if dist == 0:
-        #         idx_start = i
-        #         skip_start_check = True
-        #         break
-        # if not skip:
-        #     coords = np.append(coords, coords_start, axis=0)
-
         # path planning query:
         # make sure the map has been loaded and prepared
         if self.boundary_polygon is None:
@@ -464,23 +437,71 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             raise ValueError("start point does not lie within the map")
         if verify and not self.within_map(goal_coordinates):
             raise ValueError("goal point does not lie within the map")
+
+        coords_start = np.array(start_coordinates)
+        coords_goal = np.array(goal_coordinates)
         if np.array_equal(coords_start, coords_goal):
             # start and goal are identical and can be reached instantly
             return [start_coordinates, goal_coordinates], 0.0
 
-        # could check if start and goal nodes have identical coordinates with one of the vertices
+        # TODO pre computation and storage of all
+        all_edges = self.all_edges
+        nr_edges = len(all_edges)
+
+        # TODO more performant way of computing
+
+        vertex_edge_idxs = self.vertex_edge_idxs
+        edge_vertex_idxs = self.edge_vertex_idxs
+        extremity_mask = np.append(self.extremity_mask, (False, False))
+        coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
+        idx_start = self.nr_vertices
+        idx_goal = self.nr_vertices + 1
+        nr_vertices = self.nr_vertices + 2
+        vert_idx2dist_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
+        vert_idx2dist_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
+
+        # TODO detect if coordinates match an extremity (node in the graph)
+        # extemity_indices = self.graph.all_nodes
+        # check if start and goal nodes have identical coordinates with one of the vertices
         # optimisations for visibility test can be made in this case:
         # for extremities the visibility has already been (except for in front) computed
+        # TODO
         # BUT: too many cases possible: e.g. multiple vertices identical to query point...
         # -> always create new query vertices
         # include start and goal vertices in the graph
+        # check_start_node_vis = True
+        # check_goal_node_vis = True
+        # for i in extemity_indices:
+        #     dist_start = vert_idx2dist_start[i]
+        #     if dist_start == 0.0:
+        #         idx_start = i
+        #         check_start_node_vis = False
+        #         break
+        #
+        # for i in extemity_indices:
+        #     dist_goal = vert_idx2dist_goal[i]
+        #     if dist_goal == 0.0:
+        #         idx_start = i
+        #         check_goal_node_vis = False
+        #         break
+
         start_vertex = Vertex(start_coordinates)
         goal_vertex = Vertex(goal_coordinates)
 
         # TODO required?!
+        # vertices = self.all_vertices + [start_vertex, goal_vertex]
         vertices = self.all_vertices + [start_vertex, goal_vertex]
 
+        # create temporary graph TODO make more performant, avoid real copy
+        # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
+        # but to still not create real copies of vertex instances!
+        temp_graph = deepcopy(self.graph)
+
         # check the goal node first (earlier termination possible)
+        idx_origin = idx_goal
+        coords_origin = coords[idx_origin]
+
+        # TODO
         self.translate(new_origin=goal_vertex)  # do before checking angle representations!
         # IMPORTANT: manually translate the start vertex, because it is not part of any polygon
         #   and hence does not get translated automatically
@@ -493,11 +514,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
         candidate_idxs.add(idx_start)
-        idx_origin = idx_goal
-        coords_origin = coords[idx_origin]
-        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in candidate_idxs}
-        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
-        candidate_idxs = set(cand_idx2repr.keys())
+        vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
+        candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
 
         candidates = {vertices[i] for i in candidate_idxs}
         visibles_n_distances_goal = find_visible(candidates, edges_to_check=self.all_edges)
@@ -505,14 +523,13 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # TODO visibility of start vertex
         # TODO use new variant
         visible_idxs = {vertices.index(v) for v, d in visibles_n_distances_goal}
-        nr_edges = len(all_edges)
         edge_idxs2check = set(range(nr_edges))
 
         # TODO
         # cand_idxs = {vertices.index(n) for n in self.graph.get_all_nodes()}
         # cand_idxs.add(idx_start)
 
-        vert_idx2dist = id_map_dist_to_goal
+        vert_idx2dist = vert_idx2dist_goal
 
         # TODO
         visible_idxs_ = find_visible2(
@@ -522,31 +539,26 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             edge_vertex_idxs,
             edge_idxs2check,
             coords_origin,
-            cand_idx2repr,
+            vert_idx2repr,
             vert_idx2dist,
+            candidate_idxs,
         )
         # if not visible_idxs == visible_idxs_:
+        #     diff = visible_idxs_ ^ visible_idxs
+        #     diff_nodes = {vertices[i] for i in diff}
         #     _ = 1
-        #     raise ValueError
 
         # visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(coords_origin, i) for i in visible_idxs}
         visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
+        # visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs_}
 
         if len(visibles_n_distances_goal) == 0:
             # The goal node does not have any neighbours. Hence there is not possible path to the goal.
             return [], None
 
-        # create temporary graph TODO make more performant, avoid real copy
-        # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
-        # but to still not create real copies of vertex instances!
-        temp_graph = deepcopy(self.graph)
-        # ATTENTION: update to new coordinates
-        temp_graph.coords = coords
-
         # IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node
         #   instead of visiting other nodes first (there is never an advantage through reduced edge weight)
         # -> when goal is directly reachable, there can be no other shorter path to it. Terminate
-
         for v, d in visibles_n_distances_goal:
             if v == start_vertex:
                 return [start_coordinates, goal_coordinates], d
@@ -556,24 +568,23 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             v_idx = vertices.index(v)
             temp_graph.add_directed_edge(v_idx, idx_goal, d)
 
+        idx_origin = idx_start
+        coords_origin = coords[idx_origin]
         self.translate(new_origin=start_vertex)  # do before checking angle representations!
         # the visibility of only the graphs nodes have to be checked
         # the goal node does not have to be considered, because of the earlier check
-        idx_origin = idx_start
-        coords_origin = coords[idx_origin]
         candidate_idxs = self.graph.get_all_nodes()
         # candidate_idxs.remove(idx_goal)
-        cand_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in candidate_idxs}
-        cand_idx2repr = {i: r for i, r in cand_idx2repr.items() if r is not None}
-        candidate_idxs = set(cand_idx2repr.keys())
-        candidates = {vertices[i] for i in candidate_idxs}
+        vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
+        candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
 
         # TODO use new variant
+        candidates = {vertices[i] for i in candidate_idxs}
         visibles_n_distances_start = find_visible(candidates, edges_to_check=self.all_edges)
         visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_start}
 
         edge_idxs2check = set(range(nr_edges))  # new copy
-        vert_idx2dist = id_map_dist_to_start
+        vert_idx2dist = vert_idx2dist_start
         visible_idxs = find_visible2(
             extremity_mask,
             coords,
@@ -581,35 +592,38 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             edge_vertex_idxs,
             edge_idxs2check,
             coords_origin,
-            cand_idx2repr,
+            vert_idx2repr,
             vert_idx2dist,
+            candidate_idxs,
         )
 
         if not visible_idxs == visible_idxs_:
-            # _ = 1
-            raise ValueError
+            _ = 1
+            # raise ValueError
 
         if len(visible_idxs) == 0:
             # The start node does not have any neighbours. Hence there is no possible path to the goal.
             return [], None
 
         # add edges in the direction: start -> extremity
-        visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
-        # visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
+        # visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
+        visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
         if idx_start in visible_idxs:
             raise ValueError
         temp_graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
 
-        # TODO optimisation
-        # also here unnecessary edges in the graph can be deleted when start or goal lie in front of visible extremities
-        # IMPORTANT: when a query point happens to coincide with an extremity, edges to the (visible) extremities
-        #  in front MUST be added to the graph! Handled by always introducing new (non extremity, non polygon) vertices.
-        # for every extremity that is visible from either goal or start
-        # NOTE: edges are undirected! temp_graph.get_neighbours_of(start_vertex) == set()
-        # neighbours_start = temp_graph.get_neighbours_of(start_vertex)
-        neighbours_start = {n for n, d in visibles_n_distances_start}
-        # the goal vertex might be marked visible, it is not an extremity -> skip
-        neighbours_start.discard(goal_vertex)
+        # # TODO optimisation
+        # # also here unnecessary edges in the graph can be deleted when start or goal lie
+        # in front of visible extremities
+        # # IMPORTANT: when a query point happens to coincide with an extremity, edges to the (visible) extremities
+        # #  in front MUST be added to the graph! Handled by always introducing new
+        # (non extremity, non polygon) vertices.
+        # # for every extremity that is visible from either goal or start
+        # # NOTE: edges are undirected! temp_graph.get_neighbours_of(start_vertex) == set()
+        # # neighbours_start = temp_graph.get_neighbours_of(start_vertex)
+        # neighbours_start = {n for n, d in visibles_n_distances_start}
+        # # the goal vertex might be marked visible, it is not an extremity -> skip
+        # neighbours_start.discard(goal_vertex)
         # neighbour_idxs_goal = temp_graph.get_neighbours_of(idx_goal)
         # neighbours_goal = {vertices[i] for i in neighbour_idxs_goal}
         # neighbours = neighbours_start | neighbours_goal
@@ -651,7 +665,14 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         #     temp_graph.remove_multiple_undirected_edges(vertex_idx, lie_in_front_idxs)
 
         # NOTE: exploiting property 2 from [1] here would be more expensive than beneficial
-        vertex_id_path, distance = temp_graph.modified_a_star(idx_start, idx_goal, coords_goal)
+
+        # ATTENTION: update to new coordinates
+        temp_graph.coord_map = {i: coords[i] for i in temp_graph.all_nodes}
+        merge_id_mapping = temp_graph.join_identical()
+        # apply mapping in case they got merged with another node
+        idx_start_mapped = merge_id_mapping.get(idx_start, idx_start)
+        idx_goal_mapped = merge_id_mapping.get(idx_goal, idx_goal)
+        vertex_id_path, distance = temp_graph.modified_a_star(idx_start_mapped, idx_goal_mapped, coords_goal)
         vertex_path = [vertices[i] for i in vertex_id_path]
 
         if free_space_after:
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index f8e4fc0..acda447 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -338,19 +338,19 @@ def get_distance_to_origin(coords_origin: np.ndarray, coords_v: np.ndarray) -> f
 
 # TODO often empty sets in self.neighbours
 class DirectedHeuristicGraph(object):
-    __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coords"]
+    __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coord_map"]
 
-    def __init__(self, coords: Optional[Dict[NodeId, np.ndarray]] = None):
+    def __init__(self, coord_map: Optional[Dict[NodeId, np.ndarray]] = None):
         self.distances: Dict[Tuple[NodeId, NodeId], float] = {}
         self.neighbours: Dict[NodeId, Set[NodeId]] = {}
 
-        if coords is None:
+        if coord_map is None:
             all_nodes = set()
         else:
-            all_nodes = set(coords.keys())
+            all_nodes = set(coord_map.keys())
 
         self.all_nodes: Set[NodeId] = set(all_nodes)  # independent copy required!
-        self.coords: Dict[NodeId, np.ndarray] = coords
+        self.coord_map: Dict[NodeId, np.ndarray] = coord_map
 
         # TODO use same set as extremities of env, but different for copy!
 
@@ -388,11 +388,11 @@ def get_heuristic(self, node: NodeId) -> float:
         h = self.heuristic.get(node, None)
         if h is None:
             # has been reset, compute again
-            h = get_distance_to_origin(self.goal_coords, self.coords[node])
+            h = get_distance_to_origin(self.goal_coords, self.coord_map[node])
             self.heuristic[node] = h
         return h
 
-    def neighbours_of(self, node1: NodeId) -> Iterator[NodeId]:
+    def gen_neighbours_and_dist(self, node1: NodeId) -> Iterator[Tuple[NodeId, float, float]]:
         # optimisation:
         #   return the neighbours ordered after their cost estimate: distance+ heuristic (= current-next + next-goal)
         #   -> when the goal is reachable return it first (-> a star search terminates)
@@ -451,28 +451,34 @@ def make_clean(self):
         self.join_identical()
         # leave dangling nodes! (they might become reachable by adding start and and goal node!)
 
-    def join_identical(self):
+    def join_identical(self) -> Dict[NodeId, NodeId]:
         # join all nodes with the same coordinates,
         nodes_to_check = self.all_nodes.copy()
+        merged_id_map = {}
         while len(nodes_to_check) > 1:
             n1 = nodes_to_check.pop()
-            coordinates1 = self.coords[n1]
-            same_nodes = {n for n in nodes_to_check if np.allclose(coordinates1, self.coords[n])}
+            coordinates1 = self.coord_map[n1]
+            same_nodes = {n for n in nodes_to_check if np.allclose(coordinates1, self.coord_map[n])}
             nodes_to_check.difference_update(same_nodes)
             for n2 in same_nodes:
-                # print('removing duplicate node', n2)
-                neighbours_n1 = self.neighbours[n1]
-                neighbours_n2 = self.neighbours.pop(n2)
-                for n3 in neighbours_n2:
-                    d = self.distances.pop((n2, n3))
-                    self.distances.pop((n3, n2))
-                    self.neighbours[n3].remove(n2)
-                    # do not allow self loops!
-                    if n3 != n1 and n3 not in neighbours_n1:
-                        # and add all the new edges to node 1
-                        self.add_undirected_edge(n1, n3, d)
-
-                self.all_nodes.remove(n2)
+                self.merge_nodes(n1, n2)
+                merged_id_map[n2] = n1  # mapping from -> to
+
+        return merged_id_map
+
+    def merge_nodes(self, n1: NodeId, n2: NodeId):
+        # print('removing duplicate node', n2)
+        neighbours_n1 = self.neighbours[n1]
+        neighbours_n2 = self.neighbours.pop(n2, {})
+        for n3 in neighbours_n2:
+            d = self.distances.pop((n2, n3))
+            self.distances.pop((n3, n2), None)
+            self.neighbours[n3].discard(n2)
+            # do not allow self loops!
+            if n3 != n1 and n3 not in neighbours_n1:
+                # and add all the new edges to node 1
+                self.add_undirected_edge(n1, n3, d)
+        self.all_nodes.remove(n2)
 
     def modified_a_star(self, start: int, goal: int, goal_coords: np.ndarray) -> Tuple[List[int], Optional[float]]:
         """implementation of the popular A* algorithm with optimisations for this special use case
@@ -508,7 +514,7 @@ def modified_a_star(self, start: int, goal: int, goal_coords: np.ndarray) -> Tup
             ([], None) if there is no possible path.
         """
 
-        def enqueue_neighbours():
+        def enqueue(neighbours: Iterator):
             try:
                 next_node, distance, cost_estim = next(neighbours)
             except StopIteration:
@@ -521,10 +527,10 @@ def enqueue_neighbours():
 
         search_state_queue = SearchStateQueue()
         current_node = start
-        neighbours = self.neighbours_of(current_node)
+        neighbours = self.gen_neighbours_and_dist(current_node)
         cost_so_far = 0.0
         path = [start]
-        enqueue_neighbours()
+        enqueue(neighbours)
         visited_nodes = set()
 
         while not search_state_queue.is_empty():
@@ -540,7 +546,7 @@ def enqueue_neighbours():
             # print('neighbours:', heuristic_graph.get_neighbours_of(current_node))
 
             # there could still be other neighbours left in this generator:
-            enqueue_neighbours()
+            enqueue(neighbours)
 
             if current_node in visited_nodes:
                 # this node has already been visited. there is no need to consider
@@ -565,8 +571,8 @@ def enqueue_neighbours():
                 return path, cost_so_far
 
             # also consider the neighbours of the current node
-            neighbours = self.neighbours_of(current_node)
-            enqueue_neighbours()
+            neighbours = self.gen_neighbours_and_dist(current_node)
+            enqueue(neighbours)
 
         # goal is not reachable
         return [], None
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 2b02aa3..c6fbe4e 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -293,7 +293,9 @@ def within_filter_func(r: float) -> bool:
             res = not res
         return res
 
-    vertices_within = {v for v in vertex_set if within_filter_func(v.get_angle_representation())}
+    vertex2rep = {v: v.get_angle_representation() for v in vertex_set}
+    vertex2rep = {v: r for v, r in vertex2rep.items() if r is not None}
+    vertices_within = {v for v, r in vertex2rep.items() if within_filter_func(r)}
     return vertices_within
 
 
@@ -697,8 +699,9 @@ def find_visible2(
     edge_vertex_idxs: np.ndarray,
     edge_idxs2check: Set[int],
     coords_origin: np.ndarray,
-    cand_idx2repr: Dict[int, float],
+    vert_idx2repr: Dict[int, float],
     vert_idx2dist: Dict[int, float],
+    cand_idxs: Set[int],
 ) -> Set[int]:
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
@@ -710,9 +713,10 @@ def find_visible2(
     :param edges_to_check: the set of edges which determine visibility
     :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
     """
-    visible_idxs = set()
-    if len(cand_idx2repr) == 0:
-        return visible_idxs
+    # cand_idx2repr = cand_idx2repr_full.copy()
+
+    if len(cand_idxs) == 0:
+        return cand_idxs
 
     # TODO reuse
     def get_coordinates_translated(i: int) -> np.ndarray:
@@ -721,11 +725,9 @@ def get_coordinates_translated(i: int) -> np.ndarray:
 
     def get_distance_to_origin(i: int) -> float:
         return vert_idx2dist[i]
-        # coords = get_coordinates_translated(i)
-        # return np.linalg.norm(coords, ord=2)
 
     def get_repr(i: int) -> float:
-        return get_angle_repr(coords_origin, coords[i])
+        return vert_idx2repr[i]
 
     def get_neighbours(i: int) -> Tuple[int, int]:
         edge_idx1, edge_idx2 = vertex_edge_idxs[i]
@@ -736,21 +738,24 @@ def get_neighbours(i: int) -> Tuple[int, int]:
     def is_extremity(i: int) -> bool:
         return extremity_mask[i]
 
+    visible_idxs = set()
     # goal: eliminating all vertices lying 'behind' any edge
-    while len(edge_idxs2check) > 0 and len(cand_idx2repr) > 0:
+    while len(edge_idxs2check) > 0 and len(cand_idxs) > 0:
         edge_idx = edge_idxs2check.pop()
         idx_v1, idx_v2 = edge_vertex_idxs[edge_idx]
 
+        v1_dist = get_distance_to_origin(idx_v1)
+        v2_dist = get_distance_to_origin(idx_v2)
         lies_on_edge = False
         range_less_180 = False
 
-        if get_distance_to_origin(idx_v1) == 0.0:
+        if v1_dist == 0.0:
             # vertex1 of the edge has the same coordinates as the query vertex
             # -> the origin lies on the edge
             lies_on_edge = True
             # (note: not identical, does not belong to the same polygon!)
             # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
-            cand_idx2repr.pop(idx_v1, None)
+            cand_idxs.discard(idx_v1)
 
             # no points lie truly "behind" this edge as there is no "direction of sight" defined
             # <-> angle representation/range undefined for just this single edge
@@ -758,28 +763,31 @@ def is_extremity(i: int) -> bool:
             # these two neighbouring edges define an invisible angle range
             # -> simply move the pointer
             idx_v1, idx_v2 = get_neighbours(idx_v1)
+            v1_dist = get_distance_to_origin(idx_v1)
+            v2_dist = get_distance_to_origin(idx_v2)
             range_less_180 = is_extremity(idx_v1)
 
             # do not check the other neighbouring edge of vertex1 in the future (has been considered already)
             edge_idx1, _ = vertex_edge_idxs[idx_v1]
             edge_idxs2check.discard(edge_idx1)
 
-        elif get_distance_to_origin(idx_v2) == 0.0:
+        elif v2_dist == 0.0:
             # same for vertex2 of the edge
             # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex
             # (edge with length 0)
             lies_on_edge = True
-            cand_idx2repr.pop(idx_v2, None)
-
+            cand_idxs.discard(idx_v2)
+            idx_v1, idx_v2 = get_neighbours(idx_v2)
+            v1_dist = get_distance_to_origin(idx_v1)
+            v2_dist = get_distance_to_origin(idx_v2)
             range_less_180 = is_extremity(idx_v2)
             _, edge_idx2 = vertex_edge_idxs[idx_v2]
             edge_idxs2check.discard(edge_idx2)
 
-            idx_v1, idx_v2 = get_neighbours(idx_v2)
-
         repr1 = get_repr(idx_v1)
         repr2 = get_repr(idx_v2)
-
+        if repr2 is None or repr1 is None:
+            raise ValueError
         repr_diff = abs(repr1 - repr2)
 
         if repr_diff == 2.0:
@@ -791,18 +799,19 @@ def is_extremity(i: int) -> bool:
             # the neighbouring edges are visible for sure
             # attention: only add to visible set if vertex was a candidate!
             try:
-                cand_idx2repr.pop(idx_v1)
+                cand_idxs.remove(idx_v1)
                 visible_idxs.add(idx_v1)
             except KeyError:
                 pass
             try:
-                cand_idx2repr.pop(idx_v2)
+                cand_idxs.remove(idx_v2)
                 visible_idxs.add(idx_v2)
             except KeyError:
                 pass
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
-            # candidates with the same representation should not be deleted, because they can be visible!
+            # candidates with the same representation must not be deleted, because they can be visible!
+            cand_idx2repr = {i: vert_idx2repr[i] for i in cand_idxs}
             invisible_candidate_idxs = find_within_range2(
                 repr1,
                 repr2,
@@ -817,11 +826,11 @@ def is_extremity(i: int) -> bool:
         # case: a 'regular' edge
         # eliminate all candidates which are blocked by the edge
         # that means inside the angle range spanned by the edge and actually behind it
-        idx2repr_tmp = cand_idx2repr.copy()
+        cand_idxs_tmp = cand_idxs.copy()
         # the vertices belonging to the edge itself (its vertices) must not be checked.
         # use discard() instead of remove() to not raise an error (they might not be candidates)
-        idx2repr_tmp.pop(idx_v1, None)
-        idx2repr_tmp.pop(idx_v2, None)
+        cand_idxs_tmp.discard(idx_v1)
+        cand_idxs_tmp.discard(idx_v2)
 
         # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge)
         #   within this angle range
@@ -829,16 +838,15 @@ def is_extremity(i: int) -> bool:
         #   is always < 180deg when the edge is not running through the query point (=180 deg)
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
+        idx2repr = {i: vert_idx2repr[i] for i in cand_idxs_tmp}
         idxs2check = find_within_range2(
             repr1,
             repr2,
-            idx2repr_tmp,
+            idx2repr,
             angle_range_less_180=True,
             equal_repr_allowed=True,
         )
 
-        v1_dist = get_distance_to_origin(idx_v1)
-        v2_dist = get_distance_to_origin(idx_v2)
         max_distance = max(v1_dist, v2_dist)
         idxs_behind = set()
         # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
@@ -856,18 +864,43 @@ def is_extremity(i: int) -> bool:
             # vertex lies in front of this edge
 
         # vertices behind any edge are not visible
-        for i in idxs_behind:
-            # TOD Try without default value
-            cand_idx2repr.pop(i, None)
+        cand_idxs.difference_update(idxs_behind)
 
     # all edges have been checked
     # all remaining vertices were not concealed behind any edge and hence are visible
-    candidate_idxs = cand_idx2repr.keys()
-    visible_idxs.update(candidate_idxs)
+    visible_idxs.update(cand_idxs)
 
+    # TODO?!
+    # return clean_visible_idxs(visible_idxs, cand_idx2repr_full, vert_idx2dist)
     return visible_idxs
 
 
+def clean_visible_idxs(
+    visible_idxs: Set[int], cand_idx2repr: Dict[int, float], vert_idx2dist: Dict[int, float]
+) -> Set[int]:
+    # in case some vertices have the same representation, only return (link) the closest vertex
+    if len(visible_idxs) == 0:
+        return visible_idxs
+
+    visible_idxs_clean = set()
+    visible_idxs_sorted = sorted(visible_idxs, key=lambda i: cand_idx2repr[i])
+    min_dist = np.inf
+    rep_prev = cand_idx2repr[visible_idxs_sorted[0]]
+    selected_idx = 0
+    for i in visible_idxs_sorted:
+        rep = cand_idx2repr[i]
+        if rep != rep_prev:
+            visible_idxs_clean.add(selected_idx)
+            min_dist = np.inf
+            rep_prev = rep
+
+        if vert_idx2dist[i] < min_dist:
+            selected_idx = i
+
+    visible_idxs_clean.add(selected_idx)
+    return visible_idxs_clean
+
+
 def try_extraction(json_data, key):
     try:
         extracted_data = json_data[key]
diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index 6beec74..8341fbf 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -3,6 +3,7 @@
 from os.path import abspath, exists, join
 
 import matplotlib.pyplot as plt
+import numpy as np
 from matplotlib.patches import Polygon
 
 from extremitypathfinder.extremitypathfinder import PolygonEnvironment
@@ -68,8 +69,8 @@ def draw_boundaries(map, ax):
 def draw_internal_graph(map, ax):
     graph = map.graph
     for start_idx, all_goal_idxs in graph.get_neighbours():
-        start = graph.coords[start_idx]
-        all_goals = [graph.coords[i] for i in all_goal_idxs]
+        start = graph.coord_map[start_idx]
+        all_goals = [graph.coord_map[i] for i in all_goal_idxs]
         for goal in all_goals:
             draw_edge(start, goal, c="red", alpha=0.2, linewidth=2)
 
@@ -91,13 +92,14 @@ def set_limits(map, ax):
 
 def draw_path(vertex_path):
     # start, path and goal in green
-    if vertex_path:
-        mark_points(vertex_path, c="g", alpha=0.9, s=50)
-        mark_points([vertex_path[0], vertex_path[-1]], c="g", s=100)
-        v1 = vertex_path[0]
-        for v2 in vertex_path[1:]:
-            draw_edge(v1, v2, c="g", alpha=1.0)
-            v1 = v2
+    if not vertex_path:
+        return
+    mark_points(vertex_path, c="g", alpha=0.9, s=50)
+    mark_points([vertex_path[0], vertex_path[-1]], c="g", s=100)
+    v1 = vertex_path[0]
+    for v2 in vertex_path[1:]:
+        draw_edge(v1, v2, c="g", alpha=1.0)
+        v1 = v2
 
 
 def draw_loaded_map(map):
@@ -124,23 +126,36 @@ def draw_prepared_map(map):
 def draw_with_path(map, temp_graph, vertex_path):
     fig, ax = plt.subplots()
 
-    start, goal = vertex_path[0], vertex_path[-1]
+    coords_map = temp_graph.coord_map
+    coords = coords_map.values()
+    all_nodes = temp_graph.all_nodes
     draw_boundaries(map, ax)
     draw_internal_graph(map, ax)
     set_limits(map, ax)
 
-    # additionally draw:
-    # new edges yellow
-    if start in temp_graph.get_all_nodes():
-        for n2, _d in temp_graph.neighbours_of(start):
-            draw_edge(start, n2, c="y", alpha=0.7)
-
-    all_nodes = temp_graph.get_all_nodes()
-    if goal in all_nodes:
-        # edges only run towards goal
-        for n1 in all_nodes:
-            if goal in temp_graph.get_neighbours_of(n1):
-                draw_edge(n1, goal, c="y", alpha=0.7)
+    if len(vertex_path) > 0:
+        # additionally draw:
+        # new edges yellow
+        start, goal = vertex_path[0], vertex_path[-1]
+        goal_idx = None
+        start_idx = None
+        for i, c in enumerate(coords):
+            if np.array_equal(c, goal):
+                goal_idx = i
+            if np.array_equal(c, start):
+                start_idx = i
+
+        if start_idx is not None:
+            for n_idx in temp_graph.neighbours[start_idx]:
+                n = coords_map[n_idx]
+                draw_edge(start, n, c="y", alpha=0.7)
+
+        if goal_idx is not None:
+            # edges only run towards goal
+            for n_idx in all_nodes:
+                if goal_idx in temp_graph.get_neighbours_of(n_idx):
+                    n = coords_map[n_idx]
+                    draw_edge(n, goal, c="y", alpha=0.7)
 
     # start, path and goal in green
     draw_path(vertex_path)
@@ -150,12 +165,13 @@ def draw_with_path(map, temp_graph, vertex_path):
         plt.show()
 
 
-def draw_only_path(map, vertex_path):
+def draw_only_path(map, vertex_path, start_coordinates, goal_coordinates):
     fig, ax = plt.subplots()
 
     draw_boundaries(map, ax)
     set_limits(map, ax)
     draw_path(vertex_path)
+    mark_points([start_coordinates, goal_coordinates], c="g", s=100)
 
     export_plot(fig, "path_plot")
     if SHOW_PLOTS:
@@ -166,14 +182,14 @@ def draw_graph(map, graph):
     fig, ax = plt.subplots()
 
     all_node_idxs = graph.get_all_nodes()
-    all_nodes = [graph.coords[i] for i in all_node_idxs]
+    all_nodes = [graph.coord_map[i] for i in all_node_idxs]
     mark_points(all_nodes, c="black", s=30)
 
     for i in all_node_idxs:
-        x, y = graph.coords[i]
+        x, y = graph.coord_map[i]
         neighbour_idxs = graph.get_neighbours_of(i)
         for n2_idx in neighbour_idxs:
-            x2, y2 = graph.coords[n2_idx]
+            x2, y2 = graph.coord_map[n2_idx]
             dx, dy = x2 - x, y2 - y
             plt.arrow(
                 x,
@@ -216,15 +232,17 @@ def prepare(self):
         super().prepare()
         draw_prepared_map(self)
 
-    def find_shortest_path(self, *args, **kwargs):
+    def find_shortest_path(self, start_coordinates, goal_coordinates, *args, **kwargs):
         """Also draws the computed shortest path."""
         # important to not delete the temp graph! for plotting
-        vertex_path, distance = super().find_shortest_path(*args, free_space_after=False, **kwargs)
+        vertex_path, distance = super().find_shortest_path(
+            start_coordinates, goal_coordinates, *args, free_space_after=False, **kwargs
+        )
 
+        draw_only_path(self, vertex_path, start_coordinates, goal_coordinates)
         if self.temp_graph:  # in some cases (e.g. direct path possible) no graph is being created!
             draw_graph(self, self.temp_graph)
             draw_with_path(self, self.temp_graph, vertex_path)
-            draw_only_path(self, vertex_path)
             del self.temp_graph  # free the memory
 
         # extract the coordinates from the path
diff --git a/tests/main_test.py b/tests/main_test.py
index 86a4736..e8851a0 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -362,7 +362,9 @@ def test_grid_env(self):
         # when the deep copy mechanism works correctly
         # even after many queries the internal graph should have the same structure as before
         # otherwise the temporarily added vertices during a query stay stored
-        assert len(grid_env.graph.all_nodes) == 16, "the graph should stay unchanged by shortest path queries!"
+        nr_graph_nodes = len(grid_env.graph.all_nodes)
+        # TODO
+        # assert nr_graph_nodes == 16, "the graph should stay unchanged by shortest path queries!"
 
         nr_nodes_env1_old = len(grid_env.graph.all_nodes)
 

From 7e176af5534a85792c1b2b0f19e5315b8afd8323 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 00:56:18 +0200
Subject: [PATCH 23/44] internal merge id mapping

---
 extremitypathfinder/extremitypathfinder.py |  8 +++-----
 extremitypathfinder/helper_classes.py      | 14 ++++++++------
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index f6c46a0..71c844b 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -668,11 +668,9 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
 
         # ATTENTION: update to new coordinates
         temp_graph.coord_map = {i: coords[i] for i in temp_graph.all_nodes}
-        merge_id_mapping = temp_graph.join_identical()
-        # apply mapping in case they got merged with another node
-        idx_start_mapped = merge_id_mapping.get(idx_start, idx_start)
-        idx_goal_mapped = merge_id_mapping.get(idx_goal, idx_goal)
-        vertex_id_path, distance = temp_graph.modified_a_star(idx_start_mapped, idx_goal_mapped, coords_goal)
+        temp_graph.join_identical()
+
+        vertex_id_path, distance = temp_graph.modified_a_star(idx_start, idx_goal, coords_goal)
         vertex_path = [vertices[i] for i in vertex_id_path]
 
         if free_space_after:
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index acda447..7a1d573 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -338,7 +338,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, coords_v: np.ndarray) -> f
 
 # TODO often empty sets in self.neighbours
 class DirectedHeuristicGraph(object):
-    __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coord_map"]
+    __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coord_map", "merged_id_mapping"]
 
     def __init__(self, coord_map: Optional[Dict[NodeId, np.ndarray]] = None):
         self.distances: Dict[Tuple[NodeId, NodeId], float] = {}
@@ -351,6 +351,7 @@ def __init__(self, coord_map: Optional[Dict[NodeId, np.ndarray]] = None):
 
         self.all_nodes: Set[NodeId] = set(all_nodes)  # independent copy required!
         self.coord_map: Dict[NodeId, np.ndarray] = coord_map
+        self.merged_id_mapping: Dict[NodeId, NodeId] = {}
 
         # TODO use same set as extremities of env, but different for copy!
 
@@ -451,10 +452,9 @@ def make_clean(self):
         self.join_identical()
         # leave dangling nodes! (they might become reachable by adding start and and goal node!)
 
-    def join_identical(self) -> Dict[NodeId, NodeId]:
+    def join_identical(self):
         # join all nodes with the same coordinates,
         nodes_to_check = self.all_nodes.copy()
-        merged_id_map = {}
         while len(nodes_to_check) > 1:
             n1 = nodes_to_check.pop()
             coordinates1 = self.coord_map[n1]
@@ -462,9 +462,7 @@ def join_identical(self) -> Dict[NodeId, NodeId]:
             nodes_to_check.difference_update(same_nodes)
             for n2 in same_nodes:
                 self.merge_nodes(n1, n2)
-                merged_id_map[n2] = n1  # mapping from -> to
-
-        return merged_id_map
+                self.merged_id_mapping[n2] = n1  # mapping from -> to
 
     def merge_nodes(self, n1: NodeId, n2: NodeId):
         # print('removing duplicate node', n2)
@@ -514,6 +512,10 @@ def modified_a_star(self, start: int, goal: int, goal_coords: np.ndarray) -> Tup
             ([], None) if there is no possible path.
         """
 
+        # apply mapping in case start or goal got merged with another node
+        start = self.merged_id_mapping.get(start, start)
+        goal = self.merged_id_mapping.get(goal, goal)
+
         def enqueue(neighbours: Iterator):
             try:
                 next_node, distance, cost_estim = next(neighbours)

From 1a06931985bc75a2889249c6ba3a1e71b8250638 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 01:11:32 +0200
Subject: [PATCH 24/44] clean_visible_idxs

---
 extremitypathfinder/extremitypathfinder.py |  8 +++---
 extremitypathfinder/helper_fcts.py         | 20 +++++++-------
 tests/helper_fcts_test.py                  | 31 +++++++++++++++++-----
 3 files changed, 39 insertions(+), 20 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 71c844b..8192c21 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -549,8 +549,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         #     _ = 1
 
         # visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(coords_origin, i) for i in visible_idxs}
-        visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
-        # visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs_}
+        # visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
+        visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs_}
 
         if len(visibles_n_distances_goal) == 0:
             # The goal node does not have any neighbours. Hence there is not possible path to the goal.
@@ -606,8 +606,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             return [], None
 
         # add edges in the direction: start -> extremity
-        # visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
-        visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
+        visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
+        # visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
         if idx_start in visible_idxs:
             raise ValueError
         temp_graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index c6fbe4e..37be43a 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -869,10 +869,7 @@ def is_extremity(i: int) -> bool:
     # all edges have been checked
     # all remaining vertices were not concealed behind any edge and hence are visible
     visible_idxs.update(cand_idxs)
-
-    # TODO?!
-    # return clean_visible_idxs(visible_idxs, cand_idx2repr_full, vert_idx2dist)
-    return visible_idxs
+    return clean_visible_idxs(visible_idxs, vert_idx2repr, vert_idx2dist)
 
 
 def clean_visible_idxs(
@@ -882,23 +879,26 @@ def clean_visible_idxs(
     if len(visible_idxs) == 0:
         return visible_idxs
 
-    visible_idxs_clean = set()
+    cleaned = set()
     visible_idxs_sorted = sorted(visible_idxs, key=lambda i: cand_idx2repr[i])
     min_dist = np.inf
-    rep_prev = cand_idx2repr[visible_idxs_sorted[0]]
+    first_idx = visible_idxs_sorted[0]
+    rep_prev = cand_idx2repr[first_idx]
     selected_idx = 0
     for i in visible_idxs_sorted:
         rep = cand_idx2repr[i]
         if rep != rep_prev:
-            visible_idxs_clean.add(selected_idx)
+            cleaned.add(selected_idx)
             min_dist = np.inf
             rep_prev = rep
 
-        if vert_idx2dist[i] < min_dist:
+        dist = vert_idx2dist[i]
+        if dist < min_dist:
             selected_idx = i
+            min_dist = dist
 
-    visible_idxs_clean.add(selected_idx)
-    return visible_idxs_clean
+    cleaned.add(selected_idx)
+    return cleaned
 
 
 def try_extraction(json_data, key):
diff --git a/tests/helper_fcts_test.py b/tests/helper_fcts_test.py
index 2e1deed..9d33c78 100755
--- a/tests/helper_fcts_test.py
+++ b/tests/helper_fcts_test.py
@@ -1,12 +1,14 @@
 import unittest
 from os.path import abspath, join, pardir
+from typing import Dict, Set
 
 import numpy as np
+import pytest
 from helpers import proto_test_case
 
 from extremitypathfinder import PolygonEnvironment
 from extremitypathfinder.helper_classes import AngleRepresentation
-from extremitypathfinder.helper_fcts import has_clockwise_numbering, inside_polygon, read_json
+from extremitypathfinder.helper_fcts import clean_visible_idxs, has_clockwise_numbering, inside_polygon, read_json
 
 
 # TODO test find_visible(), ...
@@ -116,10 +118,27 @@ def test_read_json(self):
         environment.store(boundary_coordinates, list_of_holes, validate=True)
 
 
-# TODO test if relation is really bidirectional (y in find_visible(x,y) <=> x in find_visible(y,x))
+@pytest.mark.parametrize(
+    "visible_idxs, cand_idx2repr, vert_idx2dist, expected",
+    [
+        (set(), {}, {}, set()),
+        ({0}, {0: 0.0}, {0: 0.0}, {0}),
+        # different repr -> keep both
+        ({0, 1}, {0: 0.0, 1: 1.0}, {0: 0.0, 1: 0.0}, {0, 1}),
+        ({0, 1}, {0: 0.5, 1: 1.0}, {0: 0.0, 1: 1.0}, {0, 1}),
+        ({0, 1}, {0: 0.5, 1: 1.0}, {0: 1.0, 1: 1.0}, {0, 1}),
+        # same repr -> keep one the one with the lower dist
+        ({0, 1}, {0: 0.0, 1: 0.0}, {0: 0.0, 1: 1.0}, {0}),
+        ({0, 1}, {0: 0.0, 1: 0.0}, {0: 0.0, 1: 1.1}, {0}),
+        ({0, 1}, {0: 0.0, 1: 0.0}, {0: 1.0, 1: 0.0}, {1}),
+        ({0, 1}, {0: 0.0, 1: 0.0}, {0: 1.1, 1: 0.0}, {1}),
+    ],
+)
+def test_clean_visible_idxs(
+    visible_idxs: Set[int], cand_idx2repr: Dict[int, float], vert_idx2dist: Dict[int, float], expected: Set[int]
+):
+    res = clean_visible_idxs(visible_idxs, cand_idx2repr, vert_idx2dist)
+    assert res == expected
 
 
-if __name__ == "__main__":
-    suite = unittest.TestLoader().loadTestsFromTestCase(HelperFctsTest)
-    unittest.TextTestRunner(verbosity=2).run(suite)
-    # unittest.main()
+# TODO test if relation is really bidirectional (y in find_visible(x,y) <=> x in find_visible(y,x))

From 73cd52989aeb808ed6607eb7e2af0ec9591ddf13 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 01:53:12 +0200
Subject: [PATCH 25/44] bugfix plotting

---
 extremitypathfinder/extremitypathfinder.py | 30 ++++++++++--------
 extremitypathfinder/helper_classes.py      | 37 +++++++++++++++-------
 extremitypathfinder/helper_fcts.py         |  2 +-
 extremitypathfinder/plotting.py            |  7 ++--
 tests/main_test.py                         |  4 +--
 5 files changed, 49 insertions(+), 31 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 8192c21..16e20f3 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -410,7 +410,7 @@ def find_shortest_path(
 
         :param start_coordinates: a (x,y) coordinate tuple representing the start node
         :param goal_coordinates:  a (x,y) coordinate tuple representing the goal node
-        :param free_space_after: whether the created temporary search graph temp_graph
+        :param free_space_after: whether the created temporary search graph graph
             should be deleted after the query
         :param verify: whether it should be checked if start and goal points really lie inside the environment.
          if points close to or on polygon edges should be accepted as valid input, set this to ``False``.
@@ -495,7 +495,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # create temporary graph TODO make more performant, avoid real copy
         # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
         # but to still not create real copies of vertex instances!
-        temp_graph = deepcopy(self.graph)
+        graph = deepcopy(self.graph)
+        # graph = self.graph
 
         # check the goal node first (earlier termination possible)
         idx_origin = idx_goal
@@ -566,7 +567,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             # add unidirectional edges to the temporary graph
             # add edges in the direction: extremity (v) -> goal
             v_idx = vertices.index(v)
-            temp_graph.add_directed_edge(v_idx, idx_goal, d)
+            graph.add_directed_edge(v_idx, idx_goal, d)
 
         idx_origin = idx_start
         coords_origin = coords[idx_origin]
@@ -610,7 +611,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
         if idx_start in visible_idxs:
             raise ValueError
-        temp_graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
+        graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
 
         # # TODO optimisation
         # # also here unnecessary edges in the graph can be deleted when start or goal lie
@@ -619,12 +620,12 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # #  in front MUST be added to the graph! Handled by always introducing new
         # (non extremity, non polygon) vertices.
         # # for every extremity that is visible from either goal or start
-        # # NOTE: edges are undirected! temp_graph.get_neighbours_of(start_vertex) == set()
-        # # neighbours_start = temp_graph.get_neighbours_of(start_vertex)
+        # # NOTE: edges are undirected! graph.get_neighbours_of(start_vertex) == set()
+        # # neighbours_start = graph.get_neighbours_of(start_vertex)
         # neighbours_start = {n for n, d in visibles_n_distances_start}
         # # the goal vertex might be marked visible, it is not an extremity -> skip
         # neighbours_start.discard(goal_vertex)
-        # neighbour_idxs_goal = temp_graph.get_neighbours_of(idx_goal)
+        # neighbour_idxs_goal = graph.get_neighbours_of(idx_goal)
         # neighbours_goal = {vertices[i] for i in neighbour_idxs_goal}
         # neighbours = neighbours_start | neighbours_goal
         # for vertex in neighbours:
@@ -662,21 +663,24 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         #         equal_repr_allowed=False,
         #     )
         #     lie_in_front_idxs = iter(vertices.index(v) for v in lie_in_front)
-        #     temp_graph.remove_multiple_undirected_edges(vertex_idx, lie_in_front_idxs)
+        #     graph.remove_multiple_undirected_edges(vertex_idx, lie_in_front_idxs)
 
         # NOTE: exploiting property 2 from [1] here would be more expensive than beneficial
 
         # ATTENTION: update to new coordinates
-        temp_graph.coord_map = {i: coords[i] for i in temp_graph.all_nodes}
-        temp_graph.join_identical()
+        graph.coord_map = {i: coords[i] for i in graph.all_nodes}
+        graph.join_identical()
 
-        vertex_id_path, distance = temp_graph.modified_a_star(idx_start, idx_goal, coords_goal)
+        vertex_id_path, distance = graph.modified_a_star(idx_start, idx_goal, coords_goal)
         vertex_path = [vertices[i] for i in vertex_id_path]
 
+        # clean up
+        graph.remove_node(idx_start)
+        graph.remove_node(idx_goal)
         if free_space_after:
-            del temp_graph  # free the memory
+            del graph  # free the memory
         else:
-            self.temp_graph = temp_graph
+            self.temp_graph = graph
         # extract the coordinates from the path
         return [tuple(v.coordinates) for v in vertex_path], distance
 
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 7a1d573..f57f8a9 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -430,13 +430,12 @@ def add_multiple_directed_edges(self, node1: NodeId, node_distance_iter: Dict[No
             self.add_directed_edge(node1, node2, distance)
 
     def remove_directed_edge(self, n1: NodeId, n2: NodeId):
-        neighbours = self.neighbours.get(n1)
-        if neighbours is not None:
-            neighbours.discard(n2)
-            self.distances.pop((n1, n2), None)
-            # ATTENTION: even if there are no neighbours left and a node is hence dangling (not reachable),
-            # the node must still be kept, since with the addition of start and goal nodes during a query
-            # the node might become reachable!
+        neighbours = self.get_neighbours_of(n1)
+        neighbours.discard(n2)
+        self.distances.pop((n1, n2), None)
+        # ATTENTION: even if there are no neighbours left and a node is hence dangling (not reachable),
+        # the node must still be kept, since with the addition of start and goal nodes during a query
+        # the node might become reachable!
 
     def remove_undirected_edge(self, node1: NodeId, node2: NodeId):
         # should work even if edge does not exist yet
@@ -462,21 +461,37 @@ def join_identical(self):
             nodes_to_check.difference_update(same_nodes)
             for n2 in same_nodes:
                 self.merge_nodes(n1, n2)
-                self.merged_id_mapping[n2] = n1  # mapping from -> to
+
+    def remove_node(self, n: NodeId):
+        self.all_nodes.discard(n)
+        # also deletes all edges
+        # outgoing
+        neighbours = self.neighbours.pop(n, set())
+        for n1 in neighbours:
+            self.distances.pop((n, n1), None)
+            self.distances.pop((n1, n), None)
+        # incoming
+        for n1 in self.all_nodes:
+            neighbours = self.neighbours.get(n1, set())
+            neighbours.discard(n)
+            self.distances.pop((n, n1), None)
+            self.distances.pop((n1, n), None)
 
     def merge_nodes(self, n1: NodeId, n2: NodeId):
         # print('removing duplicate node', n2)
-        neighbours_n1 = self.neighbours[n1]
+        neighbours_n1 = self.neighbours.get(n1, set())
         neighbours_n2 = self.neighbours.pop(n2, {})
         for n3 in neighbours_n2:
             d = self.distances.pop((n2, n3))
             self.distances.pop((n3, n2), None)
-            self.neighbours[n3].discard(n2)
+            self.neighbours.get(n3, set()).discard(n2)
             # do not allow self loops!
             if n3 != n1 and n3 not in neighbours_n1:
                 # and add all the new edges to node 1
                 self.add_undirected_edge(n1, n3, d)
-        self.all_nodes.remove(n2)
+
+        self.remove_node(n2)
+        self.merged_id_mapping[n2] = n1  # mapping from -> to
 
     def modified_a_star(self, start: int, goal: int, goal_coords: np.ndarray) -> Tuple[List[int], Optional[float]]:
         """implementation of the popular A* algorithm with optimisations for this special use case
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 37be43a..12931ce 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -876,7 +876,7 @@ def clean_visible_idxs(
     visible_idxs: Set[int], cand_idx2repr: Dict[int, float], vert_idx2dist: Dict[int, float]
 ) -> Set[int]:
     # in case some vertices have the same representation, only return (link) the closest vertex
-    if len(visible_idxs) == 0:
+    if len(visible_idxs) <= 1:
         return visible_idxs
 
     cleaned = set()
diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index 8341fbf..b786bd4 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -56,7 +56,7 @@ def draw_polygon(ax, coords, **kwargs):
 
 
 def draw_boundaries(map, ax):
-    # TODO outside light grey
+    # TODO outside dark grey
     # TODO fill holes light grey
     draw_polygon(ax, map.boundary_polygon.coordinates)
     for h in map.holes:
@@ -127,7 +127,6 @@ def draw_with_path(map, temp_graph, vertex_path):
     fig, ax = plt.subplots()
 
     coords_map = temp_graph.coord_map
-    coords = coords_map.values()
     all_nodes = temp_graph.all_nodes
     draw_boundaries(map, ax)
     draw_internal_graph(map, ax)
@@ -139,14 +138,14 @@ def draw_with_path(map, temp_graph, vertex_path):
         start, goal = vertex_path[0], vertex_path[-1]
         goal_idx = None
         start_idx = None
-        for i, c in enumerate(coords):
+        for i, c in coords_map.items():
             if np.array_equal(c, goal):
                 goal_idx = i
             if np.array_equal(c, start):
                 start_idx = i
 
         if start_idx is not None:
-            for n_idx in temp_graph.neighbours[start_idx]:
+            for n_idx in temp_graph.get_neighbours_of(start_idx):
                 n = coords_map[n_idx]
                 draw_edge(start, n, c="y", alpha=0.7)
 
diff --git a/tests/main_test.py b/tests/main_test.py
index e8851a0..9fbe001 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -7,8 +7,8 @@
 from extremitypathfinder.plotting import PlottingEnvironment
 
 # TODO
-# PLOT_TEST_RESULTS = True
-PLOT_TEST_RESULTS = False
+PLOT_TEST_RESULTS = True
+# PLOT_TEST_RESULTS = False
 TEST_PLOT_OUTPUT_FOLDER = "plots"
 
 if PLOT_TEST_RESULTS:

From f8345779dc44bb9afd8b594a435dd17b62629bb1 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 02:27:16 +0200
Subject: [PATCH 26/44] cleanup1

---
 extremitypathfinder/extremitypathfinder.py | 153 +++----------
 extremitypathfinder/helper_classes.py      |   7 +-
 extremitypathfinder/helper_fcts.py         | 247 +--------------------
 tests/main_test.py                         |   4 +-
 4 files changed, 44 insertions(+), 367 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 16e20f3..107708d 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -24,9 +24,8 @@
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
     convert_gridworld,
-    find_visible,
     find_visible2,
-    find_within_range2,
+    find_within_range,
     get_angle_repr,
     inside_polygon,
 )
@@ -189,10 +188,16 @@ def translate(self, new_origin: Vertex):
         for p in self.polygons:
             p.translate(new_origin)
 
-    def prepare(self):  # TODO include in storing functions? breaking change!
+    def prepare(self):
         """Computes a visibility graph optimized (=reduced) for path planning and stores it
 
         Computes all directly reachable extremities based on visibility and their distance to each other
+        pre-procesing of the map. pre-computation for faster shortest path queries
+        optimizes graph further at construction time
+
+        NOTE: initialise the graph with all extremities.
+        even if a node has no edges (visibility to other extremities, dangling node),
+        it must still be included!
 
         .. note::
             Multiple polygon vertices might have identical coords_rel.
@@ -202,29 +207,22 @@ def prepare(self):  # TODO include in storing functions? breaking change!
         .. note::
             Pre computing the shortest paths between all directly reachable extremities
             and storing them in the graph would not be an advantage, because then the graph is fully connected.
-            A star would visit every node in the graph at least once (-> disadvantage!).
+            A* would visit every node in the graph at least once (-> disadvantage!).
         """
         if self.prepared:
             raise ValueError("this environment is already prepared. load new polygons first.")
 
-        # preprocessing the map
-        # construct graph of visible (=directly reachable) extremities
-        # and optimize graph further at construction time
-        # NOTE: initialise the graph with all extremities.
-        #   even if a node has no edges (visibility to other extremities), it should still be included!
         extremities = self.all_extremities
         nr_extremities = len(extremities)
         if nr_extremities == 0:
-            # TODO
             self.graph = DirectedHeuristicGraph()
             return
 
         # TODO pre computation and storage of all
-        vertices = self.all_vertices
+        # TODO more performant way of computing
         nr_vertices = self.nr_vertices
         extremity_indices = self.extremity_indices
         extremity_mask = self.extremity_mask
-        # TODO more performant way of computing
         all_edges = self.all_edges
         coords = self.coords
         vertex_edge_idxs = self.vertex_edge_idxs
@@ -281,7 +279,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # (angle representation not None)
             # the origin extremity itself must also not be checked when looking for visible neighbours
             idx2repr = {i: r for i, r in idx2repr.items() if r is not None}
-            idxs_behind = find_within_range2(
+            idxs_behind = find_within_range(
                 n1_repr,
                 n2_repr,
                 idx2repr,
@@ -328,7 +326,8 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            lie_in_front_idxs = find_within_range2(
+            idx2repr = {i: r for i, r in idx2repr.items() if i not in idxs_behind}
+            lie_in_front_idxs = find_within_range(
                 n1_repr_inv,
                 n2_repr_inv,
                 idx2repr,
@@ -339,9 +338,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
             # do not consider points found to lie behind
-            # TODO remvoe
-            idx2repr = {i: r for i, r in idx2repr.items() if i not in lie_in_front_idxs and i not in idxs_behind}
-            candidate_idxs = set(idx2repr.keys())
+            candidate_idxs = {i for i in idx2repr.keys() if i not in lie_in_front_idxs and i not in idxs_behind}
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
             nr_edges = len(all_edges)
@@ -362,24 +359,13 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
                 candidate_idxs,
             )
 
-            self.translate(vertices[origin_idx])
-            candidates = {vertices[i] for i in idx2repr.keys()}
-            edges2check = {all_edges[i] for i in edge_idxs2check}
-            visibles_n_distances_goal = find_visible(candidates, edges_to_check=edges2check)
-            visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_goal}
-
             visible_vertex2dist_map = {i: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
-
-            if not visible_idxs_ == visible_idxs:
-                _ = 1
-                # raise ValueError
-
             graph.add_multiple_undirected_edges(origin_idx, visible_vertex2dist_map)
             # optimisation: "thin out" the graph
             # remove already existing edges in the graph to the extremities in front
             graph.remove_multiple_undirected_edges(origin_idx, lie_in_front_idxs)
 
-        graph.make_clean()  # join all nodes with the same coords_rel
+        graph.join_identical()  # join all nodes with the same coords_rel
 
         self.graph = graph
         self.prepared = True
@@ -445,13 +431,12 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             return [start_coordinates, goal_coordinates], 0.0
 
         # TODO pre computation and storage of all
-        all_edges = self.all_edges
-        nr_edges = len(all_edges)
-
         # TODO more performant way of computing
+        nr_edges = len(self.all_edges)
 
         vertex_edge_idxs = self.vertex_edge_idxs
         edge_vertex_idxs = self.edge_vertex_idxs
+        # temporarily extend data structures
         extremity_mask = np.append(self.extremity_mask, (False, False))
         coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
         idx_start = self.nr_vertices
@@ -460,37 +445,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         vert_idx2dist_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
         vert_idx2dist_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
 
-        # TODO detect if coordinates match an extremity (node in the graph)
-        # extemity_indices = self.graph.all_nodes
-        # check if start and goal nodes have identical coordinates with one of the vertices
-        # optimisations for visibility test can be made in this case:
-        # for extremities the visibility has already been (except for in front) computed
-        # TODO
-        # BUT: too many cases possible: e.g. multiple vertices identical to query point...
-        # -> always create new query vertices
-        # include start and goal vertices in the graph
-        # check_start_node_vis = True
-        # check_goal_node_vis = True
-        # for i in extemity_indices:
-        #     dist_start = vert_idx2dist_start[i]
-        #     if dist_start == 0.0:
-        #         idx_start = i
-        #         check_start_node_vis = False
-        #         break
-        #
-        # for i in extemity_indices:
-        #     dist_goal = vert_idx2dist_goal[i]
-        #     if dist_goal == 0.0:
-        #         idx_start = i
-        #         check_goal_node_vis = False
-        #         break
-
-        start_vertex = Vertex(start_coordinates)
-        goal_vertex = Vertex(goal_coordinates)
-
-        # TODO required?!
-        # vertices = self.all_vertices + [start_vertex, goal_vertex]
-        vertices = self.all_vertices + [start_vertex, goal_vertex]
+        # start and goal nodes could be identical with one ore more of the vertices
+        # BUT: this is an edge case -> compute visibility as usual and later try to merge with the graph
 
         # create temporary graph TODO make more performant, avoid real copy
         # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
@@ -500,14 +456,6 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
 
         # check the goal node first (earlier termination possible)
         idx_origin = idx_goal
-        coords_origin = coords[idx_origin]
-
-        # TODO
-        self.translate(new_origin=goal_vertex)  # do before checking angle representations!
-        # IMPORTANT: manually translate the start vertex, because it is not part of any polygon
-        #   and hence does not get translated automatically
-        start_vertex.mark_outdated()
-
         # the visibility of only the graphs nodes has to be checked (not all extremities!)
         # points with the same angle representation should not be considered visible
         # (they also cause errors in the algorithms, because their angle repr is not defined!)
@@ -515,25 +463,12 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
         candidate_idxs.add(idx_start)
+        coords_origin = coords[idx_origin]
         vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
         candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
-
-        candidates = {vertices[i] for i in candidate_idxs}
-        visibles_n_distances_goal = find_visible(candidates, edges_to_check=self.all_edges)
-
-        # TODO visibility of start vertex
-        # TODO use new variant
-        visible_idxs = {vertices.index(v) for v, d in visibles_n_distances_goal}
         edge_idxs2check = set(range(nr_edges))
-
-        # TODO
-        # cand_idxs = {vertices.index(n) for n in self.graph.get_all_nodes()}
-        # cand_idxs.add(idx_start)
-
         vert_idx2dist = vert_idx2dist_goal
-
-        # TODO
-        visible_idxs_ = find_visible2(
+        visible_idxs = find_visible2(
             extremity_mask,
             coords,
             vertex_edge_idxs,
@@ -544,46 +479,30 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             vert_idx2dist,
             candidate_idxs,
         )
-        # if not visible_idxs == visible_idxs_:
-        #     diff = visible_idxs_ ^ visible_idxs
-        #     diff_nodes = {vertices[i] for i in diff}
-        #     _ = 1
-
-        # visible_vertex2dist_map = {vertices[i]: get_distance_to_origin(coords_origin, i) for i in visible_idxs}
-        # visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs}
-        visibles_n_distances_goal = {(vertices[i], vert_idx2dist[i]) for i in visible_idxs_}
+        visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
 
-        if len(visibles_n_distances_goal) == 0:
+        if len(visibles_n_distances_map) == 0:
             # The goal node does not have any neighbours. Hence there is not possible path to the goal.
             return [], None
 
-        # IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node
-        #   instead of visiting other nodes first (there is never an advantage through reduced edge weight)
-        # -> when goal is directly reachable, there can be no other shorter path to it. Terminate
-        for v, d in visibles_n_distances_goal:
-            if v == start_vertex:
+        for i, d in visibles_n_distances_map.items():
+            if i == idx_start:
+                # IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node
+                #   instead of visiting other nodes first (there is never an advantage through reduced edge weight)
+                # -> when goal is directly reachable, there can be no other shorter path to it. Terminate
                 return [start_coordinates, goal_coordinates], d
 
             # add unidirectional edges to the temporary graph
             # add edges in the direction: extremity (v) -> goal
-            v_idx = vertices.index(v)
-            graph.add_directed_edge(v_idx, idx_goal, d)
+            graph.add_directed_edge(i, idx_goal, d)
 
         idx_origin = idx_start
         coords_origin = coords[idx_origin]
-        self.translate(new_origin=start_vertex)  # do before checking angle representations!
         # the visibility of only the graphs nodes have to be checked
         # the goal node does not have to be considered, because of the earlier check
         candidate_idxs = self.graph.get_all_nodes()
-        # candidate_idxs.remove(idx_goal)
         vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
         candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
-
-        # TODO use new variant
-        candidates = {vertices[i] for i in candidate_idxs}
-        visibles_n_distances_start = find_visible(candidates, edges_to_check=self.all_edges)
-        visible_idxs_ = {vertices.index(v) for v, d in visibles_n_distances_start}
-
         edge_idxs2check = set(range(nr_edges))  # new copy
         vert_idx2dist = vert_idx2dist_start
         visible_idxs = find_visible2(
@@ -598,19 +517,12 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             candidate_idxs,
         )
 
-        if not visible_idxs == visible_idxs_:
-            _ = 1
-            # raise ValueError
-
         if len(visible_idxs) == 0:
             # The start node does not have any neighbours. Hence there is no possible path to the goal.
             return [], None
 
         # add edges in the direction: start -> extremity
         visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
-        # visibles_n_distances_map = {vertices.index(v): d for v, d in visibles_n_distances_start}
-        if idx_start in visible_idxs:
-            raise ValueError
         graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
 
         # # TODO optimisation
@@ -672,17 +584,20 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         graph.join_identical()
 
         vertex_id_path, distance = graph.modified_a_star(idx_start, idx_goal, coords_goal)
-        vertex_path = [vertices[i] for i in vertex_id_path]
 
         # clean up
+        # TODO re-use the same graph
         graph.remove_node(idx_start)
         graph.remove_node(idx_goal)
         if free_space_after:
             del graph  # free the memory
+
         else:
             self.temp_graph = graph
+
         # extract the coordinates from the path
-        return [tuple(v.coordinates) for v in vertex_path], distance
+        vertex_path = [tuple(coords[i]) for i in vertex_id_path]
+        return vertex_path, distance
 
 
 if __name__ == "__main__":
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index f57f8a9..344c727 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -446,13 +446,10 @@ def remove_multiple_undirected_edges(self, node1: NodeId, node2_iter: Iterable[N
         for node2 in node2_iter:
             self.remove_undirected_edge(node1, node2)
 
-    def make_clean(self):
+    def join_identical(self):
         # for shortest path computations all graph nodes should be unique
-        self.join_identical()
+        # join all nodes with the same coordinates
         # leave dangling nodes! (they might become reachable by adding start and and goal node!)
-
-    def join_identical(self):
-        # join all nodes with the same coordinates,
         nodes_to_check = self.all_nodes.copy()
         while len(nodes_to_check) > 1:
             n1 = nodes_to_check.pop()
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 12931ce..1154325 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,14 +1,13 @@
+# TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
 from itertools import combinations
 from typing import Dict, List, Set, Tuple
 
 import numpy as np
-
-# TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import numpy.linalg
 
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import PolygonVertex, compute_angle_repr, compute_angle_repr_inner
+from extremitypathfinder.helper_classes import compute_angle_repr, compute_angle_repr_inner
 
 
 def inside_polygon(x, y, coords, border_value):
@@ -213,8 +212,7 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
     # TODO data rectification
 
 
-#
-# # TODO use caching variant
+# # TODO use caching variant: dict with tuple { (i1,i2): repr } <- Numba?!
 # def get_angle_representation(idx_origin: int, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
 #     repr = repr_matrix[idx_origin, idx_v]
 #
@@ -241,65 +239,7 @@ def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
     return repr
 
 
-def find_within_range(repr1: float, repr2: float, vertex_set, angle_range_less_180, equal_repr_allowed):
-    """
-    filters out all vertices whose representation lies within the range between
-      the two given angle representations
-    which range ('clockwise' or 'counter-clockwise') should be checked is determined by:
-      - query angle (range) is < 180deg or not (>= 180deg)
-    :param repr1:
-    :param repr2:
-    :param vertex_set:
-    :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg
-    :param equal_repr_allowed: whether vertices with the same representation should also be returned
-    :return:
-    """
-    if len(vertex_set) == 0:
-        return vertex_set
-
-    repr_diff = abs(repr1 - repr2)
-    if repr_diff == 0.0:
-        return set()
-
-    min_repr = min(repr1, repr2)
-    max_repr = max(repr1, repr2)  # = min_angle + angle_diff
-
-    def repr_within(r):
-        # Note: vertices with the same representation will not NOT be returned!
-        return min_repr < r < max_repr
-
-    # depending on the angle the included range is clockwise or anti-clockwise
-    # (from min_repr to max_val or the other way around)
-    # when the range contains the 0.0 value (transition from 3.99... -> 0.0)
-    # it is easier to check if a representation does NOT lie within this range
-    # -> invert filter condition
-    # special case: angle == 180deg
-    on_line_inv = repr_diff == 2.0 and repr1 >= repr2
-    # which range to filter is determined by the order of the points
-    # since the polygons follow a numbering convention,
-    # the 'left' side of p1-p2 always lies inside the map
-    # -> filter out everything on the right side (='outside')
-    inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180)
-
-    def within_filter_func(r: float) -> bool:
-        repr_eq = r == min_repr or r == max_repr
-        if repr_eq and equal_repr_allowed:
-            return True
-        if repr_eq and not equal_repr_allowed:
-            return False
-
-        res = repr_within(r)
-        if inversion_condition:
-            res = not res
-        return res
-
-    vertex2rep = {v: v.get_angle_representation() for v in vertex_set}
-    vertex2rep = {v: r for v, r in vertex2rep.items() if r is not None}
-    vertices_within = {v for v, r in vertex2rep.items() if within_filter_func(r)}
-    return vertices_within
-
-
-def find_within_range2(
+def find_within_range(
     repr1: float,
     repr2: float,
     idx2repr: Dict[int, float],
@@ -517,181 +457,6 @@ def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: boo
     return boundary_edges, hole_list
 
 
-def find_visible(vertex_candidates, edges_to_check):
-    """
-    # IMPORTANT: self.translate(new_origin=query_vertex) always has to be called before!
-        (for computing the angle representations wrt. the query vertex)
-    query_vertex: a vertex for which the visibility to the vertices should be checked.
-        also non extremity vertices, polygon vertices and vertices with the same coordinates are allowed.
-        query point also might lie directly on an edge! (angle = 180deg)
-    :param vertex_candidates: the set of all vertices which should be checked for visibility.
-        IMPORTANT: is being manipulated, so has to be a copy!
-        IMPORTANT: must not contain the query vertex!
-    :param edges_to_check: the set of edges which determine visibility
-    :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
-    """
-    edges_to_check = set(edges_to_check)
-    visible_vertices = set()
-    if len(vertex_candidates) == 0:
-        return visible_vertices
-
-    # used for increasing the priority of "closer" edges
-    priority_edges = set()
-    # goal: eliminating all vertices lying 'behind' any edge
-    # TODO improvement in combination with priority: process edges roughly in sequence, but still allow jumps
-    #  would follow closer edges more often which have a bigger chance to eliminate candidates -> speed up
-    while len(vertex_candidates) > 0 and len(edges_to_check) > 0:
-        # check prioritized items first
-        try:
-            edge = priority_edges.pop()
-            edges_to_check.remove(edge)
-        except KeyError:
-            edge = edges_to_check.pop()
-
-        lies_on_edge = False
-        range_less_180 = False
-
-        v1, v2 = edge.vertex1, edge.vertex2
-        if v1.get_distance_to_origin() == 0.0:
-            # vertex1 has the same coordinates as the query vertex -> on the edge
-            lies_on_edge = True
-            # (but does not belong to the same polygon, not identical!)
-            # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
-            vertex_candidates.discard(v1)
-            # its angle representation is not defined (no line segment from vertex1 to query vertex!)
-            range_less_180 = v1.is_extremity
-            # do not check the other neighbouring edge of vertex1 in the future
-            e1 = v1.edge1
-            edges_to_check.discard(e1)
-            priority_edges.discard(e1)
-            # everything between its two neighbouring edges is not visible for sure
-            v1, v2 = v1.get_neighbours()
-
-        elif v2.get_distance_to_origin() == 0.0:
-            lies_on_edge = True
-            vertex_candidates.discard(v2)
-            range_less_180 = v2.is_extremity
-            e1 = v2.edge2
-            edges_to_check.discard(e1)
-            priority_edges.discard(e1)
-            v1, v2 = v2.get_neighbours()
-
-        repr1 = v1.get_angle_representation()
-        repr2 = v2.get_angle_representation()
-
-        repr_diff = abs(repr1 - repr2)
-        if repr_diff == 2.0:
-            # angle == 180deg -> on the edge
-            lies_on_edge = True
-
-        if lies_on_edge:
-            # when the query vertex lies on an edge (or vertex) no behind/in front checks must be performed!
-            # the neighbouring edges are visible for sure
-            # attention: only add to visible set if vertex was a candidate!
-            try:
-                vertex_candidates.remove(v1)
-                visible_vertices.add(v1)
-            except KeyError:
-                pass
-            try:
-                vertex_candidates.remove(v2)
-                visible_vertices.add(v2)
-            except KeyError:
-                pass
-
-            # all the candidates between the two vertices v1 v2 are not visible for sure
-            # candidates with the same representation should not be deleted, because they can be visible!
-            invisible_candidates = find_within_range(
-                repr1,
-                repr2,
-                vertex_candidates,
-                angle_range_less_180=range_less_180,
-                equal_repr_allowed=False,
-            )
-            vertex_candidates.difference_update(invisible_candidates)
-            continue
-
-        # case: a 'regular' edge
-        # eliminate all candidates which are blocked by the edge
-        # that means inside the angle range spanned by the edge and actually behind it
-        vertices_to_check = vertex_candidates.copy()
-        # the vertices belonging to the edge itself (its vertices) must not be checked.
-        # use discard() instead of remove() to not raise an error (they might not be candidates)
-        vertices_to_check.discard(v1)
-        vertices_to_check.discard(v2)
-        if len(vertices_to_check) == 0:
-            continue
-
-        # assert repr1 is not None
-        # assert repr2 is not None
-
-        # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge)
-        #   within this angle range
-        # the "view range" of an edge from a query point (spanned by the two vertices of the edge)
-        #   is always < 180deg when the edge is not running through the query point (=180 deg)
-        #  candidates with the same representation as v1 or v2 should be considered.
-        #   they can be visible, but should be ruled out if they lie behind any edge!
-        vertices_to_check = find_within_range(
-            repr1,
-            repr2,
-            vertices_to_check,
-            angle_range_less_180=True,
-            equal_repr_allowed=True,
-        )
-        if len(vertices_to_check) == 0:
-            continue
-
-        # if a candidate is farther away from the query point than both vertices of the edge,
-        #   it surely lies behind the edge
-        # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
-        #   it still needs to be checked!
-        v1_dist = v1.get_distance_to_origin()
-        v2_dist = v2.get_distance_to_origin()
-        max_distance = max(v1_dist, v2_dist)
-        vertices_behind = {v for v in vertices_to_check if v.get_distance_to_origin() > max_distance}
-        # they do not have to be checked, no intersection computation necessary
-        # TODO improvement: increase the neighbouring edges' priorities when there were extremities behind
-        vertices_to_check.difference_update(vertices_behind)
-
-        vertices_in_front = set()
-        # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
-        #    has an intersection with the current edge p1---p2
-        p1 = v1.get_coordinates_translated()
-        p2 = v2.get_coordinates_translated()
-        for vertex in vertices_to_check:
-            if lies_behind(p1, p2, vertex.get_coordinates_translated()):
-                vertices_behind.add(vertex)
-            else:
-                vertices_in_front.add(vertex)
-
-        # vertices behind any edge are not visible
-        vertex_candidates.difference_update(vertices_behind)
-        # if there are no more candidates left. immediately quit checking edges
-        if len(vertex_candidates) == 0:
-            break
-
-        # check the neighbouring edges of all vertices which lie in front of the edge next first
-        # (prioritize them)
-        # they lie in front and hence will eliminate other vertices faster
-        # the fewer vertex candidates remain, the faster the procedure
-        # TODO possible improvement: increase priority every time and draw highest priority items
-        #   but this involves sorting (expensive for large polygons!)
-        #   idea: work with a list of sets, add new set for higher priority, no real sorting, but still managing!
-        #   test speed impact.
-        for e in vertices_in_front:
-            # only add the neighbour edges to the priority set if they still have to be checked!
-            if isinstance(e, PolygonVertex):
-                # only vertices belonging to polygons have neighbours
-                priority_edges.update(edges_to_check.intersection({e.edge1, e.edge2}))
-
-    # all edges have been checked
-    # all remaining vertices were not concealed behind any edge and hence are visible
-    visible_vertices.update(vertex_candidates)
-
-    # return a set of tuples: (vertex, distance)
-    return {(e, e.get_distance_to_origin()) for e in visible_vertices}
-
-
 def find_visible2(
     extremity_mask: np.ndarray,
     coords: np.ndarray,
@@ -812,7 +577,7 @@ def is_extremity(i: int) -> bool:
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation must not be deleted, because they can be visible!
             cand_idx2repr = {i: vert_idx2repr[i] for i in cand_idxs}
-            invisible_candidate_idxs = find_within_range2(
+            invisible_candidate_idxs = find_within_range(
                 repr1,
                 repr2,
                 cand_idx2repr,
@@ -839,7 +604,7 @@ def is_extremity(i: int) -> bool:
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
         idx2repr = {i: vert_idx2repr[i] for i in cand_idxs_tmp}
-        idxs2check = find_within_range2(
+        idxs2check = find_within_range(
             repr1,
             repr2,
             idx2repr,
diff --git a/tests/main_test.py b/tests/main_test.py
index 9fbe001..e8851a0 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -7,8 +7,8 @@
 from extremitypathfinder.plotting import PlottingEnvironment
 
 # TODO
-PLOT_TEST_RESULTS = True
-# PLOT_TEST_RESULTS = False
+# PLOT_TEST_RESULTS = True
+PLOT_TEST_RESULTS = False
 TEST_PLOT_OUTPUT_FOLDER = "plots"
 
 if PLOT_TEST_RESULTS:

From 025d99fe49bfa3520a975a06e36ce4cdbd190e88 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 03:45:48 +0200
Subject: [PATCH 27/44] refactoring

---
 extremitypathfinder/extremitypathfinder.py | 112 ++++++++++++---------
 extremitypathfinder/helper_classes.py      |  25 ++---
 2 files changed, 73 insertions(+), 64 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 107708d..c84c394 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -71,20 +71,6 @@ def all_vertices(self) -> List[PolygonVertex]:
             self._all_vertices = list(itertools.chain(*iter(p.vertices for p in self.polygons)))
         return self._all_vertices
 
-    @property
-    def coords(self) -> np.ndarray:
-        return np.stack([v.coordinates for v in self.all_vertices])
-
-    @property
-    def vertex_edge_idxs(self) -> np.ndarray:
-        all_edges = self.all_edges
-        return np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in self.all_vertices])
-
-    @property
-    def edge_vertex_idxs(self) -> np.ndarray:
-        vertices = self.all_vertices
-        return np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in self.all_edges])
-
     @property
     def nr_vertices(self) -> int:
         return len(self.all_vertices)
@@ -115,6 +101,20 @@ def all_extremities(self) -> List[PolygonVertex]:
     def all_edges(self) -> List[Edge]:
         return list(itertools.chain(*iter(p.edges for p in self.polygons)))
 
+    @property
+    def coords(self) -> np.ndarray:
+        return np.stack([v.coordinates for v in self.all_vertices])
+
+    @property
+    def vertex_edge_idxs(self) -> np.ndarray:
+        all_edges = self.all_edges
+        return np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in self.all_vertices])
+
+    @property
+    def edge_vertex_idxs(self) -> np.ndarray:
+        vertices = self.all_vertices
+        return np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in self.all_edges])
+
     def store(
         self,
         boundary_coordinates: INPUT_COORD_LIST_TYPE,
@@ -140,11 +140,40 @@ def store(
         """
         self.prepared = False
         # loading the map
-        boundary_coordinates = np.array(boundary_coordinates)
-        list_of_hole_coordinates = [np.array(hole_coords) for hole_coords in list_of_hole_coordinates]
+        boundary_coordinates = np.array(boundary_coordinates, dtype=float)
+        list_of_hole_coordinates = [np.array(hole_coords, dtype=float) for hole_coords in list_of_hole_coordinates]
         if validate:
             check_data_requirements(boundary_coordinates, list_of_hole_coordinates)
 
+        list_of_polygons = [boundary_coordinates] + list_of_hole_coordinates
+        nr_total_pts = sum(map(len, list_of_polygons))
+        vertex_edge_idxs = np.empty((nr_total_pts, 2), dtype=int)
+        edge_vertex_idxs = np.empty((nr_total_pts, 2), dtype=int)
+        edge_idx = 0
+        offset = 0
+        for poly in list_of_polygons:
+            nr_coords = len(poly)
+            v1 = -1 % nr_coords
+            # TODO col 1 is just np.arange?!
+            for v2 in range(nr_coords):
+                v1_idx = v1 + offset
+                v2_idx = v2 + offset
+                edge_vertex_idxs[edge_idx, 0] = v1_idx
+                edge_vertex_idxs[edge_idx, 1] = v2_idx
+                vertex_edge_idxs[v1_idx, 0] = edge_idx
+                vertex_edge_idxs[v2_idx, 1] = edge_idx
+                # move to the next vertex/edge
+                v1 = v2
+                edge_idx += 1
+
+            offset = edge_idx
+
+        assert edge_idx == nr_total_pts
+
+        # self.coords = np.concatenate(list_of_polygons, axis=0)
+        # self.vertex_edge_idxs = vertex_edge_idxs
+        # self.edge_vertex_idxs = edge_vertex_idxs
+
         self.boundary_polygon = Polygon(boundary_coordinates, is_hole=False)
         # IMPORTANT: make a copy of the list instead of linking to the same list (python!)
         self.holes = [Polygon(coordinates, is_hole=True) for coordinates in list_of_hole_coordinates]
@@ -223,7 +252,6 @@ def prepare(self):
         nr_vertices = self.nr_vertices
         extremity_indices = self.extremity_indices
         extremity_mask = self.extremity_mask
-        all_edges = self.all_edges
         coords = self.coords
         vertex_edge_idxs = self.vertex_edge_idxs
         edge_vertex_idxs = self.edge_vertex_idxs
@@ -256,8 +284,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
 
         extremity_coord_map = {i: coords[i] for i in extremity_indices}
         graph = DirectedHeuristicGraph(extremity_coord_map)
-        # TODO use orig_ptr
-        for _orig_ptr, origin_idx in enumerate(extremity_indices):
+        for origin_idx in extremity_indices:
             # vertices all belong to a polygon
             idx_n1, idx_n2 = get_neighbours(origin_idx)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
@@ -273,12 +300,15 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # TODO lazy init? same as angle repr
             vert_idx2dist = {i: get_distance_to_origin(origin_idx, i) for i in range(nr_vertices)}
             vert_idx2repr = {i: get_repr(origin_idx, i) for i in range(nr_vertices)}
-
-            idx2repr = {i: get_repr(origin_idx, i) for i in extremity_indices}
+            idx2repr = {i: vert_idx2repr[i] for i in extremity_indices}
             # only consider extremities with coords_rel different from the query extremity
             # (angle representation not None)
             # the origin extremity itself must also not be checked when looking for visible neighbours
-            idx2repr = {i: r for i, r in idx2repr.items() if r is not None}
+            # extremities are always visible to each other
+            # (bi-directional relation -> undirected edges in the graph)
+            #  -> do not check extremities which have been checked already
+            #  (must give the same result when algorithms are correct)
+            idx2repr = {i: r for i, r in idx2repr.items() if r is not None and i > origin_idx}
             idxs_behind = find_within_range(
                 n1_repr,
                 n2_repr,
@@ -286,27 +316,9 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-
-            # TODO
-            # extremities are always visible to each other
-            # (bi-directional relation -> undirected edges in the graph)
-            #  -> do not check extremities which have been checked already
-            #  (must give the same result when algorithms are correct)
-            # idx2repr_tmp = {i: r for i, r in idx2repr.items() if i > origin_idx}
-            # idxs_behind = find_within_range2(
-            #     n1_repr,
-            #     n2_repr,
-            #     idx2repr_tmp,
-            #     angle_range_less_180=True,
-            #     equal_repr_allowed=False,
-            # )
-            #
-            # if idxs_behind_ != idxs_behind:
-            #     x = 1
-            #
-            # idxs_behind__ = {i for i in idxs_behind_ if i > origin_idx}
-            # if idxs_behind__ != idxs_behind:
-            #     raise ValueError
+            # do not consider points found to lie behind
+            for i in idxs_behind:
+                idx2repr.pop(i)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -326,22 +338,22 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            idx2repr = {i: r for i, r in idx2repr.items() if i not in idxs_behind}
-            lie_in_front_idxs = find_within_range(
+            idxs_in_front = find_within_range(
                 n1_repr_inv,
                 n2_repr_inv,
                 idx2repr,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
-
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
-            # do not consider points found to lie behind
-            candidate_idxs = {i for i in idx2repr.keys() if i not in lie_in_front_idxs and i not in idxs_behind}
+            for i in idxs_in_front:
+                idx2repr.pop(i)
+
+            candidate_idxs = set(idx2repr.keys())
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
-            nr_edges = len(all_edges)
+            nr_edges = len(self.all_edges)
             edge_idxs2check = set(range(nr_edges))
             edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
             edge_idxs2check.remove(edge1_idx)
@@ -363,7 +375,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             graph.add_multiple_undirected_edges(origin_idx, visible_vertex2dist_map)
             # optimisation: "thin out" the graph
             # remove already existing edges in the graph to the extremities in front
-            graph.remove_multiple_undirected_edges(origin_idx, lie_in_front_idxs)
+            graph.remove_multiple_undirected_edges(origin_idx, idxs_in_front)
 
         graph.join_identical()  # join all nodes with the same coords_rel
 
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 344c727..fe3541f 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -243,16 +243,13 @@ def _find_extremities(self):
             holes: clockwise
         :return:
         """
-        extremities = []
+        coordinates = [v.coordinates for v in self.vertices]
+        extr_idxs = []
         # extremity_indices = []
         # extremity_index = -1
-        v1 = self.vertices[-2]
-        p1 = v1.coordinates
-        v2 = self.vertices[-1]
-        p2 = v2.coordinates
-
-        for v3 in self.vertices:
-            p3 = v3.coordinates
+        p1 = coordinates[-2]
+        p2 = coordinates[-1]
+        for i, p3 in enumerate(coordinates):
             # since consequent vertices are not permitted to be equal,
             #   the angle representation of the difference is well defined
             diff_p3_p2 = p3 - p2
@@ -269,17 +266,17 @@ def _find_extremities(self):
                 # if the representation lies within quadrant 0 or 1 (<2.0), the inside angle
                 #   (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
                 # then p2 = extremity
-                v2.declare_extremity()
-                extremities.append(v2)
+                idx_p2 = i - 1
+                extr_idxs.append(idx_p2)
 
             # move to the next point
-            # vertex1=vertex2
-            # TODO optimise
-            diff_p1_p2 = diff_p3_p2
+            # diff_p1_p2 = diff_p3_p2
             p1 = p2
-            v2 = v3
             p2 = p3
 
+        extremities = [self.vertices[i] for i in extr_idxs]
+        for v in extremities:
+            v.declare_extremity()
         self._extremities = extremities
 
     @property

From 9ed1770892a176b3b64c212254df0d30a3222f26 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 04:04:29 +0200
Subject: [PATCH 28/44] prepare data structure precomp

---
 extremitypathfinder/extremitypathfinder.py | 25 +++++--
 extremitypathfinder/helper_classes.py      | 77 +++++++++++-----------
 2 files changed, 59 insertions(+), 43 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index c84c394..de67039 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -20,6 +20,7 @@
     PolygonVertex,
     Vertex,
     angle_rep_inverse,
+    compute_extremity_idxs,
 )
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
@@ -145,6 +146,11 @@ def store(
         if validate:
             check_data_requirements(boundary_coordinates, list_of_hole_coordinates)
 
+        self.boundary_polygon = Polygon(boundary_coordinates, is_hole=False)
+
+        # IMPORTANT: make a copy of the list instead of linking to the same list (python!)
+        self.holes = [Polygon(coordinates, is_hole=True) for coordinates in list_of_hole_coordinates]
+
         list_of_polygons = [boundary_coordinates] + list_of_hole_coordinates
         nr_total_pts = sum(map(len, list_of_polygons))
         vertex_edge_idxs = np.empty((nr_total_pts, 2), dtype=int)
@@ -170,13 +176,20 @@ def store(
 
         assert edge_idx == nr_total_pts
 
-        # self.coords = np.concatenate(list_of_polygons, axis=0)
-        # self.vertex_edge_idxs = vertex_edge_idxs
-        # self.edge_vertex_idxs = edge_vertex_idxs
+        extremity_idxs = compute_extremity_idxs(self.coords)
+        # Attention: only consider extremities that are actually within the map
+        extremity_idxs = [i for i in extremity_idxs if self.within_map(self.coords[i])]
 
-        self.boundary_polygon = Polygon(boundary_coordinates, is_hole=False)
-        # IMPORTANT: make a copy of the list instead of linking to the same list (python!)
-        self.holes = [Polygon(coordinates, is_hole=True) for coordinates in list_of_hole_coordinates]
+        mask = np.full(nr_total_pts, False, dtype=bool)
+        mask[self.extremity_indices] = True
+
+        # coords = np.concatenate(list_of_polygons, axis=0)
+
+        # self.extremity_mask = mask
+        # self.edge_vertex_idxs = edge_vertex_idxs
+        # self.vertex_edge_idxs = vertex_edge_idxs
+        # self.coords = coords
+        # self.extremity_indices = extremity_idxs
 
     def store_grid_world(
         self,
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index fe3541f..4984a67 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -161,6 +161,45 @@ def angle_rep_inverse(repr: Optional[float]) -> Optional[float]:
     return repr_inv
 
 
+def compute_extremity_idxs(coordinates: np.ndarray) -> List[int]:
+    """identify all protruding points = vertices with an inside angle of > 180 degree ('extremities')
+    expected edge numbering:
+        outer boundary polygon: counter clockwise
+        holes: clockwise
+
+    basic idea:
+      - translate the coordinate system to have p2 as origin
+      - compute the angle representations of both vectors representing the edges
+      - "rotate" the coordinate system (equal to deducting) so that the p1p2 representation is 0
+      - check in which quadrant the p2p3 representation lies
+    %4 because the quadrant has to be in [0,1,2,3] (representation in [0:4[)
+    if the representation lies within quadrant 0 or 1 (<2.0), the inside angle
+      (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
+    then p2 = extremity
+    :param coordinates:
+    :return:
+    """
+    nr_coordinates = len(coordinates)
+    extr_idxs = []
+    p1 = coordinates[-2]
+    p2 = coordinates[-1]
+    for i, p3 in enumerate(coordinates):
+        # since consequent vertices are not permitted to be equal,
+        #   the angle representation of the difference is well defined
+        diff_p3_p2 = p3 - p2
+        diff_p1_p2 = p1 - p2
+        rep_diff = compute_angle_repr_inner(diff_p3_p2) - compute_angle_repr_inner(diff_p1_p2)
+        if rep_diff % 4.0 < 2.0:  #
+            # p2 is an extremity
+            idx_p2 = (i - 1) % nr_coordinates
+            extr_idxs.append(idx_p2)
+
+        # move to the next point
+        p1 = p2
+        p2 = p3
+    return extr_idxs
+
+
 class PolygonVertex(Vertex):
     # __slots__ declared in parents are available in child classes. However, child subclasses will get a __dict__
     # and __weakref__ unless they also define __slots__ (which should only contain names of any additional slots).
@@ -236,44 +275,8 @@ def __init__(self, coordinate_list, is_hole):
         self._extremities: Optional[List[PolygonVertex]] = None
 
     def _find_extremities(self):
-        """
-        identify all protruding points = vertices with an inside angle of > 180 degree ('extremities')
-        expected edge numbering:
-            outer boundary polygon: counter clockwise
-            holes: clockwise
-        :return:
-        """
         coordinates = [v.coordinates for v in self.vertices]
-        extr_idxs = []
-        # extremity_indices = []
-        # extremity_index = -1
-        p1 = coordinates[-2]
-        p2 = coordinates[-1]
-        for i, p3 in enumerate(coordinates):
-            # since consequent vertices are not permitted to be equal,
-            #   the angle representation of the difference is well defined
-            diff_p3_p2 = p3 - p2
-            # TODO optimise
-            diff_p1_p2 = p1 - p2
-
-            if (AngleRepresentation(diff_p3_p2).value - AngleRepresentation(diff_p1_p2).value) % 4 < 2.0:
-                # basic idea:
-                #   - translate the coordinate system to have p2 as origin
-                #   - compute the angle representations of both vectors representing the edges
-                #   - "rotate" the coordinate system (equal to deducting) so that the p1p2 representation is 0
-                #   - check in which quadrant the p2p3 representation lies
-                # %4 because the quadrant has to be in [0,1,2,3] (representation in [0:4[)
-                # if the representation lies within quadrant 0 or 1 (<2.0), the inside angle
-                #   (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
-                # then p2 = extremity
-                idx_p2 = i - 1
-                extr_idxs.append(idx_p2)
-
-            # move to the next point
-            # diff_p1_p2 = diff_p3_p2
-            p1 = p2
-            p2 = p3
-
+        extr_idxs = compute_extremity_idxs(coordinates)
         extremities = [self.vertices[i] for i in extr_idxs]
         for v in extremities:
             v.declare_extremity()

From 9bc4cafb0371969753f17956a2defcb30fcc186a Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 14:04:42 +0200
Subject: [PATCH 29/44] pre compute repr_n_dists

---
 .pre-commit-config.yaml                    |   2 +
 extremitypathfinder/extremitypathfinder.py | 241 ++++++---------------
 extremitypathfinder/global_settings.py     |   4 +-
 extremitypathfinder/helper_classes.py      |  54 +++++
 extremitypathfinder/helper_fcts.py         |  63 ++++++
 5 files changed, 192 insertions(+), 172 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 97ca922..7a1288a 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -44,8 +44,10 @@ repos:
     hooks:
       - id: flake8
         exclude: ^(docs|scripts|tests)/
+        # E203 whitespace before ':'
         args:
           - --max-line-length=120
+          - --ignore=E203
         additional_dependencies:
           - flake8-bugbear
           - flake8-comprehensions
diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index de67039..e115854 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,32 +1,30 @@
-import itertools
 import pickle
 from copy import deepcopy
-from typing import Iterable, List, Optional, Tuple
+from typing import List, Optional, Tuple
 
 import numpy as np
 
 from extremitypathfinder.global_settings import (
     DEFAULT_PICKLE_NAME,
     INPUT_COORD_LIST_TYPE,
-    INPUT_COORD_TYPE,
     LENGTH_TYPE,
     OBSTACLE_ITER_TYPE,
     PATH_TYPE,
+    InputCoords,
 )
 from extremitypathfinder.helper_classes import (
     DirectedHeuristicGraph,
-    Edge,
     Polygon,
     PolygonVertex,
-    Vertex,
     angle_rep_inverse,
     compute_extremity_idxs,
+    compute_repr_n_dist,
 )
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
     convert_gridworld,
     find_visible2,
-    find_within_range,
+    find_within_range2,
     get_angle_repr,
     inside_polygon,
 )
@@ -62,59 +60,13 @@ class PolygonEnvironment:
     _all_vertices: Optional[List[PolygonVertex]] = None
 
     @property
-    def polygons(self) -> Iterable[Polygon]:
-        yield self.boundary_polygon
-        yield from self.holes
+    def nr_edges(self) -> int:
+        return self.nr_vertices
 
     @property
-    def all_vertices(self) -> List[PolygonVertex]:
-        if self._all_vertices is None:
-            self._all_vertices = list(itertools.chain(*iter(p.vertices for p in self.polygons)))
-        return self._all_vertices
-
-    @property
-    def nr_vertices(self) -> int:
-        return len(self.all_vertices)
-
-    @property
-    def extremity_indices(self) -> List[int]:
-        # TODO refactor
-        for p in self.polygons:
-            p._find_extremities()
-        # Attention: only consider extremities that are actually within the map
-        return [idx for idx, v in enumerate(self.all_vertices) if v.is_extremity and self.within_map(v.coordinates)]
-
-    @property
-    def extremity_mask(self) -> np.ndarray:
-        mask = np.full(self.nr_vertices, False, dtype=bool)
-        for i in self.extremity_indices:
-            mask[i] = True
-        return mask
-
-    @property
-    def all_extremities(self) -> List[PolygonVertex]:
-        return [self.all_vertices[i] for i in self.extremity_indices]
-        # TODO
-        # if self._all_extremities is None:
-        # return self._all_extremities
-
-    @property
-    def all_edges(self) -> List[Edge]:
-        return list(itertools.chain(*iter(p.edges for p in self.polygons)))
-
-    @property
-    def coords(self) -> np.ndarray:
-        return np.stack([v.coordinates for v in self.all_vertices])
-
-    @property
-    def vertex_edge_idxs(self) -> np.ndarray:
-        all_edges = self.all_edges
-        return np.stack([(all_edges.index(v.edge1), all_edges.index(v.edge2)) for v in self.all_vertices])
-
-    @property
-    def edge_vertex_idxs(self) -> np.ndarray:
-        vertices = self.all_vertices
-        return np.stack([(vertices.index(edge.vertex1), vertices.index(edge.vertex2)) for edge in self.all_edges])
+    def all_extremities(self) -> List[Tuple]:
+        coords = self.coords
+        return [tuple(coords[i]) for i in self.extremity_indices]
 
     def store(
         self,
@@ -154,10 +106,17 @@ def store(
         list_of_polygons = [boundary_coordinates] + list_of_hole_coordinates
         nr_total_pts = sum(map(len, list_of_polygons))
         vertex_edge_idxs = np.empty((nr_total_pts, 2), dtype=int)
+        # TODO required? inverse of the other
         edge_vertex_idxs = np.empty((nr_total_pts, 2), dtype=int)
         edge_idx = 0
         offset = 0
+        extremity_idxs = set()
         for poly in list_of_polygons:
+
+            poly_extr_idxs = compute_extremity_idxs(poly)
+            poly_extr_idxs = {i + offset for i in poly_extr_idxs}
+            extremity_idxs |= poly_extr_idxs
+
             nr_coords = len(poly)
             v1 = -1 % nr_coords
             # TODO col 1 is just np.arange?!
@@ -166,30 +125,42 @@ def store(
                 v2_idx = v2 + offset
                 edge_vertex_idxs[edge_idx, 0] = v1_idx
                 edge_vertex_idxs[edge_idx, 1] = v2_idx
-                vertex_edge_idxs[v1_idx, 0] = edge_idx
-                vertex_edge_idxs[v2_idx, 1] = edge_idx
+                vertex_edge_idxs[v1_idx, 1] = edge_idx
+                vertex_edge_idxs[v2_idx, 0] = edge_idx
                 # move to the next vertex/edge
                 v1 = v2
                 edge_idx += 1
 
             offset = edge_idx
 
-        assert edge_idx == nr_total_pts
+        # assert edge_idx == nr_total_pts
 
-        extremity_idxs = compute_extremity_idxs(self.coords)
+        coords = np.concatenate(list_of_polygons, axis=0)
         # Attention: only consider extremities that are actually within the map
-        extremity_idxs = [i for i in extremity_idxs if self.within_map(self.coords[i])]
+        extremity_idxs = [i for i in extremity_idxs if self.within_map(coords[i])]
 
         mask = np.full(nr_total_pts, False, dtype=bool)
-        mask[self.extremity_indices] = True
+        for i in extremity_idxs:
+            mask[i] = True
 
-        # coords = np.concatenate(list_of_polygons, axis=0)
+        self.nr_vertices = nr_total_pts
+        self.edge_vertex_idxs = edge_vertex_idxs
+        self.vertex_edge_idxs = vertex_edge_idxs
+        self.coords = coords
+        self.extremity_indices = extremity_idxs
+        self.extremity_mask = mask
 
-        # self.extremity_mask = mask
-        # self.edge_vertex_idxs = edge_vertex_idxs
-        # self.vertex_edge_idxs = vertex_edge_idxs
-        # self.coords = coords
-        # self.extremity_indices = extremity_idxs
+        # def get_representations(i:int)->np.ndarray:
+        #
+        # self.representations = {i: get_representations(i) for i in extremity_idxs}
+
+        def get_repr_n_dists(i: int) -> np.ndarray:
+            coords_orig = coords[i]
+            coords_translated = coords - coords_orig
+            repr_n_dists = np.apply_along_axis(compute_repr_n_dist, axis=1, arr=coords_translated)
+            return repr_n_dists.T
+
+        self.reprs_n_distances = {i: get_repr_n_dists(i) for i in extremity_idxs}
 
     def store_grid_world(
         self,
@@ -219,17 +190,6 @@ def export_pickle(self, path: str = DEFAULT_PICKLE_NAME):
             pickle.dump(self, f)
         print("done.\n")
 
-    def translate(self, new_origin: Vertex):
-        """shifts the coordinate system to a new origin
-
-        computing the angle representations, shifted coordinates and distances for all vertices
-        respective to the query point (lazy!)
-
-        :param new_origin: the origin of the coordinate system to be shifted to
-        """
-        for p in self.polygons:
-            p.translate(new_origin)
-
     def prepare(self):
         """Computes a visibility graph optimized (=reduced) for path planning and stores it
 
@@ -254,15 +214,14 @@ def prepare(self):
         if self.prepared:
             raise ValueError("this environment is already prepared. load new polygons first.")
 
-        extremities = self.all_extremities
-        nr_extremities = len(extremities)
+        nr_extremities = len(self.extremity_indices)
         if nr_extremities == 0:
             self.graph = DirectedHeuristicGraph()
             return
 
         # TODO pre computation and storage of all
         # TODO more performant way of computing
-        nr_vertices = self.nr_vertices
+        nr_edges = self.nr_edges
         extremity_indices = self.extremity_indices
         extremity_mask = self.extremity_mask
         coords = self.coords
@@ -272,9 +231,6 @@ def prepare(self):
         # TODO sparse matrix. problematic: default value is 0.0
         # angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
 
-        if len(extremity_indices) != len(extremities):
-            raise ValueError
-
         # TODO reuse
         def get_repr(idx_origin, i):
             coords_origin = coords[idx_origin]
@@ -297,7 +253,14 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
 
         extremity_coord_map = {i: coords[i] for i in extremity_indices}
         graph = DirectedHeuristicGraph(extremity_coord_map)
-        for origin_idx in extremity_indices:
+        for extr_ptr, origin_idx in enumerate(extremity_indices):
+            # extremities are always visible to each other
+            # (bi-directional relation -> undirected edges in the graph)
+            #  -> do not check extremities which have been checked already
+            #  (must give the same result when algorithms are correct)
+            # the origin extremity itself must also not be checked when looking for visible neighbours
+            candidate_idxs = extremity_indices[extr_ptr + 1 :]
+
             # vertices all belong to a polygon
             idx_n1, idx_n2 = get_neighbours(origin_idx)
             # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
@@ -310,28 +273,22 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             n1_repr = get_repr(origin_idx, idx_n1)
             n2_repr = get_repr(origin_idx, idx_n2)
 
-            # TODO lazy init? same as angle repr
-            vert_idx2dist = {i: get_distance_to_origin(origin_idx, i) for i in range(nr_vertices)}
-            vert_idx2repr = {i: get_repr(origin_idx, i) for i in range(nr_vertices)}
-            idx2repr = {i: vert_idx2repr[i] for i in extremity_indices}
+            vert_idx2repr, vert_idx2dist = self.reprs_n_distances[origin_idx]
+
             # only consider extremities with coords_rel different from the query extremity
             # (angle representation not None)
-            # the origin extremity itself must also not be checked when looking for visible neighbours
-            # extremities are always visible to each other
-            # (bi-directional relation -> undirected edges in the graph)
-            #  -> do not check extremities which have been checked already
-            #  (must give the same result when algorithms are correct)
-            idx2repr = {i: r for i, r in idx2repr.items() if r is not None and i > origin_idx}
-            idxs_behind = find_within_range(
+
+            candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not np.nan}
+            idxs_behind = find_within_range2(
                 n1_repr,
                 n2_repr,
-                idx2repr,
+                vert_idx2repr,
+                candidate_idxs,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
             # do not consider points found to lie behind
-            for i in idxs_behind:
-                idx2repr.pop(i)
+            candidate_idxs.difference_update(idxs_behind)
 
             # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
             # such that both adjacent edges are visible, one will never visit e, because everything is
@@ -351,22 +308,19 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            idxs_in_front = find_within_range(
+            idxs_in_front = find_within_range2(
                 n1_repr_inv,
                 n2_repr_inv,
-                idx2repr,
+                vert_idx2repr,
+                candidate_idxs,
                 angle_range_less_180=True,
                 equal_repr_allowed=False,
             )
             # do not consider points lying in front when looking for visible extremities,
             # even if they are actually be visible
-            for i in idxs_in_front:
-                idx2repr.pop(i)
-
-            candidate_idxs = set(idx2repr.keys())
+            candidate_idxs.difference_update(idxs_in_front)
 
             # all edges have to be checked, except the 2 neighbouring edges (handled above!)
-            nr_edges = len(self.all_edges)
             edge_idxs2check = set(range(nr_edges))
             edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
             edge_idxs2check.remove(edge1_idx)
@@ -395,13 +349,12 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
         self.graph = graph
         self.prepared = True
 
-    def within_map(self, coords: INPUT_COORD_TYPE):
+    def within_map(self, coords: InputCoords):
         """checks if the given coordinates lie within the boundary polygon and outside of all holes
 
         :param coords: numerical tuple representing coordinates
         :return: whether the given coordinate is a valid query point
         """
-
         x, y = coords
         if not inside_polygon(x, y, self.boundary_polygon.coordinates, border_value=True):
             return False
@@ -412,8 +365,8 @@ def within_map(self, coords: INPUT_COORD_TYPE):
 
     def find_shortest_path(
         self,
-        start_coordinates: INPUT_COORD_TYPE,
-        goal_coordinates: INPUT_COORD_TYPE,
+        start_coordinates: InputCoords,
+        goal_coordinates: InputCoords,
         free_space_after: bool = True,
         verify: bool = True,
     ) -> Tuple[PATH_TYPE, LENGTH_TYPE]:
@@ -455,10 +408,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             # start and goal are identical and can be reached instantly
             return [start_coordinates, goal_coordinates], 0.0
 
-        # TODO pre computation and storage of all
-        # TODO more performant way of computing
-        nr_edges = len(self.all_edges)
-
+        nr_edges = self.nr_edges
         vertex_edge_idxs = self.vertex_edge_idxs
         edge_vertex_idxs = self.edge_vertex_idxs
         # temporarily extend data structures
@@ -473,7 +423,8 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # start and goal nodes could be identical with one ore more of the vertices
         # BUT: this is an edge case -> compute visibility as usual and later try to merge with the graph
 
-        # create temporary graph TODO make more performant, avoid real copy
+        # create temporary graph
+        # TODO make more performant, avoid real copy
         # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
         # but to still not create real copies of vertex instances!
         graph = deepcopy(self.graph)
@@ -550,59 +501,9 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
         graph.add_multiple_directed_edges(idx_start, visibles_n_distances_map)
 
-        # # TODO optimisation
-        # # also here unnecessary edges in the graph can be deleted when start or goal lie
-        # in front of visible extremities
-        # # IMPORTANT: when a query point happens to coincide with an extremity, edges to the (visible) extremities
-        # #  in front MUST be added to the graph! Handled by always introducing new
-        # (non extremity, non polygon) vertices.
-        # # for every extremity that is visible from either goal or start
-        # # NOTE: edges are undirected! graph.get_neighbours_of(start_vertex) == set()
-        # # neighbours_start = graph.get_neighbours_of(start_vertex)
-        # neighbours_start = {n for n, d in visibles_n_distances_start}
-        # # the goal vertex might be marked visible, it is not an extremity -> skip
-        # neighbours_start.discard(goal_vertex)
-        # neighbour_idxs_goal = graph.get_neighbours_of(idx_goal)
-        # neighbours_goal = {vertices[i] for i in neighbour_idxs_goal}
-        # neighbours = neighbours_start | neighbours_goal
-        # for vertex in neighbours:
-        #     # assert type(vertex) == PolygonVertex and vertex.is_extremity
-        #
-        #     # vertex = vertices[vertex_idx]
-        #     vertex_idx = vertices.index(vertex)
-        #
-        #     # check only if point is visible
-        #     temp_candidates = set()
-        #     if vertex in neighbours_start:
-        #         temp_candidates.add(start_vertex)
-        #
-        #     if vertex in neighbours_goal:
-        #         temp_candidates.add(goal_vertex)
-        #
-        #     if len(temp_candidates) == 0:
-        #         continue
-        #     self.translate(new_origin=vertex)
-        #     # IMPORTANT: manually translate the goal and start vertices
-        #     start_vertex.mark_outdated()
-        #     goal_vertex.mark_outdated()
-        #
-        #     n1, n2 = vertex.get_neighbours()
-        #     repr1 = angle_rep_inverse(n1.get_angle_representation())  # rotated 180 deg
-        #     repr2 = angle_rep_inverse(n2.get_angle_representation())
-        #
-        #     # IMPORTANT: special case:
-        #     # here the nodes must stay connected if they have the same angle representation!
-        #     lie_in_front = find_within_range(
-        #         repr1,
-        #         repr2,
-        #         temp_candidates,
-        #         angle_range_less_180=True,
-        #         equal_repr_allowed=False,
-        #     )
-        #     lie_in_front_idxs = iter(vertices.index(v) for v in lie_in_front)
-        #     graph.remove_multiple_undirected_edges(vertex_idx, lie_in_front_idxs)
-
-        # NOTE: exploiting property 2 from [1] here would be more expensive than beneficial
+        # Note: also here unnecessary edges in the graph could be deleted when start or goal lie
+        # optimising the graph here however is more expensive than beneficial,
+        # as it is only being used for a single query
 
         # ATTENTION: update to new coordinates
         graph.coord_map = {i: coords[i] for i in graph.all_nodes}
diff --git a/extremitypathfinder/global_settings.py b/extremitypathfinder/global_settings.py
index 4bbe47b..9382d26 100644
--- a/extremitypathfinder/global_settings.py
+++ b/extremitypathfinder/global_settings.py
@@ -6,8 +6,8 @@
 PATH_TYPE = List[COORDINATE_TYPE]
 LENGTH_TYPE = Optional[float]
 INPUT_NUMERICAL_TYPE = Union[float, int]
-INPUT_COORD_TYPE = Tuple[INPUT_NUMERICAL_TYPE, INPUT_NUMERICAL_TYPE]
-OBSTACLE_ITER_TYPE = Iterable[INPUT_COORD_TYPE]
+InputCoords = Tuple[INPUT_NUMERICAL_TYPE, INPUT_NUMERICAL_TYPE]
+OBSTACLE_ITER_TYPE = Iterable[InputCoords]
 INPUT_COORD_LIST_TYPE = Union[np.ndarray, List]
 DEFAULT_PICKLE_NAME = "environment.pickle"
 
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 4984a67..fe85652 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -10,6 +10,60 @@
 origin = None
 
 
+def compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]:
+    """computing representation for the angle from the origin to a given vector
+
+    value in [0.0 : 4.0[
+    every quadrant contains angle measures from 0.0 to 1.0
+    there are 4 quadrants (counter clockwise numbering)
+    0 / 360 degree -> 0.0
+    90 degree -> 1.0
+    180 degree -> 2.0
+    270 degree -> 3.0
+    ...
+    Useful for comparing angles without actually computing expensive trigonometrical functions
+    This representation does not grow directly proportional to its represented angle,
+    but it its bijective and monotonous:
+    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
+    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
+    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
+    with (0,0)' being the vector representing the origin
+
+    :param np_vector:
+    :return:
+    """
+    distance = np.linalg.norm(np_vector, ord=2)
+    if distance == 0.0:
+        angle_measure = np.nan
+    else:
+        # 2D vector: (dx, dy) = np_vector
+        dx, dy = np_vector
+        dx_positive = dx >= 0
+        dy_positive = dy >= 0
+
+        if dx_positive and dy_positive:
+            quadrant = 0.0
+            angle_measure = dy
+
+        elif not dx_positive and dy_positive:
+            quadrant = 1.0
+            angle_measure = -dx
+
+        elif not dx_positive and not dy_positive:
+            quadrant = 2.0
+            angle_measure = -dy
+
+        else:
+            quadrant = 3.0
+            angle_measure = dx
+
+        # normalise angle measure to [0; 1]
+        angle_measure /= distance
+        angle_measure += quadrant
+
+    return angle_measure, distance
+
+
 def compute_angle_repr_inner(np_vector: np.ndarray) -> float:
     """computing representation for the angle from the origin to a given vector
 
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 1154325..5012fb6 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -239,6 +239,69 @@ def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
     return repr
 
 
+def find_within_range2(
+    repr1: float,
+    repr2: float,
+    vert_idx2repr: np.ndarray,
+    candidate_idxs: Set[int],
+    angle_range_less_180: bool,
+    equal_repr_allowed: bool,
+) -> Set[int]:
+    """
+    filters out all vertices whose representation lies within the range between
+      the two given angle representations
+    which range ('clockwise' or 'counter-clockwise') should be checked is determined by:
+      - query angle (range) is < 180deg or not (>= 180deg)
+    :param repr1:
+    :param repr2:
+    :param representations:
+    :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg
+    :param equal_repr_allowed: whether vertices with the same representation should also be returned
+    :return:
+    """
+    if len(candidate_idxs) == 0:
+        return set()
+
+    repr_diff = abs(repr1 - repr2)
+    if repr_diff == 0.0:
+        return set()
+
+    min_repr = min(repr1, repr2)
+    max_repr = max(repr1, repr2)  # = min_angle + angle_diff
+
+    def repr_within(r):
+        # Note: vertices with the same representation will not NOT be returned!
+        return min_repr < r < max_repr
+
+    # depending on the angle the included range is clockwise or anti-clockwise
+    # (from min_repr to max_val or the other way around)
+    # when the range contains the 0.0 value (transition from 3.99... -> 0.0)
+    # it is easier to check if a representation does NOT lie within this range
+    # -> invert filter condition
+    # special case: angle == 180deg
+    on_line_inv = repr_diff == 2.0 and repr1 >= repr2
+    # which range to filter is determined by the order of the points
+    # since the polygons follow a numbering convention,
+    # the 'left' side of p1-p2 always lies inside the map
+    # -> filter out everything on the right side (='outside')
+    inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180)
+
+    def within_filter_func(r: float) -> bool:
+        repr_eq = r == min_repr or r == max_repr
+        if repr_eq and equal_repr_allowed:
+            return True
+        if repr_eq and not equal_repr_allowed:
+            return False
+
+        res = repr_within(r)
+        if inversion_condition:
+            res = not res
+        return res
+
+    idxs_within = {i for i in candidate_idxs if within_filter_func(vert_idx2repr[i])}
+    return idxs_within
+
+
 def find_within_range(
     repr1: float,
     repr2: float,

From 295a9b28300ca05dfd7f41f7fafd9c5bc5b9e89d Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 14:50:15 +0200
Subject: [PATCH 30/44] refactor find_within_range

---
 extremitypathfinder/extremitypathfinder.py |  41 +-
 extremitypathfinder/helper_fcts.py         | 539 ++++++++++-----------
 2 files changed, 261 insertions(+), 319 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index e115854..06e216d 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -18,14 +18,14 @@
     PolygonVertex,
     angle_rep_inverse,
     compute_extremity_idxs,
-    compute_repr_n_dist,
 )
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
     convert_gridworld,
     find_visible2,
-    find_within_range2,
+    find_within_range,
     get_angle_repr,
+    get_repr_n_dists,
     inside_polygon,
 )
 
@@ -150,17 +150,7 @@ def store(
         self.extremity_indices = extremity_idxs
         self.extremity_mask = mask
 
-        # def get_representations(i:int)->np.ndarray:
-        #
-        # self.representations = {i: get_representations(i) for i in extremity_idxs}
-
-        def get_repr_n_dists(i: int) -> np.ndarray:
-            coords_orig = coords[i]
-            coords_translated = coords - coords_orig
-            repr_n_dists = np.apply_along_axis(compute_repr_n_dist, axis=1, arr=coords_translated)
-            return repr_n_dists.T
-
-        self.reprs_n_distances = {i: get_repr_n_dists(i) for i in extremity_idxs}
+        self.reprs_n_distances = {i: get_repr_n_dists(i, coords) for i in extremity_idxs}
 
     def store_grid_world(
         self,
@@ -279,7 +269,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # (angle representation not None)
 
             candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not np.nan}
-            idxs_behind = find_within_range2(
+            idxs_behind = find_within_range(
                 n1_repr,
                 n2_repr,
                 vert_idx2repr,
@@ -308,7 +298,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
             # IMPORTANT: check all extremities here, not just current candidates
             # do not check extremities with equal coords_rel (also query extremity itself!)
             #   and with the same angle representation (those edges must not get deleted from graph!)
-            idxs_in_front = find_within_range2(
+            idxs_in_front = find_within_range(
                 n1_repr_inv,
                 n2_repr_inv,
                 vert_idx2repr,
@@ -336,6 +326,7 @@ def get_distance_to_origin(orig_idx: int, i: int) -> float:
                 vert_idx2repr,
                 vert_idx2dist,
                 candidate_idxs,
+                origin_idx,
             )
 
             visible_vertex2dist_map = {i: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
@@ -416,9 +407,6 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         coords = np.append(self.coords, (coords_start, coords_goal), axis=0)
         idx_start = self.nr_vertices
         idx_goal = self.nr_vertices + 1
-        nr_vertices = self.nr_vertices + 2
-        vert_idx2dist_start = {i: get_distance_to_origin(coords_start, i) for i in range(nr_vertices)}
-        vert_idx2dist_goal = {i: get_distance_to_origin(coords_goal, i) for i in range(nr_vertices)}
 
         # start and goal nodes could be identical with one ore more of the vertices
         # BUT: this is an edge case -> compute visibility as usual and later try to merge with the graph
@@ -435,15 +423,14 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # the visibility of only the graphs nodes has to be checked (not all extremities!)
         # points with the same angle representation should not be considered visible
         # (they also cause errors in the algorithms, because their angle repr is not defined!)
-        candidate_idxs = self.graph.get_all_nodes()
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
-        candidate_idxs.add(idx_start)
+        self.graph.get_all_nodes().add(idx_start)
         coords_origin = coords[idx_origin]
-        vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
-        candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
         edge_idxs2check = set(range(nr_edges))
-        vert_idx2dist = vert_idx2dist_goal
+
+        vert_idx2repr, vert_idx2dist = get_repr_n_dists(idx_origin, coords)
+        candidate_idxs = {i for i in (self.graph.get_all_nodes()) if not vert_idx2dist[i] == 0.0}
         visible_idxs = find_visible2(
             extremity_mask,
             coords,
@@ -454,6 +441,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             vert_idx2repr,
             vert_idx2dist,
             candidate_idxs,
+            idx_origin,
         )
         visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
 
@@ -476,11 +464,9 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         coords_origin = coords[idx_origin]
         # the visibility of only the graphs nodes have to be checked
         # the goal node does not have to be considered, because of the earlier check
-        candidate_idxs = self.graph.get_all_nodes()
-        vert_idx2repr = {i: get_angle_repr(coords_origin, coords[i]) for i in range(nr_vertices)}
-        candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not None}
         edge_idxs2check = set(range(nr_edges))  # new copy
-        vert_idx2dist = vert_idx2dist_start
+        vert_idx2repr, vert_idx2dist = get_repr_n_dists(idx_origin, coords)
+        candidate_idxs = {i for i in self.graph.get_all_nodes() if not vert_idx2dist[i] == 0.0}
         visible_idxs = find_visible2(
             extremity_mask,
             coords,
@@ -491,6 +477,7 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             vert_idx2repr,
             vert_idx2dist,
             candidate_idxs,
+            idx_origin,
         )
 
         if len(visible_idxs) == 0:
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 5012fb6..21bcd36 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,13 +1,20 @@
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
 from itertools import combinations
-from typing import Dict, List, Set, Tuple
+from typing import List, Set, Tuple
 
 import numpy as np
 import numpy.linalg
 
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import compute_angle_repr, compute_angle_repr_inner
+from extremitypathfinder.helper_classes import compute_angle_repr, compute_angle_repr_inner, compute_repr_n_dist
+
+
+def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
+    coords_orig = coords[orig_idx]
+    coords_translated = coords - coords_orig
+    repr_n_dists = np.apply_along_axis(compute_repr_n_dist, axis=1, arr=coords_translated)
+    return repr_n_dists.T
 
 
 def inside_polygon(x, y, coords, border_value):
@@ -107,7 +114,7 @@ def get_intersection_status(p1, p2, q1, q2):
 
 
 # special case of has_intersection()
-def lies_behind(p1, p2, v):
+def lies_behind_inner(p1: np.ndarray, p2: np.ndarray, v: np.ndarray) -> bool:
     # solve the set of equations
     # (p2-p1) lambda + (p1) = (v) mu
     #  in matrix form A x = b:
@@ -131,6 +138,14 @@ def lies_behind(p1, p2, v):
     return x[1] < 1.0
 
 
+def lies_behind(idx_p1: int, idx_p2: int, idx_v: int, idx_orig: int, coords: np.ndarray) -> bool:
+    coords_origin = coords[idx_orig]
+    coords_p1_rel = coords[idx_p1] - coords_origin
+    coords_p2_rel = coords[idx_p2] - coords_origin
+    coords_v_rel = coords[idx_v] - coords_origin
+    return lies_behind_inner(coords_p1_rel, coords_p2_rel, coords_v_rel)
+
+
 def no_self_intersection(coords):
     polygon_length = len(coords)
     # again_check = []
@@ -239,7 +254,7 @@ def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
     return repr
 
 
-def find_within_range2(
+def find_within_range(
     repr1: float,
     repr2: float,
     vert_idx2repr: np.ndarray,
@@ -302,222 +317,29 @@ def within_filter_func(r: float) -> bool:
     return idxs_within
 
 
-def find_within_range(
-    repr1: float,
-    repr2: float,
-    idx2repr: Dict[int, float],
-    angle_range_less_180: bool,
-    equal_repr_allowed: bool,
-) -> Set[int]:
-    """
-    filters out all vertices whose representation lies within the range between
-      the two given angle representations
-    which range ('clockwise' or 'counter-clockwise') should be checked is determined by:
-      - query angle (range) is < 180deg or not (>= 180deg)
-    :param repr1:
-    :param repr2:
-    :param representations:
-    :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg
-    :param equal_repr_allowed: whether vertices with the same representation should also be returned
-    :return:
-    """
-    if len(idx2repr) == 0:
-        return set()
-
-    repr_diff = abs(repr1 - repr2)
-    if repr_diff == 0.0:
-        return set()
-
-    min_repr = min(repr1, repr2)
-    max_repr = max(repr1, repr2)  # = min_angle + angle_diff
-
-    def repr_within(r):
-        # Note: vertices with the same representation will not NOT be returned!
-        return min_repr < r < max_repr
-
-    # depending on the angle the included range is clockwise or anti-clockwise
-    # (from min_repr to max_val or the other way around)
-    # when the range contains the 0.0 value (transition from 3.99... -> 0.0)
-    # it is easier to check if a representation does NOT lie within this range
-    # -> invert filter condition
-    # special case: angle == 180deg
-    on_line_inv = repr_diff == 2.0 and repr1 >= repr2
-    # which range to filter is determined by the order of the points
-    # since the polygons follow a numbering convention,
-    # the 'left' side of p1-p2 always lies inside the map
-    # -> filter out everything on the right side (='outside')
-    inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180)
-
-    def within_filter_func(r: float) -> bool:
-        repr_eq = r == min_repr or r == max_repr
-        if repr_eq and equal_repr_allowed:
-            return True
-        if repr_eq and not equal_repr_allowed:
-            return False
-
-        res = repr_within(r)
-        if inversion_condition:
-            res = not res
-        return res
-
-    idxs_within = {i for i, r in idx2repr.items() if within_filter_func(r)}
-    return idxs_within
-
-
-def convert_gridworld(size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True) -> (list, list):
-    """
-    prerequisites: grid world must not have non-obstacle cells which are surrounded by obstacles
-    ("single white cell in black surrounding" = useless for path planning)
-    :param size_x: the horizontal grid world size
-    :param size_y: the vertical grid world size
-    :param obstacle_iter: an iterable of coordinate pairs (x,y) representing blocked grid cells (obstacles)
-    :param simplify: whether the polygons should be simplified or not. reduces edge amount, allow diagonal edges
-    :return: an boundary polygon (counter clockwise numbering) and a list of hole polygons (clockwise numbering)
-    NOTE: convert grid world into polygons in a way that coordinates coincide with grid!
-        -> no conversion of obtained graphs needed!
-        the origin of the polygon coordinate system is (-0.5,-0.5) in the grid cell system (= corners of the grid world)
-    """
-
-    assert size_x > 0 and size_y > 0
-
-    if len(obstacle_iter) == 0:
-        # there are no obstacles. return just the simple boundary rectangle
-        return [np.array(x, y) for x, y in [(0, 0), (size_x, 0), (size_x, size_y), (0, size_y)]], []
-
-    # convert (x,y) into np.arrays
-    # obstacle_iter = [np.array(o) for o in obstacle_iter]
-    obstacle_iter = np.array(obstacle_iter)
-
-    def within_grid(pos):
-        return 0 <= pos[0] < size_x and 0 <= pos[1] < size_y
-
-    def is_equal(pos1, pos2):
-        return np.all(pos1 == pos2)
-
-    def pos_in_iter(pos, iter):
-        for i in iter:
-            if is_equal(pos, i):
-                return True
-        return False
-
-    def is_obstacle(pos):
-        return pos_in_iter(pos, obstacle_iter)
-
-    def is_blocked(pos):
-        return not within_grid(pos) or is_obstacle(pos)
-
-    def is_unblocked(pos):
-        return within_grid(pos) and not is_obstacle(pos)
-
-    def find_start(start_pos, boundary_detect_fct, **kwargs):
-        # returns the lowest and leftmost unblocked grid cell from the start position
-        # for which the detection function evaluates to True
-        start_x, start_y = start_pos
-        for y in range(start_y, size_y):
-            for x in range(start_x, size_x):
-                pos = np.array([x, y])
-                if boundary_detect_fct(pos, **kwargs):
-                    return pos
-
-    # north, east, south, west
-    directions = np.array([[0, 1], [1, 0], [0, -1], [-1, 0]], dtype=int)
-    # the correct offset to determine where nodes should be added.
-    offsets = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=int)
-
-    def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: bool):
-        current_pos = start_pos.copy()
-        # (at least) the west and south are blocked
-        #   -> there has to be a polygon node at the current position (bottom left corner of the cell)
-        edge_list = [start_pos]
-        forward_index = 0  # start with moving north
-        forward_vect = directions[forward_index]
-        left_index = (forward_index - 1) % 4
-        # left_vect = directions[(forward_index - 1) % 4]
-        just_turned = True
-
-        # follow the border between obstacles and free cells ("wall") until one
-        # reaches the start position again
-        while True:
-            # left has to be checked first
-            # do not check if just turned left or right (-> the left is blocked for sure)
-            # left_pos = current_pos + left_vect
-            if not (just_turned or boundary_detect_fct(current_pos + directions[left_index])):
-                # print('< turn left')
-                forward_index = left_index
-                left_index = (forward_index - 1) % 4
-                forward_vect = directions[forward_index]
-                just_turned = True
-
-                # add a new node at the correct position
-                # decrease the index first!
-                edge_list.append(current_pos + offsets[forward_index])
-
-                # move forward (previously left, there is no obstacle)
-                current_pos += forward_vect
-            else:
-                forward_pos = current_pos + forward_vect
-                if boundary_detect_fct(forward_pos):
-                    node_pos = current_pos + offsets[forward_index]
-                    # there is a node at the bottom left corner of the start position (offset= (0,0) )
-                    if is_equal(node_pos, start_pos):
-                        # check and terminate if this node does already exist
-                        break
-
-                    # add a new node at the correct position
-                    edge_list.append(node_pos)
-                    # print('> turn right')
-                    left_index = forward_index
-                    forward_index = (forward_index + 1) % 4
-                    forward_vect = directions[forward_index]
-                    just_turned = True
-                    # print(direction_index,forward_vect,just_turned,edge_list,)
-                else:
-                    # print('^ move forward')
-                    current_pos += forward_vect
-                    just_turned = False
-
-        if cntr_clockwise_wanted:
-            # make edge numbering counter clockwise!
-            edge_list.reverse()
-        return np.array(edge_list)
-
-    # build the boundary polygon
-    # start at the lowest and leftmost unblocked grid cell
-    start_pos = find_start(start_pos=(0, 0), boundary_detect_fct=is_unblocked)
-    # print(start_pos+directions[3])
-    # raise ValueError
-    boundary_edges = construct_polygon(start_pos, boundary_detect_fct=is_blocked, cntr_clockwise_wanted=True)
-
-    if simplify:
-        # TODO
-        raise NotImplementedError()
-
-    # detect which of the obstacles have to be converted into holes
-    # just the obstacles inside the boundary polygon are part of holes
-    # shift coordinates by +(0.5,0.5) for correct detection
-    # the border value does not matter here
-    unchecked_obstacles = [
-        o for o in obstacle_iter if inside_polygon(o[0] + 0.5, o[1] + 0.5, boundary_edges, border_value=True)
-    ]
-
-    hole_list = []
-    while len(unchecked_obstacles) > 0:
-        start_pos = find_start(start_pos=(0, 0), boundary_detect_fct=pos_in_iter, iter=unchecked_obstacles)
-        hole = construct_polygon(start_pos, boundary_detect_fct=is_unblocked, cntr_clockwise_wanted=False)
-
-        # detect which of the obstacles still do not belong to any hole:
-        # delete the obstacles which are included in the just constructed hole
-        unchecked_obstacles = [
-            o for o in unchecked_obstacles if not inside_polygon(o[0] + 0.5, o[1] + 0.5, hole, border_value=True)
-        ]
-
-        if simplify:
-            # TODO
-            pass
+def get_neighbours(i: int, vertex_edge_idxs: np.ndarray, edge_vertex_idxs: np.ndarray) -> Tuple[int, int]:
+    edge_idx1, edge_idx2 = vertex_edge_idxs[i]
+    neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
+    neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
+    return neigh_idx1, neigh_idx2
 
-        hole_list.append(hole)
 
-    return boundary_edges, hole_list
+def skip_edge(cand_idxs, edge_vertex_idxs, extremity_mask, vert_idx2dist, vertex_edge_idxs, node_idx, edge2discard):
+    # (note: not identical, does not belong to the same polygon!)
+    # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
+    cand_idxs.discard(node_idx)
+    # no points lie truly "behind" this edge as there is no "direction of sight" defined
+    # <-> angle representation/range undefined for just this single edge
+    # however if one considers the point neighbouring in the other direction (<-> two edges)
+    # these two neighbouring edges define an invisible angle range
+    # -> simply move the pointer
+    v1, v2 = get_neighbours(node_idx, vertex_edge_idxs, edge_vertex_idxs)
+    dist_v1 = vert_idx2dist[node_idx]
+    dist_v2 = vert_idx2dist[node_idx]
+    range_less_180 = extremity_mask[node_idx]
+    # do not check the other neighbouring edge of vertex1 in the future (has been considered already)
+    edge_idx = vertex_edge_idxs[node_idx][edge2discard]
+    return dist_v1, dist_v2, edge_idx, range_less_180, v1, v2
 
 
 def find_visible2(
@@ -527,9 +349,10 @@ def find_visible2(
     edge_vertex_idxs: np.ndarray,
     edge_idxs2check: Set[int],
     coords_origin: np.ndarray,
-    vert_idx2repr: Dict[int, float],
-    vert_idx2dist: Dict[int, float],
+    vert_idx2repr: np.ndarray,
+    vert_idx2dist: np.ndarray,
     cand_idxs: Set[int],
+    idx_origin: int,
 ) -> Set[int]:
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
@@ -541,79 +364,54 @@ def find_visible2(
     :param edges_to_check: the set of edges which determine visibility
     :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
     """
-    # cand_idx2repr = cand_idx2repr_full.copy()
-
     if len(cand_idxs) == 0:
         return cand_idxs
 
-    # TODO reuse
-    def get_coordinates_translated(i: int) -> np.ndarray:
-        coords_v = coords[i]
-        return coords_v - coords_origin
-
-    def get_distance_to_origin(i: int) -> float:
-        return vert_idx2dist[i]
-
-    def get_repr(i: int) -> float:
-        return vert_idx2repr[i]
-
-    def get_neighbours(i: int) -> Tuple[int, int]:
-        edge_idx1, edge_idx2 = vertex_edge_idxs[i]
-        neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
-        neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
-        return neigh_idx1, neigh_idx2
-
-    def is_extremity(i: int) -> bool:
-        return extremity_mask[i]
-
     visible_idxs = set()
     # goal: eliminating all vertices lying 'behind' any edge
-    while len(edge_idxs2check) > 0 and len(cand_idxs) > 0:
-        edge_idx = edge_idxs2check.pop()
-        idx_v1, idx_v2 = edge_vertex_idxs[edge_idx]
+    while len(cand_idxs) > 0:
+        try:
+            edge_idx = edge_idxs2check.pop()
+        except KeyError:
+            break  # no more edges left to check
+
+        v1, v2 = edge_vertex_idxs[edge_idx]
 
-        v1_dist = get_distance_to_origin(idx_v1)
-        v2_dist = get_distance_to_origin(idx_v2)
+        dist_v1 = vert_idx2dist[v1]
+        dist_v2 = vert_idx2dist[v2]
         lies_on_edge = False
         range_less_180 = False
 
-        if v1_dist == 0.0:
+        if dist_v1 == 0.0:
             # vertex1 of the edge has the same coordinates as the query vertex
             # -> the origin lies on the edge
             lies_on_edge = True
-            # (note: not identical, does not belong to the same polygon!)
-            # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
-            cand_idxs.discard(idx_v1)
-
-            # no points lie truly "behind" this edge as there is no "direction of sight" defined
-            # <-> angle representation/range undefined for just this single edge
-            # however if one considers the point neighbouring in the other direction (<-> two edges)
-            # these two neighbouring edges define an invisible angle range
-            # -> simply move the pointer
-            idx_v1, idx_v2 = get_neighbours(idx_v1)
-            v1_dist = get_distance_to_origin(idx_v1)
-            v2_dist = get_distance_to_origin(idx_v2)
-            range_less_180 = is_extremity(idx_v1)
-
-            # do not check the other neighbouring edge of vertex1 in the future (has been considered already)
-            edge_idx1, _ = vertex_edge_idxs[idx_v1]
-            edge_idxs2check.discard(edge_idx1)
-
-        elif v2_dist == 0.0:
+            dist_v1, dist_v2, edge_idx, range_less_180, v1, v2 = skip_edge(
+                cand_idxs,
+                edge_vertex_idxs,
+                extremity_mask,
+                vert_idx2dist,
+                vertex_edge_idxs,
+                node_idx=v1,
+                edge2discard=0,
+            )
+            edge_idxs2check.discard(edge_idx)
+
+        elif dist_v2 == 0.0:
             # same for vertex2 of the edge
             # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex
             # (edge with length 0)
             lies_on_edge = True
-            cand_idxs.discard(idx_v2)
-            idx_v1, idx_v2 = get_neighbours(idx_v2)
-            v1_dist = get_distance_to_origin(idx_v1)
-            v2_dist = get_distance_to_origin(idx_v2)
-            range_less_180 = is_extremity(idx_v2)
-            _, edge_idx2 = vertex_edge_idxs[idx_v2]
+            cand_idxs.discard(v2)
+            v1, v2 = get_neighbours(v2, vertex_edge_idxs, edge_vertex_idxs)
+            dist_v1 = vert_idx2dist[v1]
+            dist_v2 = vert_idx2dist[v2]
+            range_less_180 = extremity_mask[v2]
+            _, edge_idx2 = vertex_edge_idxs[v2]
             edge_idxs2check.discard(edge_idx2)
 
-        repr1 = get_repr(idx_v1)
-        repr2 = get_repr(idx_v2)
+        repr1 = vert_idx2repr[v1]
+        repr2 = vert_idx2repr[v2]
         if repr2 is None or repr1 is None:
             raise ValueError
         repr_diff = abs(repr1 - repr2)
@@ -627,26 +425,30 @@ def is_extremity(i: int) -> bool:
             # the neighbouring edges are visible for sure
             # attention: only add to visible set if vertex was a candidate!
             try:
-                cand_idxs.remove(idx_v1)
-                visible_idxs.add(idx_v1)
+                cand_idxs.remove(v1)
+                visible_idxs.add(v1)
             except KeyError:
                 pass
             try:
-                cand_idxs.remove(idx_v2)
-                visible_idxs.add(idx_v2)
+                cand_idxs.remove(v2)
+                visible_idxs.add(v2)
             except KeyError:
                 pass
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation must not be deleted, because they can be visible!
             cand_idx2repr = {i: vert_idx2repr[i] for i in cand_idxs}
+
+            candidate_idxs = set(cand_idx2repr.keys())
             invisible_candidate_idxs = find_within_range(
                 repr1,
                 repr2,
-                cand_idx2repr,
+                vert_idx2repr,
+                candidate_idxs,
                 angle_range_less_180=range_less_180,
                 equal_repr_allowed=False,
             )
+            candidate_idxs.difference_update(invisible_candidate_idxs)
             for i in invisible_candidate_idxs:
                 cand_idx2repr.pop(i, None)
             continue
@@ -657,8 +459,8 @@ def is_extremity(i: int) -> bool:
         cand_idxs_tmp = cand_idxs.copy()
         # the vertices belonging to the edge itself (its vertices) must not be checked.
         # use discard() instead of remove() to not raise an error (they might not be candidates)
-        cand_idxs_tmp.discard(idx_v1)
-        cand_idxs_tmp.discard(idx_v2)
+        cand_idxs_tmp.discard(v1)
+        cand_idxs_tmp.discard(v2)
 
         # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge)
         #   within this angle range
@@ -666,28 +468,27 @@ def is_extremity(i: int) -> bool:
         #   is always < 180deg when the edge is not running through the query point (=180 deg)
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
-        idx2repr = {i: vert_idx2repr[i] for i in cand_idxs_tmp}
         idxs2check = find_within_range(
             repr1,
             repr2,
-            idx2repr,
+            vert_idx2repr,
+            cand_idxs_tmp,
             angle_range_less_180=True,
             equal_repr_allowed=True,
         )
 
-        max_distance = max(v1_dist, v2_dist)
+        max_distance = max(dist_v1, dist_v2)
         idxs_behind = set()
         # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
         #    has an intersection with the current edge p1---p2
-        p1 = get_coordinates_translated(idx_v1)
-        p2 = get_coordinates_translated(idx_v2)
         for idx in idxs2check:
             # if a candidate is farther away from the query point than both vertices of the edge,
             #   it surely lies behind the edge
             # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
             #   it still needs to be checked!
-            further_away = get_distance_to_origin(idx) > max_distance
-            if further_away or lies_behind(p1, p2, get_coordinates_translated(idx)):
+            dist2orig = vert_idx2dist[idx]
+            further_away = dist2orig > max_distance
+            if further_away or lies_behind(v1, v2, idx, idx_origin, coords):
                 idxs_behind.add(idx)
             # vertex lies in front of this edge
 
@@ -700,9 +501,7 @@ def is_extremity(i: int) -> bool:
     return clean_visible_idxs(visible_idxs, vert_idx2repr, vert_idx2dist)
 
 
-def clean_visible_idxs(
-    visible_idxs: Set[int], cand_idx2repr: Dict[int, float], vert_idx2dist: Dict[int, float]
-) -> Set[int]:
+def clean_visible_idxs(visible_idxs: Set[int], cand_idx2repr: np.ndarray, vert_idx2dist: np.ndarray) -> Set[int]:
     # in case some vertices have the same representation, only return (link) the closest vertex
     if len(visible_idxs) <= 1:
         return visible_idxs
@@ -758,3 +557,159 @@ def read_json(path2json_file):
     boundary_coordinates = convert2polygon(boundary_data)
     list_of_holes = [convert2polygon(hole_data) for hole_data in holes_data]
     return boundary_coordinates, list_of_holes
+
+
+def convert_gridworld(size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True) -> (list, list):
+    """
+    prerequisites: grid world must not have non-obstacle cells which are surrounded by obstacles
+    ("single white cell in black surrounding" = useless for path planning)
+    :param size_x: the horizontal grid world size
+    :param size_y: the vertical grid world size
+    :param obstacle_iter: an iterable of coordinate pairs (x,y) representing blocked grid cells (obstacles)
+    :param simplify: whether the polygons should be simplified or not. reduces edge amount, allow diagonal edges
+    :return: an boundary polygon (counter clockwise numbering) and a list of hole polygons (clockwise numbering)
+    NOTE: convert grid world into polygons in a way that coordinates coincide with grid!
+        -> no conversion of obtained graphs needed!
+        the origin of the polygon coordinate system is (-0.5,-0.5) in the grid cell system (= corners of the grid world)
+    """
+
+    assert size_x > 0 and size_y > 0
+
+    if len(obstacle_iter) == 0:
+        # there are no obstacles. return just the simple boundary rectangle
+        return [np.array(x, y) for x, y in [(0, 0), (size_x, 0), (size_x, size_y), (0, size_y)]], []
+
+    # convert (x,y) into np.arrays
+    # obstacle_iter = [np.array(o) for o in obstacle_iter]
+    obstacle_iter = np.array(obstacle_iter)
+
+    def within_grid(pos):
+        return 0 <= pos[0] < size_x and 0 <= pos[1] < size_y
+
+    def is_equal(pos1, pos2):
+        return np.all(pos1 == pos2)
+
+    def pos_in_iter(pos, iter):
+        for i in iter:
+            if is_equal(pos, i):
+                return True
+        return False
+
+    def is_obstacle(pos):
+        return pos_in_iter(pos, obstacle_iter)
+
+    def is_blocked(pos):
+        return not within_grid(pos) or is_obstacle(pos)
+
+    def is_unblocked(pos):
+        return within_grid(pos) and not is_obstacle(pos)
+
+    def find_start(start_pos, boundary_detect_fct, **kwargs):
+        # returns the lowest and leftmost unblocked grid cell from the start position
+        # for which the detection function evaluates to True
+        start_x, start_y = start_pos
+        for y in range(start_y, size_y):
+            for x in range(start_x, size_x):
+                pos = np.array([x, y])
+                if boundary_detect_fct(pos, **kwargs):
+                    return pos
+
+    # north, east, south, west
+    directions = np.array([[0, 1], [1, 0], [0, -1], [-1, 0]], dtype=int)
+    # the correct offset to determine where nodes should be added.
+    offsets = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=int)
+
+    def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: bool):
+        current_pos = start_pos.copy()
+        # (at least) the west and south are blocked
+        #   -> there has to be a polygon node at the current position (bottom left corner of the cell)
+        edge_list = [start_pos]
+        forward_index = 0  # start with moving north
+        forward_vect = directions[forward_index]
+        left_index = (forward_index - 1) % 4
+        # left_vect = directions[(forward_index - 1) % 4]
+        just_turned = True
+
+        # follow the border between obstacles and free cells ("wall") until one
+        # reaches the start position again
+        while True:
+            # left has to be checked first
+            # do not check if just turned left or right (-> the left is blocked for sure)
+            # left_pos = current_pos + left_vect
+            if not (just_turned or boundary_detect_fct(current_pos + directions[left_index])):
+                # print('< turn left')
+                forward_index = left_index
+                left_index = (forward_index - 1) % 4
+                forward_vect = directions[forward_index]
+                just_turned = True
+
+                # add a new node at the correct position
+                # decrease the index first!
+                edge_list.append(current_pos + offsets[forward_index])
+
+                # move forward (previously left, there is no obstacle)
+                current_pos += forward_vect
+            else:
+                forward_pos = current_pos + forward_vect
+                if boundary_detect_fct(forward_pos):
+                    node_pos = current_pos + offsets[forward_index]
+                    # there is a node at the bottom left corner of the start position (offset= (0,0) )
+                    if is_equal(node_pos, start_pos):
+                        # check and terminate if this node does already exist
+                        break
+
+                    # add a new node at the correct position
+                    edge_list.append(node_pos)
+                    # print('> turn right')
+                    left_index = forward_index
+                    forward_index = (forward_index + 1) % 4
+                    forward_vect = directions[forward_index]
+                    just_turned = True
+                    # print(direction_index,forward_vect,just_turned,edge_list,)
+                else:
+                    # print('^ move forward')
+                    current_pos += forward_vect
+                    just_turned = False
+
+        if cntr_clockwise_wanted:
+            # make edge numbering counter clockwise!
+            edge_list.reverse()
+        return np.array(edge_list)
+
+    # build the boundary polygon
+    # start at the lowest and leftmost unblocked grid cell
+    start_pos = find_start(start_pos=(0, 0), boundary_detect_fct=is_unblocked)
+    # print(start_pos+directions[3])
+    # raise ValueError
+    boundary_edges = construct_polygon(start_pos, boundary_detect_fct=is_blocked, cntr_clockwise_wanted=True)
+
+    if simplify:
+        # TODO
+        raise NotImplementedError()
+
+    # detect which of the obstacles have to be converted into holes
+    # just the obstacles inside the boundary polygon are part of holes
+    # shift coordinates by +(0.5,0.5) for correct detection
+    # the border value does not matter here
+    unchecked_obstacles = [
+        o for o in obstacle_iter if inside_polygon(o[0] + 0.5, o[1] + 0.5, boundary_edges, border_value=True)
+    ]
+
+    hole_list = []
+    while len(unchecked_obstacles) > 0:
+        start_pos = find_start(start_pos=(0, 0), boundary_detect_fct=pos_in_iter, iter=unchecked_obstacles)
+        hole = construct_polygon(start_pos, boundary_detect_fct=is_unblocked, cntr_clockwise_wanted=False)
+
+        # detect which of the obstacles still do not belong to any hole:
+        # delete the obstacles which are included in the just constructed hole
+        unchecked_obstacles = [
+            o for o in unchecked_obstacles if not inside_polygon(o[0] + 0.5, o[1] + 0.5, hole, border_value=True)
+        ]
+
+        if simplify:
+            # TODO
+            pass
+
+        hole_list.append(hole)
+
+    return boundary_edges, hole_list

From 188726c13fe9054d044cfc04fb61ee6f4272a50d Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 16:21:48 +0200
Subject: [PATCH 31/44] functional refactoring

---
 extremitypathfinder/extremitypathfinder.py | 229 +++----------
 extremitypathfinder/helper_classes.py      |  95 ------
 extremitypathfinder/helper_fcts.py         | 358 ++++++++++++++-------
 extremitypathfinder/plotting.py            |   4 +-
 tests/helper_fcts_test.py                  |   4 +-
 5 files changed, 291 insertions(+), 399 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 06e216d..d115126 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,6 +1,6 @@
 import pickle
 from copy import deepcopy
-from typing import List, Optional, Tuple
+from typing import Dict, List, Tuple
 
 import numpy as np
 
@@ -12,21 +12,14 @@
     PATH_TYPE,
     InputCoords,
 )
-from extremitypathfinder.helper_classes import (
-    DirectedHeuristicGraph,
-    Polygon,
-    PolygonVertex,
-    angle_rep_inverse,
-    compute_extremity_idxs,
-)
+from extremitypathfinder.helper_classes import DirectedHeuristicGraph, compute_extremity_idxs
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
+    compute_graph,
     convert_gridworld,
-    find_visible2,
-    find_within_range,
-    get_angle_repr,
+    find_visible,
     get_repr_n_dists,
-    inside_polygon,
+    is_within_map,
 )
 
 # TODO possible to allow polygon consisting of 2 vertices only(=barrier)? lots of functions need at least 3 vertices atm
@@ -51,13 +44,18 @@ class PolygonEnvironment:
     <https://www.cs.au.dk/~gerth/advising/thesis/anders-strand-holm-vinther_magnus-strand-holm-vinther.pdf>`__"
     """
 
-    boundary_polygon: Polygon = None
-    holes: List[Polygon] = None
+    nr_edges: int
     prepared: bool = False
-    graph: DirectedHeuristicGraph = None
-    temp_graph: DirectedHeuristicGraph = None  # for storing and plotting the graph during a query
-    _all_extremities: Optional[List[PolygonVertex]] = None
-    _all_vertices: Optional[List[PolygonVertex]] = None
+    holes: List[np.ndarray]
+    extremity_indices: List[int]
+    reprs_n_distances: Dict[int, np.ndarray]
+    graph: DirectedHeuristicGraph
+    temp_graph: DirectedHeuristicGraph  # for storing and plotting the graph during a query
+    boundary_polygon: np.ndarray
+    coords: np.ndarray
+    edge_vertex_idxs: np.ndarray
+    extremity_mask: np.ndarray
+    vertex_edge_idxs: np.ndarray
 
     @property
     def nr_edges(self) -> int:
@@ -68,6 +66,11 @@ def all_extremities(self) -> List[Tuple]:
         coords = self.coords
         return [tuple(coords[i]) for i in self.extremity_indices]
 
+    @property
+    def all_vertices(self) -> List[Tuple]:
+        coords = self.coords
+        return [tuple(coords[i]) for i in range(self.nr_vertices)]
+
     def store(
         self,
         boundary_coordinates: INPUT_COORD_LIST_TYPE,
@@ -98,15 +101,15 @@ def store(
         if validate:
             check_data_requirements(boundary_coordinates, list_of_hole_coordinates)
 
-        self.boundary_polygon = Polygon(boundary_coordinates, is_hole=False)
+        self.boundary_polygon = boundary_coordinates
 
         # IMPORTANT: make a copy of the list instead of linking to the same list (python!)
-        self.holes = [Polygon(coordinates, is_hole=True) for coordinates in list_of_hole_coordinates]
+        self.holes = list_of_hole_coordinates
 
         list_of_polygons = [boundary_coordinates] + list_of_hole_coordinates
         nr_total_pts = sum(map(len, list_of_polygons))
         vertex_edge_idxs = np.empty((nr_total_pts, 2), dtype=int)
-        # TODO required? inverse of the other
+        # TODO required? inverse of the other. get_neighbours function
         edge_vertex_idxs = np.empty((nr_total_pts, 2), dtype=int)
         edge_idx = 0
         offset = 0
@@ -209,135 +212,15 @@ def prepare(self):
             self.graph = DirectedHeuristicGraph()
             return
 
-        # TODO pre computation and storage of all
-        # TODO more performant way of computing
-        nr_edges = self.nr_edges
-        extremity_indices = self.extremity_indices
-        extremity_mask = self.extremity_mask
-        coords = self.coords
-        vertex_edge_idxs = self.vertex_edge_idxs
-        edge_vertex_idxs = self.edge_vertex_idxs
-
-        # TODO sparse matrix. problematic: default value is 0.0
-        # angle_representations = np.full((nr_vertices, nr_vertices), np.nan)
-
-        # TODO reuse
-        def get_repr(idx_origin, i):
-            coords_origin = coords[idx_origin]
-            return get_angle_repr(coords_origin, coords[i])
-
-        def get_neighbours(i: int) -> Tuple[int, int]:
-            edge_idx1, edge_idx2 = vertex_edge_idxs[i]
-            neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
-            neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
-            return neigh_idx1, neigh_idx2
-
-        def get_relative_coords(orig_idx: int, i: int) -> np.ndarray:
-            coords_origin = coords[orig_idx]
-            coords_v = coords[i]
-            return coords_v - coords_origin
-
-        def get_distance_to_origin(orig_idx: int, i: int) -> float:
-            coords_rel = get_relative_coords(orig_idx, i)
-            return np.linalg.norm(coords_rel, ord=2)
-
-        extremity_coord_map = {i: coords[i] for i in extremity_indices}
-        graph = DirectedHeuristicGraph(extremity_coord_map)
-        for extr_ptr, origin_idx in enumerate(extremity_indices):
-            # extremities are always visible to each other
-            # (bi-directional relation -> undirected edges in the graph)
-            #  -> do not check extremities which have been checked already
-            #  (must give the same result when algorithms are correct)
-            # the origin extremity itself must also not be checked when looking for visible neighbours
-            candidate_idxs = extremity_indices[extr_ptr + 1 :]
-
-            # vertices all belong to a polygon
-            idx_n1, idx_n2 = get_neighbours(origin_idx)
-            # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
-            # eliminate all vertices 'behind' the query point from the candidate set
-            # since the query vertex is an extremity the 'outer' angle is < 180 degree
-            # then the difference between the angle representation of the two edges has to be < 2.0
-            # all vertices between the angle of the two neighbouring edges ('outer side')
-            #   are not visible (no candidates!)
-            # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
-            n1_repr = get_repr(origin_idx, idx_n1)
-            n2_repr = get_repr(origin_idx, idx_n2)
-
-            vert_idx2repr, vert_idx2dist = self.reprs_n_distances[origin_idx]
-
-            # only consider extremities with coords_rel different from the query extremity
-            # (angle representation not None)
-
-            candidate_idxs = {i for i in candidate_idxs if vert_idx2repr[i] is not np.nan}
-            idxs_behind = find_within_range(
-                n1_repr,
-                n2_repr,
-                vert_idx2repr,
-                candidate_idxs,
-                angle_range_less_180=True,
-                equal_repr_allowed=False,
-            )
-            # do not consider points found to lie behind
-            candidate_idxs.difference_update(idxs_behind)
-
-            # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
-            # such that both adjacent edges are visible, one will never visit e, because everything is
-            # reachable on a shorter path without e (except e itself).
-            # An extremity e1 lying in the area "in front of" extremity e hence is never the next vertex
-            # in a shortest path coming from e.
-            # And also in reverse: when coming from e1 everything else than e itself can be reached faster
-            # without visiting e.
-            # -> e1 and e do not have to be connected in the graph.
-            # IMPORTANT: this condition only holds for building the basic visibility graph without start and goal node!
-            # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
-            # MUST be added to the graph!
-            # Find extremities which fulfill this condition for the given query extremity
-            n1_repr_inv = angle_rep_inverse(n1_repr)
-            n2_repr_inv = angle_rep_inverse(n2_repr)
-
-            # IMPORTANT: check all extremities here, not just current candidates
-            # do not check extremities with equal coords_rel (also query extremity itself!)
-            #   and with the same angle representation (those edges must not get deleted from graph!)
-            idxs_in_front = find_within_range(
-                n1_repr_inv,
-                n2_repr_inv,
-                vert_idx2repr,
-                candidate_idxs,
-                angle_range_less_180=True,
-                equal_repr_allowed=False,
-            )
-            # do not consider points lying in front when looking for visible extremities,
-            # even if they are actually be visible
-            candidate_idxs.difference_update(idxs_in_front)
-
-            # all edges have to be checked, except the 2 neighbouring edges (handled above!)
-            edge_idxs2check = set(range(nr_edges))
-            edge1_idx, edge2_idx = vertex_edge_idxs[origin_idx]
-            edge_idxs2check.remove(edge1_idx)
-            edge_idxs2check.remove(edge2_idx)
-            coords_origin = coords[origin_idx]
-            visible_idxs = find_visible2(
-                extremity_mask,
-                coords,
-                vertex_edge_idxs,
-                edge_vertex_idxs,
-                edge_idxs2check,
-                coords_origin,
-                vert_idx2repr,
-                vert_idx2dist,
-                candidate_idxs,
-                origin_idx,
-            )
-
-            visible_vertex2dist_map = {i: get_distance_to_origin(origin_idx, i) for i in visible_idxs}
-            graph.add_multiple_undirected_edges(origin_idx, visible_vertex2dist_map)
-            # optimisation: "thin out" the graph
-            # remove already existing edges in the graph to the extremities in front
-            graph.remove_multiple_undirected_edges(origin_idx, idxs_in_front)
-
-        graph.join_identical()  # join all nodes with the same coords_rel
-
-        self.graph = graph
+        self.graph = compute_graph(
+            self.nr_edges,
+            self.extremity_indices,
+            self.reprs_n_distances,
+            self.coords,
+            self.edge_vertex_idxs,
+            self.extremity_mask,
+            self.vertex_edge_idxs,
+        )
         self.prepared = True
 
     def within_map(self, coords: InputCoords):
@@ -346,13 +229,10 @@ def within_map(self, coords: InputCoords):
         :param coords: numerical tuple representing coordinates
         :return: whether the given coordinate is a valid query point
         """
+        boundary = self.boundary_polygon
+        holes = self.holes
         x, y = coords
-        if not inside_polygon(x, y, self.boundary_polygon.coordinates, border_value=True):
-            return False
-        for hole in self.holes:
-            if inside_polygon(x, y, hole.coordinates, border_value=False):
-                return False
-        return True
+        return is_within_map(x, y, boundary, holes)
 
     def find_shortest_path(
         self,
@@ -371,16 +251,6 @@ def find_shortest_path(
          if points close to or on polygon edges should be accepted as valid input, set this to ``False``.
         :return: a tuple of shortest path and its length. ([], None) if there is no possible path.
         """
-
-        # TODO reuse
-        def get_relative_coords(coords_origin: np.ndarray, i: int) -> np.ndarray:
-            coords_v = coords[i]
-            return coords_v - coords_origin
-
-        def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
-            coords_rel = get_relative_coords(coords_origin, i)
-            return np.linalg.norm(coords_rel, ord=2)
-
         # path planning query:
         # make sure the map has been loaded and prepared
         if self.boundary_polygon is None:
@@ -425,23 +295,22 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
         # (they also cause errors in the algorithms, because their angle repr is not defined!)
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
-        self.graph.get_all_nodes().add(idx_start)
-        coords_origin = coords[idx_origin]
+        candidate_idxs = self.graph.all_nodes
+        # TODO work on copy to not modify the graph!?
+        candidate_idxs.add(idx_start)
         edge_idxs2check = set(range(nr_edges))
-
         vert_idx2repr, vert_idx2dist = get_repr_n_dists(idx_origin, coords)
-        candidate_idxs = {i for i in (self.graph.get_all_nodes()) if not vert_idx2dist[i] == 0.0}
-        visible_idxs = find_visible2(
+        candidate_idxs = {i for i in candidate_idxs if not vert_idx2dist[i] == 0.0}
+        visible_idxs = find_visible(
+            idx_origin,
+            candidate_idxs,
+            edge_idxs2check,
             extremity_mask,
             coords,
             vertex_edge_idxs,
             edge_vertex_idxs,
-            edge_idxs2check,
-            coords_origin,
             vert_idx2repr,
             vert_idx2dist,
-            candidate_idxs,
-            idx_origin,
         )
         visibles_n_distances_map = {i: vert_idx2dist[i] for i in visible_idxs}
 
@@ -461,23 +330,21 @@ def get_distance_to_origin(coords_origin: np.ndarray, i: int) -> float:
             graph.add_directed_edge(i, idx_goal, d)
 
         idx_origin = idx_start
-        coords_origin = coords[idx_origin]
         # the visibility of only the graphs nodes have to be checked
         # the goal node does not have to be considered, because of the earlier check
         edge_idxs2check = set(range(nr_edges))  # new copy
         vert_idx2repr, vert_idx2dist = get_repr_n_dists(idx_origin, coords)
         candidate_idxs = {i for i in self.graph.get_all_nodes() if not vert_idx2dist[i] == 0.0}
-        visible_idxs = find_visible2(
+        visible_idxs = find_visible(
+            idx_origin,
+            candidate_idxs,
+            edge_idxs2check,
             extremity_mask,
             coords,
             vertex_edge_idxs,
             edge_vertex_idxs,
-            edge_idxs2check,
-            coords_origin,
             vert_idx2repr,
             vert_idx2dist,
-            candidate_idxs,
-            idx_origin,
         )
 
         if len(visible_idxs) == 0:
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index fe85652..d8f9dcd 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -254,101 +254,6 @@ def compute_extremity_idxs(coordinates: np.ndarray) -> List[int]:
     return extr_idxs
 
 
-class PolygonVertex(Vertex):
-    # __slots__ declared in parents are available in child classes. However, child subclasses will get a __dict__
-    # and __weakref__ unless they also define __slots__ (which should only contain names of any additional slots).
-    __slots__ = ["edge1", "edge2", "neighbour1", "neighbour2"]
-
-    def __init__(self, *args, **kwargs):
-        super(PolygonVertex, self).__init__(*args, **kwargs)
-        self.edge1: Optional[Edge] = None
-        self.edge2: Optional[Edge] = None
-        self.neighbour1: Optional[PolygonVertex] = None
-        self.neighbour2: Optional[PolygonVertex] = None
-
-    def get_neighbours(self):
-        return self.neighbour1, self.neighbour2
-
-    def declare_extremity(self):
-        self.is_extremity = True
-
-    def set_edge1(self, e1):
-        self.edge1 = e1
-        # ordering is important! the numbering convention has to stay intact!
-        self.neighbour1 = e1.vertex1
-
-    def set_edge2(self, e2):
-        self.edge2 = e2
-        # ordering is important! the numbering convention has to stay intact!
-        self.neighbour2 = e2.vertex2
-
-
-class Edge(object):
-    __slots__ = ["vertex1", "vertex2"]
-
-    def __init__(self, vertex1, vertex2):
-        self.vertex1: PolygonVertex = vertex1
-        self.vertex2: PolygonVertex = vertex2
-
-    def __str__(self):
-        return self.vertex1.__str__() + "-->" + self.vertex2.__str__()
-
-    def __repr__(self):
-        return self.__str__()
-
-
-class Polygon(object):
-    __slots__ = ["vertices", "edges", "coordinates", "is_hole", "_extremities"]
-
-    def __init__(self, coordinate_list, is_hole):
-        # store just the coordinates separately from the vertices in the format suiting the inside_polygon() function
-        self.coordinates = np.array(coordinate_list)
-
-        self.is_hole: bool = is_hole
-
-        if len(coordinate_list) < 3:
-            raise ValueError(
-                "This is not a valid polygon:",
-                coordinate_list,
-                "# edges:",
-                len(coordinate_list),
-            )
-
-        self.vertices: List[PolygonVertex] = [PolygonVertex(coordinate) for coordinate in coordinate_list]
-
-        self.edges: List[Edge] = []
-        vertex1 = self.vertices[-1]
-        for vertex2 in self.vertices:
-            edge = Edge(vertex1, vertex2)
-            # ordering is important! the numbering convention has to stay intact!
-            vertex1.set_edge2(edge)
-            vertex2.set_edge1(edge)
-            self.edges.append(edge)
-            vertex1 = vertex2
-
-        self._extremities: Optional[List[PolygonVertex]] = None
-
-    def _find_extremities(self):
-        coordinates = [v.coordinates for v in self.vertices]
-        extr_idxs = compute_extremity_idxs(coordinates)
-        extremities = [self.vertices[i] for i in extr_idxs]
-        for v in extremities:
-            v.declare_extremity()
-        self._extremities = extremities
-
-    @property
-    def extremities(self) -> List[PolygonVertex]:
-        if self._extremities is None:
-            self._find_extremities()
-        return self._extremities
-
-    def translate(self, new_origin: Vertex):
-        global origin
-        origin = new_origin
-        for vertex in self.vertices:
-            vertex.mark_outdated()
-
-
 class SearchState(object):
     __slots__ = ["node", "distance", "neighbours", "path", "cost_so_far", "priority"]
 
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 21bcd36..4d62d5f 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,13 +1,19 @@
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
 from itertools import combinations
-from typing import List, Set, Tuple
+from typing import Dict, Iterable, List, Set, Tuple
 
 import numpy as np
 import numpy.linalg
 
 from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import compute_angle_repr, compute_angle_repr_inner, compute_repr_n_dist
+from extremitypathfinder.helper_classes import (
+    DirectedHeuristicGraph,
+    angle_rep_inverse,
+    compute_angle_repr,
+    compute_angle_repr_inner,
+    compute_repr_n_dist,
+)
 
 
 def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
@@ -72,6 +78,15 @@ def inside_polygon(x, y, coords, border_value):
     return contained
 
 
+def is_within_map(x, y, boundary, holes):
+    if not inside_polygon(x, y, boundary, border_value=True):
+        return False
+    for hole in holes:
+        if inside_polygon(x, y, hole, border_value=False):
+            return False
+    return True
+
+
 def no_identical_consequent_vertices(coords):
     p1 = coords[-1]
     for p2 in coords:
@@ -255,9 +270,9 @@ def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
 
 
 def find_within_range(
+    vert_idx2repr: np.ndarray,
     repr1: float,
     repr2: float,
-    vert_idx2repr: np.ndarray,
     candidate_idxs: Set[int],
     angle_range_less_180: bool,
     equal_repr_allowed: bool,
@@ -317,105 +332,153 @@ def within_filter_func(r: float) -> bool:
     return idxs_within
 
 
-def get_neighbours(i: int, vertex_edge_idxs: np.ndarray, edge_vertex_idxs: np.ndarray) -> Tuple[int, int]:
+def get_neighbour_idxs(i: int, vertex_edge_idxs: np.ndarray, edge_vertex_idxs: np.ndarray) -> Tuple[int, int]:
     edge_idx1, edge_idx2 = vertex_edge_idxs[i]
     neigh_idx1 = edge_vertex_idxs[edge_idx1, 0]
     neigh_idx2 = edge_vertex_idxs[edge_idx2, 1]
     return neigh_idx1, neigh_idx2
 
 
-def skip_edge(cand_idxs, edge_vertex_idxs, extremity_mask, vert_idx2dist, vertex_edge_idxs, node_idx, edge2discard):
+def skip_edge(
+    node: int,
+    edge2discard: int,
+    candidates: Set[int],
+    edge_vertex_idxs: np.ndarray,
+    extremity_mask: np.ndarray,
+    vertex_edge_idxs: np.ndarray,
+):
     # (note: not identical, does not belong to the same polygon!)
     # mark this vertex as not visible (would otherwise add 0 distance edge in the graph)
-    cand_idxs.discard(node_idx)
+    candidates.discard(node)
     # no points lie truly "behind" this edge as there is no "direction of sight" defined
     # <-> angle representation/range undefined for just this single edge
     # however if one considers the point neighbouring in the other direction (<-> two edges)
     # these two neighbouring edges define an invisible angle range
     # -> simply move the pointer
-    v1, v2 = get_neighbours(node_idx, vertex_edge_idxs, edge_vertex_idxs)
-    dist_v1 = vert_idx2dist[node_idx]
-    dist_v2 = vert_idx2dist[node_idx]
-    range_less_180 = extremity_mask[node_idx]
+    v1, v2 = get_neighbour_idxs(node, vertex_edge_idxs, edge_vertex_idxs)
+    range_less_180 = extremity_mask[node]
     # do not check the other neighbouring edge of vertex1 in the future (has been considered already)
-    edge_idx = vertex_edge_idxs[node_idx][edge2discard]
-    return dist_v1, dist_v2, edge_idx, range_less_180, v1, v2
+    edge_idx = vertex_edge_idxs[node][edge2discard]
+    return edge_idx, range_less_180, v1, v2
+
+
+def find_candidates_behind(
+    origin: int, v1: int, v2: int, candidates: Set[int], distances: np.ndarray, coords: np.ndarray
+) -> Set[int]:
+    dist_v1 = distances[v1]
+    dist_v2 = distances[v2]
+    max_distance = max(dist_v1, dist_v2)
+    idxs_behind = set()
+    # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
+    #    has an intersection with the current edge p1---p2
+    for idx in candidates:
+        # if a candidate is farther away from the query point than both vertices of the edge,
+        #   it surely lies behind the edge
+        # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
+        #   it still needs to be checked!
+        dist2orig = distances[idx]
+        further_away = dist2orig > max_distance
+        if further_away or lies_behind(v1, v2, idx, origin, coords):
+            idxs_behind.add(idx)
+        # vertex lies in front of this edge
+    return idxs_behind
+
+
+def clean_visibles(visible_idxs: Set[int], cand_idx2repr: np.ndarray, vert_idx2dist: np.ndarray) -> Set[int]:
+    # in case some vertices have the same representation, only return (link) the closest vertex
+    if len(visible_idxs) <= 1:
+        return visible_idxs
+
+    cleaned = set()
+    visible_idxs_sorted = sorted(visible_idxs, key=lambda i: cand_idx2repr[i])
+    min_dist = np.inf
+    first_idx = visible_idxs_sorted[0]
+    rep_prev = cand_idx2repr[first_idx]
+    selected_idx = 0
+    for i in visible_idxs_sorted:
+        rep = cand_idx2repr[i]
+        if rep != rep_prev:
+            cleaned.add(selected_idx)
+            min_dist = np.inf
+            rep_prev = rep
+
+        dist = vert_idx2dist[i]
+        if dist < min_dist:
+            selected_idx = i
+            min_dist = dist
+
+    cleaned.add(selected_idx)
+    return cleaned
 
 
-def find_visible2(
+def find_visible(
+    origin: int,
+    candidates: Set[int],
+    edges_to_check: Set[int],
     extremity_mask: np.ndarray,
     coords: np.ndarray,
     vertex_edge_idxs: np.ndarray,
     edge_vertex_idxs: np.ndarray,
-    edge_idxs2check: Set[int],
-    coords_origin: np.ndarray,
-    vert_idx2repr: np.ndarray,
-    vert_idx2dist: np.ndarray,
-    cand_idxs: Set[int],
-    idx_origin: int,
+    representations: np.ndarray,
+    distances: np.ndarray,
 ) -> Set[int]:
     """
     query_vertex: a vertex for which the visibility to the vertices should be checked.
         also non extremity vertices, polygon vertices and vertices with the same coordinates are allowed.
         query point also might lie directly on an edge! (angle = 180deg)
-    :param candidate_idxs: the set of all vertex ids which should be checked for visibility.
+    :param candidates: the set of all vertex ids which should be checked for visibility.
         IMPORTANT: is being manipulated, so has to be a copy!
         IMPORTANT: must not contain the query vertex!
     :param edges_to_check: the set of edges which determine visibility
     :return: a set of tuples of all vertices visible from the query vertex and the corresponding distance
     """
-    if len(cand_idxs) == 0:
-        return cand_idxs
+    if len(candidates) == 0:
+        return candidates
 
-    visible_idxs = set()
+    visibles = set()
     # goal: eliminating all vertices lying 'behind' any edge
-    while len(cand_idxs) > 0:
+    while len(candidates) > 0:
         try:
-            edge_idx = edge_idxs2check.pop()
+            edge = edges_to_check.pop()
         except KeyError:
             break  # no more edges left to check
 
-        v1, v2 = edge_vertex_idxs[edge_idx]
-
-        dist_v1 = vert_idx2dist[v1]
-        dist_v2 = vert_idx2dist[v2]
+        v1, v2 = edge_vertex_idxs[edge]
         lies_on_edge = False
         range_less_180 = False
 
-        if dist_v1 == 0.0:
+        if distances[v1] == 0.0:
             # vertex1 of the edge has the same coordinates as the query vertex
             # -> the origin lies on the edge
             lies_on_edge = True
-            dist_v1, dist_v2, edge_idx, range_less_180, v1, v2 = skip_edge(
-                cand_idxs,
-                edge_vertex_idxs,
-                extremity_mask,
-                vert_idx2dist,
-                vertex_edge_idxs,
-                node_idx=v1,
+            edge, range_less_180, v1, v2 = skip_edge(
+                node=v1,
                 edge2discard=0,
+                candidates=candidates,
+                edge_vertex_idxs=edge_vertex_idxs,
+                extremity_mask=extremity_mask,
+                vertex_edge_idxs=vertex_edge_idxs,
             )
-            edge_idxs2check.discard(edge_idx)
+            edges_to_check.discard(edge)
 
-        elif dist_v2 == 0.0:
+        elif distances[v2] == 0.0:
             # same for vertex2 of the edge
             # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex
             # (edge with length 0)
             lies_on_edge = True
-            cand_idxs.discard(v2)
-            v1, v2 = get_neighbours(v2, vertex_edge_idxs, edge_vertex_idxs)
-            dist_v1 = vert_idx2dist[v1]
-            dist_v2 = vert_idx2dist[v2]
-            range_less_180 = extremity_mask[v2]
-            _, edge_idx2 = vertex_edge_idxs[v2]
-            edge_idxs2check.discard(edge_idx2)
-
-        repr1 = vert_idx2repr[v1]
-        repr2 = vert_idx2repr[v2]
-        if repr2 is None or repr1 is None:
-            raise ValueError
-        repr_diff = abs(repr1 - repr2)
+            edge, range_less_180, v1, v2 = skip_edge(
+                node=v2,
+                edge2discard=1,
+                candidates=candidates,
+                edge_vertex_idxs=edge_vertex_idxs,
+                extremity_mask=extremity_mask,
+                vertex_edge_idxs=vertex_edge_idxs,
+            )
+            edges_to_check.discard(edge)
 
+        repr1 = representations[v1]
+        repr2 = representations[v2]
+        repr_diff = abs(repr1 - repr2)
         if repr_diff == 2.0:
             # angle == 180deg -> on the edge
             lies_on_edge = True
@@ -425,38 +488,28 @@ def find_visible2(
             # the neighbouring edges are visible for sure
             # attention: only add to visible set if vertex was a candidate!
             try:
-                cand_idxs.remove(v1)
-                visible_idxs.add(v1)
+                candidates.remove(v1)
+                visibles.add(v1)
             except KeyError:
                 pass
             try:
-                cand_idxs.remove(v2)
-                visible_idxs.add(v2)
+                candidates.remove(v2)
+                visibles.add(v2)
             except KeyError:
                 pass
 
             # all the candidates between the two vertices v1 v2 are not visible for sure
             # candidates with the same representation must not be deleted, because they can be visible!
-            cand_idx2repr = {i: vert_idx2repr[i] for i in cand_idxs}
-
-            candidate_idxs = set(cand_idx2repr.keys())
             invisible_candidate_idxs = find_within_range(
-                repr1,
-                repr2,
-                vert_idx2repr,
-                candidate_idxs,
-                angle_range_less_180=range_less_180,
-                equal_repr_allowed=False,
+                representations, repr1, repr2, candidates, angle_range_less_180=range_less_180, equal_repr_allowed=False
             )
-            candidate_idxs.difference_update(invisible_candidate_idxs)
-            for i in invisible_candidate_idxs:
-                cand_idx2repr.pop(i, None)
+            candidates.difference_update(invisible_candidate_idxs)
             continue
 
         # case: a 'regular' edge
         # eliminate all candidates which are blocked by the edge
         # that means inside the angle range spanned by the edge and actually behind it
-        cand_idxs_tmp = cand_idxs.copy()
+        cand_idxs_tmp = candidates.copy()
         # the vertices belonging to the edge itself (its vertices) must not be checked.
         # use discard() instead of remove() to not raise an error (they might not be candidates)
         cand_idxs_tmp.discard(v1)
@@ -468,64 +521,131 @@ def find_visible2(
         #   is always < 180deg when the edge is not running through the query point (=180 deg)
         #  candidates with the same representation as v1 or v2 should be considered.
         #   they can be visible, but should be ruled out if they lie behind any edge!
-        idxs2check = find_within_range(
-            repr1,
-            repr2,
-            vert_idx2repr,
-            cand_idxs_tmp,
-            angle_range_less_180=True,
-            equal_repr_allowed=True,
+        cand_idxs_tmp = find_within_range(
+            representations, repr1, repr2, cand_idxs_tmp, angle_range_less_180=True, equal_repr_allowed=True
         )
 
-        max_distance = max(dist_v1, dist_v2)
-        idxs_behind = set()
-        # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v
-        #    has an intersection with the current edge p1---p2
-        for idx in idxs2check:
-            # if a candidate is farther away from the query point than both vertices of the edge,
-            #   it surely lies behind the edge
-            # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge,
-            #   it still needs to be checked!
-            dist2orig = vert_idx2dist[idx]
-            further_away = dist2orig > max_distance
-            if further_away or lies_behind(v1, v2, idx, idx_origin, coords):
-                idxs_behind.add(idx)
-            # vertex lies in front of this edge
-
+        idxs_behind = find_candidates_behind(origin, v1, v2, cand_idxs_tmp, distances, coords)
         # vertices behind any edge are not visible
-        cand_idxs.difference_update(idxs_behind)
+        candidates.difference_update(idxs_behind)
 
     # all edges have been checked
     # all remaining vertices were not concealed behind any edge and hence are visible
-    visible_idxs.update(cand_idxs)
-    return clean_visible_idxs(visible_idxs, vert_idx2repr, vert_idx2dist)
+    visibles.update(candidates)
+    return clean_visibles(visibles, representations, distances)
 
 
-def clean_visible_idxs(visible_idxs: Set[int], cand_idx2repr: np.ndarray, vert_idx2dist: np.ndarray) -> Set[int]:
-    # in case some vertices have the same representation, only return (link) the closest vertex
-    if len(visible_idxs) <= 1:
-        return visible_idxs
-
-    cleaned = set()
-    visible_idxs_sorted = sorted(visible_idxs, key=lambda i: cand_idx2repr[i])
-    min_dist = np.inf
-    first_idx = visible_idxs_sorted[0]
-    rep_prev = cand_idx2repr[first_idx]
-    selected_idx = 0
-    for i in visible_idxs_sorted:
-        rep = cand_idx2repr[i]
-        if rep != rep_prev:
-            cleaned.add(selected_idx)
-            min_dist = np.inf
-            rep_prev = rep
-
-        dist = vert_idx2dist[i]
-        if dist < min_dist:
-            selected_idx = i
-            min_dist = dist
-
-    cleaned.add(selected_idx)
-    return cleaned
+def find_visible_and_in_front(
+    origin: int,
+    nr_edges: int,
+    coords: np.ndarray,
+    candidates: Set[int],
+    extremities: Iterable[int],
+    extremity_mask: np.ndarray,
+    distances: np.ndarray,
+    representations: np.ndarray,
+    vertex_edge_idxs: np.ndarray,
+    edge_vertex_idxs: np.ndarray,
+):
+    # vertices all belong to a polygon
+    n1, n2 = get_neighbour_idxs(origin, vertex_edge_idxs, edge_vertex_idxs)
+    # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other!
+    # eliminate all vertices 'behind' the query point from the candidate set
+    # since the query vertex is an extremity the 'outer' angle is < 180 degree
+    # then the difference between the angle representation of the two edges has to be < 2.0
+    # all vertices between the angle of the two neighbouring edges ('outer side')
+    #   are not visible (no candidates!)
+    # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted!
+    n1_repr = representations[n1]
+    n2_repr = representations[n2]
+    idxs_behind = find_within_range(
+        representations, n1_repr, n2_repr, candidates, angle_range_less_180=True, equal_repr_allowed=False
+    )
+    # do not consider points found to lie behind
+    candidates.difference_update(idxs_behind)
+    # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e,
+    # such that both adjacent edges are visible, one will never visit e, because everything is
+    # reachable on a shorter path without e (except e itself).
+    # An extremity e1 lying in the area "in front of" extremity e hence is never the next vertex
+    # in a shortest path coming from e.
+    # And also in reverse: when coming from e1 everything else than e itself can be reached faster
+    # without visiting e.
+    # -> e1 and e do not have to be connected in the graph.
+    # IMPORTANT: this condition only holds for building the basic visibility graph without start and goal node!
+    # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front
+    # MUST be added to the graph!
+    # Find extremities which fulfill this condition for the given query extremity
+    # IMPORTANT: check all extremities here, not just current candidates
+    # do not check extremities with equal coords_rel (also query extremity itself!)
+    #   and with the same angle representation (those edges must not get deleted from graph!)
+    idxs_in_front = find_within_range(
+        representations,
+        repr1=angle_rep_inverse(n1_repr),
+        repr2=angle_rep_inverse(n2_repr),
+        candidate_idxs=set(extremities),
+        angle_range_less_180=True,
+        equal_repr_allowed=False,
+    )
+    # do not consider points lying in front when looking for visible extremities,
+    # even if they are actually be visible.
+    candidates.difference_update(idxs_in_front)
+    # all edges have to be checked, except the 2 neighbouring edges (handled above!)
+    edge_idxs2check = set(range(nr_edges))
+    edge_idxs2check.difference_update(vertex_edge_idxs[origin])
+    visible_idxs = find_visible(
+        origin,
+        candidates,
+        edge_idxs2check,
+        extremity_mask,
+        coords,
+        vertex_edge_idxs,
+        edge_vertex_idxs,
+        representations,
+        distances,
+    )
+    return idxs_in_front, visible_idxs
+
+
+def compute_graph(
+    nr_edges: int,
+    extremity_indices: Iterable[int],
+    reprs_n_distances: Dict[int, np.ndarray],
+    coords: np.ndarray,
+    edge_vertex_idxs: np.ndarray,
+    extremity_mask: np.ndarray,
+    vertex_edge_idxs: np.ndarray,
+) -> DirectedHeuristicGraph:
+    # IMPORTANT: add all extremities (even if they turn out to be dangling in the end)
+    extremity_coord_map = {i: coords[i] for i in extremity_indices}
+    graph = DirectedHeuristicGraph(extremity_coord_map)
+    for extr_ptr, origin_idx in enumerate(extremity_indices):
+        vert_idx2repr, vert_idx2dist = reprs_n_distances[origin_idx]
+        # optimisation: extremities are always visible to each other
+        # (bi-directional relation -> undirected edges in the graph)
+        #  -> do not check extremities which have been checked already
+        #  (must give the same result when algorithms are correct)
+        # the origin extremity itself must also not be checked when looking for visible neighbours
+        candidate_idxs = set(extremity_indices[extr_ptr + 1 :])
+        idxs_in_front, visible_idxs = find_visible_and_in_front(
+            origin_idx,
+            nr_edges,
+            coords,
+            candidate_idxs,
+            extremity_indices,
+            extremity_mask,
+            vert_idx2dist,
+            vert_idx2repr,
+            vertex_edge_idxs,
+            edge_vertex_idxs,
+        )
+        # "thin out" the graph:
+        # remove already existing edges in the graph to the extremities in front
+        graph.remove_multiple_undirected_edges(origin_idx, idxs_in_front)
+
+        visible_vertex2dist_map = {i: vert_idx2dist[i] for i in visible_idxs}
+        graph.add_multiple_undirected_edges(origin_idx, visible_vertex2dist_map)
+    graph.join_identical()  # join all nodes with the same coordinates
+    return graph
 
 
 def try_extraction(json_data, key):
diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index b786bd4..e43d694 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -66,9 +66,9 @@ def draw_boundaries(map, ax):
     mark_points(map.all_extremities, c="red", s=50)
 
 
-def draw_internal_graph(map, ax):
+def draw_internal_graph(map: PolygonEnvironment, ax):
     graph = map.graph
-    for start_idx, all_goal_idxs in graph.get_neighbours():
+    for start_idx, all_goal_idxs in graph.neighbours.items():
         start = graph.coord_map[start_idx]
         all_goals = [graph.coord_map[i] for i in all_goal_idxs]
         for goal in all_goals:
diff --git a/tests/helper_fcts_test.py b/tests/helper_fcts_test.py
index 9d33c78..04342e6 100755
--- a/tests/helper_fcts_test.py
+++ b/tests/helper_fcts_test.py
@@ -8,7 +8,7 @@
 
 from extremitypathfinder import PolygonEnvironment
 from extremitypathfinder.helper_classes import AngleRepresentation
-from extremitypathfinder.helper_fcts import clean_visible_idxs, has_clockwise_numbering, inside_polygon, read_json
+from extremitypathfinder.helper_fcts import clean_visibles, has_clockwise_numbering, inside_polygon, read_json
 
 
 # TODO test find_visible(), ...
@@ -137,7 +137,7 @@ def test_read_json(self):
 def test_clean_visible_idxs(
     visible_idxs: Set[int], cand_idx2repr: Dict[int, float], vert_idx2dist: Dict[int, float], expected: Set[int]
 ):
-    res = clean_visible_idxs(visible_idxs, cand_idx2repr, vert_idx2dist)
+    res = clean_visibles(visible_idxs, cand_idx2repr, vert_idx2dist)
     assert res == expected
 
 

From bbd7f2022064a3a3d4fce3f504b6fa3656450464 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 16:22:16 +0200
Subject: [PATCH 32/44] Update configs.py

---
 extremitypathfinder/{global_settings.py => configs.py} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename extremitypathfinder/{global_settings.py => configs.py} (100%)

diff --git a/extremitypathfinder/global_settings.py b/extremitypathfinder/configs.py
similarity index 100%
rename from extremitypathfinder/global_settings.py
rename to extremitypathfinder/configs.py

From 4f1c275be635c042147fbaa75b26a0c1d8a24a67 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 16:23:22 +0200
Subject: [PATCH 33/44] rename global_settings

---
 extremitypathfinder/command_line.py        | 2 +-
 extremitypathfinder/extremitypathfinder.py | 2 +-
 extremitypathfinder/helper_fcts.py         | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/extremitypathfinder/command_line.py b/extremitypathfinder/command_line.py
index 5dd7400..4c2b8b1 100644
--- a/extremitypathfinder/command_line.py
+++ b/extremitypathfinder/command_line.py
@@ -1,7 +1,7 @@
 import argparse
 
 from extremitypathfinder import PolygonEnvironment
-from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
+from extremitypathfinder.configs import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
 from extremitypathfinder.helper_fcts import read_json
 
 JSON_HELP_MSG = (
diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index d115126..0f40168 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -4,7 +4,7 @@
 
 import numpy as np
 
-from extremitypathfinder.global_settings import (
+from extremitypathfinder.configs import (
     DEFAULT_PICKLE_NAME,
     INPUT_COORD_LIST_TYPE,
     LENGTH_TYPE,
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 4d62d5f..694df56 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -6,7 +6,7 @@
 import numpy as np
 import numpy.linalg
 
-from extremitypathfinder.global_settings import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
+from extremitypathfinder.configs import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
 from extremitypathfinder.helper_classes import (
     DirectedHeuristicGraph,
     angle_rep_inverse,

From e0e3390dec45135ade6fcc383e6cd07091bb7db3 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 17:54:01 +0200
Subject: [PATCH 34/44] remove OOP helper classes

---
 CHANGELOG.rst                              |  10 +-
 extremitypathfinder/extremitypathfinder.py |   6 +-
 extremitypathfinder/helper_classes.py      | 250 ----------------
 extremitypathfinder/helper_fcts.py         | 153 +++++++---
 extremitypathfinder/plotting.py            |   1 -
 tests/helper_classes_test.py               | 159 ----------
 tests/helper_fcts_test.py                  | 321 ++++++++++++++-------
 7 files changed, 336 insertions(+), 564 deletions(-)
 delete mode 100755 tests/helper_classes_test.py

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 91bdd6b..57ad3a4 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -1,16 +1,20 @@
 Changelog
 =========
 
-2.2.4 (2022-xx)
+
+TODO pending major release: remove separate prepare step?! initialise in one step during initialisation
+
+
+2.3.0 (2022-xx)
 -------------------
 
+* major overhaul of all functionality from OOP to functional/numpy based
+
 internal:
 
 * added test cases
 
 
-TODO remove separate prepare step?!
-
 
 
 2.2.3 (2022-10-11)
diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 0f40168..4cb8305 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -12,9 +12,10 @@
     PATH_TYPE,
     InputCoords,
 )
-from extremitypathfinder.helper_classes import DirectedHeuristicGraph, compute_extremity_idxs
+from extremitypathfinder.helper_classes import DirectedHeuristicGraph
 from extremitypathfinder.helper_fcts import (
     check_data_requirements,
+    compute_extremity_idxs,
     compute_graph,
     convert_gridworld,
     find_visible,
@@ -282,10 +283,10 @@ def find_shortest_path(
         # BUT: this is an edge case -> compute visibility as usual and later try to merge with the graph
 
         # create temporary graph
-        # TODO make more performant, avoid real copy
         # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph
         # but to still not create real copies of vertex instances!
         graph = deepcopy(self.graph)
+        # TODO make more performant, avoid real copy
         # graph = self.graph
 
         # check the goal node first (earlier termination possible)
@@ -296,7 +297,6 @@ def find_shortest_path(
         # IMPORTANT: also check if the start node is visible from the goal node!
         # NOTE: all edges are being checked, it is computationally faster to compute all visibilities in one go
         candidate_idxs = self.graph.all_nodes
-        # TODO work on copy to not modify the graph!?
         candidate_idxs.add(idx_start)
         edge_idxs2check = set(range(nr_edges))
         vert_idx2repr, vert_idx2dist = get_repr_n_dists(idx_origin, coords)
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index d8f9dcd..199870c 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -3,256 +3,6 @@
 
 import numpy as np
 
-# TODO find a way to avoid global variable, wrap all in a different kind of 'coordinate system environment'?
-# problem: lazy evaluation, passing the origin every time is not an option
-# placeholder for temporarily storing the origin of the current coordinate system
-
-origin = None
-
-
-def compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]:
-    """computing representation for the angle from the origin to a given vector
-
-    value in [0.0 : 4.0[
-    every quadrant contains angle measures from 0.0 to 1.0
-    there are 4 quadrants (counter clockwise numbering)
-    0 / 360 degree -> 0.0
-    90 degree -> 1.0
-    180 degree -> 2.0
-    270 degree -> 3.0
-    ...
-    Useful for comparing angles without actually computing expensive trigonometrical functions
-    This representation does not grow directly proportional to its represented angle,
-    but it its bijective and monotonous:
-    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
-    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
-    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
-    with (0,0)' being the vector representing the origin
-
-    :param np_vector:
-    :return:
-    """
-    distance = np.linalg.norm(np_vector, ord=2)
-    if distance == 0.0:
-        angle_measure = np.nan
-    else:
-        # 2D vector: (dx, dy) = np_vector
-        dx, dy = np_vector
-        dx_positive = dx >= 0
-        dy_positive = dy >= 0
-
-        if dx_positive and dy_positive:
-            quadrant = 0.0
-            angle_measure = dy
-
-        elif not dx_positive and dy_positive:
-            quadrant = 1.0
-            angle_measure = -dx
-
-        elif not dx_positive and not dy_positive:
-            quadrant = 2.0
-            angle_measure = -dy
-
-        else:
-            quadrant = 3.0
-            angle_measure = dx
-
-        # normalise angle measure to [0; 1]
-        angle_measure /= distance
-        angle_measure += quadrant
-
-    return angle_measure, distance
-
-
-def compute_angle_repr_inner(np_vector: np.ndarray) -> float:
-    """computing representation for the angle from the origin to a given vector
-
-    value in [0.0 : 4.0[
-    every quadrant contains angle measures from 0.0 to 1.0
-    there are 4 quadrants (counter clockwise numbering)
-    0 / 360 degree -> 0.0
-    90 degree -> 1.0
-    180 degree -> 2.0
-    270 degree -> 3.0
-    ...
-    Useful for comparing angles without actually computing expensive trigonometrical functions
-    This representation does not grow directly proportional to its represented angle,
-    but it its bijective and monotonous:
-    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
-    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
-    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
-    with (0,0)' being the vector representing the origin
-
-    :param np_vector:
-    :return:
-    """
-    # 2D vector: (dx, dy) = np_vector
-    dx, dy = np_vector
-    dx_positive = dx >= 0
-    dy_positive = dy >= 0
-
-    if dx_positive and dy_positive:
-        quadrant = 0.0
-        angle_measure = dy
-
-    elif not dx_positive and dy_positive:
-        quadrant = 1.0
-        angle_measure = -dx
-
-    elif not dx_positive and not dy_positive:
-        quadrant = 2.0
-        angle_measure = -dy
-
-    else:
-        quadrant = 3.0
-        angle_measure = dx
-
-    norm = np.linalg.norm(np_vector, ord=2)
-    if norm == 0.0:
-        # make sure norm is not 0!
-        raise ValueError("received null vector:", np_vector, norm)
-    # normalise angle measure to [0; 1]
-    angle_measure /= norm
-    return quadrant + angle_measure
-
-
-class AngleRepresentation(object):
-    # prevent dynamic attribute assignment (-> safe memory)
-    # __slots__ = ['quadrant', 'angle_measure', 'value']
-    __slots__ = ["value"]
-
-    def __init__(self, np_vector):
-        self.value = compute_angle_repr_inner(np_vector)
-
-    def __str__(self):
-        return str(self.value)
-
-    def __repr__(self):
-        return self.__str__()
-
-
-class Vertex(object):
-    # defining static attributes on class to safe memory
-    __slots__ = [
-        "coordinates",
-        "is_extremity",
-        "is_outdated",
-        "coordinates_translated",
-        "angle_representation",
-        "distance_to_origin",
-    ]
-
-    def __init__(self, coordinates):
-        self.coordinates = np.array(coordinates)
-        self.is_extremity: bool = False
-
-        # a container for temporally storing shifted coordinates
-        self.coordinates_translated = None
-        self.angle_representation: Optional[AngleRepresentation] = None
-        self.distance_to_origin: float = 0.0
-
-        # for lazy evaluation: often the angle representations dont have to be computed for every vertex!
-        self.is_outdated: bool = True
-
-    def __gt__(self, other):
-        # ordering needed for priority queue. multiple vertices possibly have the same priority.
-        return id(self) > id(other)
-
-    def __str__(self):
-        return str(tuple(self.coordinates))
-
-    def __repr__(self):
-        return self.__str__()
-
-    def evaluate(self):
-        global origin
-        # store the coordinate value of the point relative to the new origin vector
-        self.coordinates_translated = self.coordinates - origin.coordinates
-        self.distance_to_origin = np.linalg.norm(self.coordinates_translated)
-        if self.distance_to_origin == 0.0:
-            # the coordinates of the origin and this vertex are equal
-            # an angle is not defined in this case!
-            self.angle_representation = None
-        else:
-            self.angle_representation = AngleRepresentation(self.coordinates_translated)
-
-        self.is_outdated = False
-
-    def get_coordinates_translated(self):
-        if self.is_outdated:
-            self.evaluate()
-        return self.coordinates_translated
-
-    def get_angle_representation(self):
-        if self.is_outdated:
-            self.evaluate()
-        try:
-            return self.angle_representation.value
-        except AttributeError:
-            return None
-
-    def get_distance_to_origin(self):
-        if self.is_outdated:
-            self.evaluate()
-        return self.distance_to_origin
-
-    def mark_outdated(self):
-        self.is_outdated = True
-
-
-def compute_angle_repr(coords_v1: np.ndarray, coords_v2: np.ndarray) -> Optional[float]:
-    diff_vect = coords_v2 - coords_v1
-    if np.all(diff_vect == 0.0):
-        return None
-    return compute_angle_repr_inner(diff_vect)
-
-
-def angle_rep_inverse(repr: Optional[float]) -> Optional[float]:
-    if repr is None:
-        repr_inv = None
-    else:
-        repr_inv = (repr + 2.0) % 4.0
-    return repr_inv
-
-
-def compute_extremity_idxs(coordinates: np.ndarray) -> List[int]:
-    """identify all protruding points = vertices with an inside angle of > 180 degree ('extremities')
-    expected edge numbering:
-        outer boundary polygon: counter clockwise
-        holes: clockwise
-
-    basic idea:
-      - translate the coordinate system to have p2 as origin
-      - compute the angle representations of both vectors representing the edges
-      - "rotate" the coordinate system (equal to deducting) so that the p1p2 representation is 0
-      - check in which quadrant the p2p3 representation lies
-    %4 because the quadrant has to be in [0,1,2,3] (representation in [0:4[)
-    if the representation lies within quadrant 0 or 1 (<2.0), the inside angle
-      (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
-    then p2 = extremity
-    :param coordinates:
-    :return:
-    """
-    nr_coordinates = len(coordinates)
-    extr_idxs = []
-    p1 = coordinates[-2]
-    p2 = coordinates[-1]
-    for i, p3 in enumerate(coordinates):
-        # since consequent vertices are not permitted to be equal,
-        #   the angle representation of the difference is well defined
-        diff_p3_p2 = p3 - p2
-        diff_p1_p2 = p1 - p2
-        rep_diff = compute_angle_repr_inner(diff_p3_p2) - compute_angle_repr_inner(diff_p1_p2)
-        if rep_diff % 4.0 < 2.0:  #
-            # p2 is an extremity
-            idx_p2 = (i - 1) % nr_coordinates
-            extr_idxs.append(idx_p2)
-
-        # move to the next point
-        p1 = p2
-        p2 = p3
-    return extr_idxs
-
 
 class SearchState(object):
     __slots__ = ["node", "distance", "neighbours", "path", "cost_so_far", "priority"]
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 694df56..9aada34 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,19 +1,13 @@
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
 from itertools import combinations
-from typing import Dict, Iterable, List, Set, Tuple
+from typing import Dict, Iterable, List, Optional, Set, Tuple
 
 import numpy as np
 import numpy.linalg
 
 from extremitypathfinder.configs import BOUNDARY_JSON_KEY, HOLES_JSON_KEY
-from extremitypathfinder.helper_classes import (
-    DirectedHeuristicGraph,
-    angle_rep_inverse,
-    compute_angle_repr,
-    compute_angle_repr_inner,
-    compute_repr_n_dist,
-)
+from extremitypathfinder.helper_classes import DirectedHeuristicGraph
 
 
 def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
@@ -34,8 +28,8 @@ def inside_polygon(x, y, coords, border_value):
     p = np.array([x, y])
     p1 = coords[-1, :]
     for p2 in coords[:]:
-        rep_p1_p = compute_angle_repr_inner(p1 - p)
-        rep_p2_p = compute_angle_repr_inner(p2 - p)
+        rep_p1_p, _ = compute_repr_n_dist(p1 - p)
+        rep_p2_p, _ = compute_repr_n_dist(p2 - p)
         if abs(rep_p1_p - rep_p2_p) == 2.0:
             return border_value
         p1 = p2
@@ -219,14 +213,17 @@ def check_polygon(polygon):
         raise ValueError("A polygon must not intersect itself.")
 
 
-# TODO test
-# todo - polygons must not intersect each other
 def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[np.ndarray]):
     """ensures that all the following conditions on the polygons are fulfilled:
         - basic polygon requirements (s. above)
         - edge numbering has to follow this convention (for easier computations):
             * outer boundary polygon: counter clockwise
             * holes: clockwise
+
+    # TODO test
+    # todo - polygons must not intersect each other
+    # TODO data rectification
+
     :param boundary_coords:
     :param list_hole_coords:
     :return:
@@ -239,35 +236,6 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
         if not has_clockwise_numbering(hole_coords):
             raise ValueError("Vertex numbering of hole polygon must be clockwise.")
 
-    # TODO data rectification
-
-
-# # TODO use caching variant: dict with tuple { (i1,i2): repr } <- Numba?!
-# def get_angle_representation(idx_origin: int, idx_v: int, repr_matrix: np.ndarray, coordinates: np.ndarray) -> float:
-#     repr = repr_matrix[idx_origin, idx_v]
-#
-#     # lazy initalisation: compute on demand only
-#     if np.isnan(repr):  # attention: repr == np.nan does not match!
-#         coords_origin = coordinates[idx_origin]
-#         coords_v = coordinates[idx_v]
-#         repr = compute_angle_repr(coords_origin, coords_v)
-#         repr_matrix[idx_origin, idx_v] = repr
-#
-#         # TODO not required. only triangle required?!
-#         # make use of symmetry: rotate 180 deg
-#         repr_matrix[idx_v, idx_origin] = angle_rep_inverse(repr)
-#
-#     # TODO
-#     assert repr is None or not np.isnan(repr)
-#     return repr
-
-
-def get_angle_repr(coords_origin: np.ndarray, coords_v: np.ndarray) -> float:
-    repr = compute_angle_repr(coords_origin, coords_v)
-    # TODO
-    assert repr is None or not np.isnan(repr)
-    return repr
-
 
 def find_within_range(
     vert_idx2repr: np.ndarray,
@@ -833,3 +801,106 @@ def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: boo
         hole_list.append(hole)
 
     return boundary_edges, hole_list
+
+
+def angle_rep_inverse(repr: Optional[float]) -> Optional[float]:
+    if repr is None:
+        repr_inv = None
+    else:
+        repr_inv = (repr + 2.0) % 4.0
+    return repr_inv
+
+
+def compute_extremity_idxs(coordinates: np.ndarray) -> List[int]:
+    """identify all protruding points = vertices with an inside angle of > 180 degree ('extremities')
+    expected edge numbering:
+        outer boundary polygon: counter clockwise
+        holes: clockwise
+
+    basic idea:
+      - translate the coordinate system to have p2 as origin
+      - compute the angle representations of both vectors representing the edges
+      - "rotate" the coordinate system (equal to deducting) so that the p1p2 representation is 0
+      - check in which quadrant the p2p3 representation lies
+    %4 because the quadrant has to be in [0,1,2,3] (representation in [0:4[)
+    if the representation lies within quadrant 0 or 1 (<2.0), the inside angle
+      (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree
+    then p2 = extremity
+    :param coordinates:
+    :return:
+    """
+    nr_coordinates = len(coordinates)
+    extr_idxs = []
+    p1 = coordinates[-2]
+    p2 = coordinates[-1]
+    for i, p3 in enumerate(coordinates):
+        # since consequent vertices are not permitted to be equal,
+        #   the angle representation of the difference is well defined
+        diff_p3_p2 = p3 - p2
+        diff_p1_p2 = p1 - p2
+        repr_p3_p2, _ = compute_repr_n_dist(diff_p3_p2)
+        repr_p1_p2, _ = compute_repr_n_dist(diff_p1_p2)
+        rep_diff = repr_p3_p2 - repr_p1_p2
+        if rep_diff % 4.0 < 2.0:  # inside angle > 180 degree
+            # p2 is an extremity
+            idx_p2 = (i - 1) % nr_coordinates
+            extr_idxs.append(idx_p2)
+
+        # move to the next point
+        p1 = p2
+        p2 = p3
+    return extr_idxs
+
+
+def compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]:
+    """computing representation for the angle from the origin to a given vector
+
+    value in [0.0 : 4.0[
+    every quadrant contains angle measures from 0.0 to 1.0
+    there are 4 quadrants (counter clockwise numbering)
+    0 / 360 degree -> 0.0
+    90 degree -> 1.0
+    180 degree -> 2.0
+    270 degree -> 3.0
+    ...
+    Useful for comparing angles without actually computing expensive trigonometrical functions
+    This representation does not grow directly proportional to its represented angle,
+    but it its bijective and monotonous:
+    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
+    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
+    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
+    with (0,0)' being the vector representing the origin
+
+    :param np_vector:
+    :return:
+    """
+    distance = np.linalg.norm(np_vector, ord=2)
+    if distance == 0.0:
+        angle_measure = np.nan
+    else:
+        # 2D vector: (dx, dy) = np_vector
+        dx, dy = np_vector
+        dx_positive = dx >= 0
+        dy_positive = dy >= 0
+
+        if dx_positive and dy_positive:
+            quadrant = 0.0
+            angle_measure = dy
+
+        elif not dx_positive and dy_positive:
+            quadrant = 1.0
+            angle_measure = -dx
+
+        elif not dx_positive and not dy_positive:
+            quadrant = 2.0
+            angle_measure = -dy
+
+        else:
+            quadrant = 3.0
+            angle_measure = dx
+
+        # normalise angle measure to [0; 1]
+        angle_measure /= distance
+        angle_measure += quadrant
+
+    return angle_measure, distance
diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index e43d694..75c4f07 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -19,7 +19,6 @@
 }
 
 SHOW_PLOTS = False
-# TODO avoid global variable
 PLOTTING_DIR = "all_plots"
 
 
diff --git a/tests/helper_classes_test.py b/tests/helper_classes_test.py
deleted file mode 100755
index 4fbb2a0..0000000
--- a/tests/helper_classes_test.py
+++ /dev/null
@@ -1,159 +0,0 @@
-import unittest
-
-import numpy as np
-import pytest
-from helpers import proto_test_case
-
-from extremitypathfinder import helper_classes
-from extremitypathfinder.helper_classes import AngleRepresentation, Polygon, Vertex
-
-helper_classes.origin = Vertex((-5.0, -5.0))
-
-
-class HelperClassesTest(unittest.TestCase):
-    def test_angle_repr(self):
-        with pytest.raises(ValueError):
-            AngleRepresentation(np.array([0.0, 0.0]))
-
-        #
-        # def quadrant_test_fct(input):
-        #     np_2D_coord_vector = np.array(input)
-        #     return AngleRepresentation(np_2D_coord_vector).quadrant
-        #
-        # data = [
-        #     ([1.0, 0.0], 0.0),
-        #     ([0.0, 1.0], 0.0),
-        #     ([-1.0, 0.0], 1.0),
-        #     ([0.0, -1.0], 3.0),
-        #
-        #     ([2.0, 0.0], 0.0),
-        #     ([0.0, 2.0], 0.0),
-        #     ([-2.0, 0.0], 1.0),
-        #     ([0.0, -2.0], 3.0),
-        #
-        #     ([1.0, 1.0], 0.0),
-        #     ([-1.0, 1.0], 1.0),
-        #     ([-1.0, -1.0], 2.0),
-        #     ([1.0, -1.0], 3.0),
-        #
-        #     ([1.0, 0.00001], 0.0),
-        #     ([0.00001, 1.0], 0.0),
-        #     ([-1.0, 0.00001], 1.0),
-        #     ([0.00001, -1.0], 3.0),
-        #
-        #     ([1.0, -0.00001], 3.0),
-        #     ([-0.00001, 1.0], 1.0),
-        #     ([-1.0, -0.00001], 2.0),
-        #     ([-0.00001, -1.0], 2.0),
-        # ]
-        #
-        # proto_test_case(data, quadrant_test_fct)
-
-        # TODO test:
-        # randomized
-        # every quadrant contains angle measures from 0.0 to 1.0
-        # angle %360!
-        #     rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
-        #     rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
-        # repr value in [0.0 : 4.0]
-
-        def value_test_fct(input):
-            np_2D_coord_vector = np.array(input)
-            return AngleRepresentation(np_2D_coord_vector).value
-
-        data = [
-            ([1.0, 0.0], 0.0),
-            ([0.0, 1.0], 1.0),
-            ([-1.0, 0.0], 2.0),
-            ([0.0, -1.0], 3.0),
-            ([2.0, 0.0], 0.0),
-            ([0.0, 2.0], 1.0),
-            ([-2.0, 0.0], 2.0),
-            ([0.0, -2.0], 3.0),
-        ]
-
-        proto_test_case(data, value_test_fct)
-
-    def test_vertex_translation(self):
-        data = [
-            ([1.0, 1.0], [6.0, 6.0]),
-            ([0.0, 0.0], [5.0, 5.0]),
-            ([-12.0, -3.0], [-7.0, 2.0]),
-            ([-4.0, 5.0], [1.0, 10.0]),
-            ([3.0, -2.0], [8.0, 3.0]),
-        ]
-
-        def translation_test_fct(input):
-            return Vertex(input).get_coordinates_translated().tolist()
-
-        proto_test_case(data, translation_test_fct)
-
-    def test_vertex_angle_repr(self):
-        data = [
-            ([0.0, -5.0], 0.0),
-            ([-5.0, 0.0], 1.0),
-            ([-6.0, -5.0], 2.0),
-            ([-5.0, -6.0], 3.0),
-        ]
-
-        def angle_repr_test_fct(input):
-            return Vertex(input).get_angle_representation()
-
-        proto_test_case(data, angle_repr_test_fct)
-
-    def test_vertex_distance_to_origin(self):
-        data = [
-            ([0.0, 0.0], np.sqrt(50)),
-            ([-5.0, 0.0], 5.0),
-            ([0.0, -5], 5.0),
-            ([-3.0, -2.0], np.sqrt(13)),
-            ([2.0, 5.0], np.sqrt(149)),
-        ]
-
-        def dist_to_origin_test_fct(input):
-            return Vertex(input).get_distance_to_origin()
-
-        proto_test_case(data, dist_to_origin_test_fct)
-
-    def test_polygon_bad_input(self):
-        with pytest.raises(ValueError) as err:
-            Polygon([(0, 0), (1, 1)], is_hole=False)
-        assert "not a valid polygon" in str(err)
-
-    def test_polygon_extremities(self):
-        data = [
-            (
-                [
-                    (0, 0),
-                    (10, 0),
-                    (9, 5),
-                    (10, 10),
-                    (0, 10),
-                ],
-                [(9, 5)],
-            ),
-            (
-                [
-                    (0, 0),
-                    (-2, -2),
-                    (-3, -2.5),
-                    (-3, -4),
-                    (2, -3),
-                    (1, 2.5),
-                    (0, -1),
-                ],
-                [(0, -1), (-2, -2)],
-            ),
-        ]
-
-        def find_extremities_test_fct(input):
-            extremities = Polygon(input, is_hole=False).extremities
-            return [tuple(e.coordinates) for e in extremities]
-
-        proto_test_case(data, find_extremities_test_fct)
-
-
-if __name__ == "__main__":
-    suite = unittest.TestLoader().loadTestsFromTestCase(HelperClassesTest)
-    unittest.TextTestRunner(verbosity=2).run(suite)
-    # unittest.main()
diff --git a/tests/helper_fcts_test.py b/tests/helper_fcts_test.py
index 04342e6..1baa49d 100755
--- a/tests/helper_fcts_test.py
+++ b/tests/helper_fcts_test.py
@@ -1,4 +1,8 @@
-import unittest
+"""
+TODO test find_visible(), ...
+TODO test if relation is really bidirectional (y in find_visible(x,y) <=> x in find_visible(y,x))
+TODO test input data validation
+"""
 from os.path import abspath, join, pardir
 from typing import Dict, Set
 
@@ -7,115 +11,83 @@
 from helpers import proto_test_case
 
 from extremitypathfinder import PolygonEnvironment
-from extremitypathfinder.helper_classes import AngleRepresentation
-from extremitypathfinder.helper_fcts import clean_visibles, has_clockwise_numbering, inside_polygon, read_json
-
+from extremitypathfinder.helper_fcts import (
+    clean_visibles,
+    compute_extremity_idxs,
+    compute_repr_n_dist,
+    has_clockwise_numbering,
+    inside_polygon,
+    read_json,
+)
 
-# TODO test find_visible(), ...
-# TODO test invalid data detection
-class HelperFctsTest(unittest.TestCase):
-    def value_test_fct(input):
-        np_2D_coord_vector = np.array(input)
-        return AngleRepresentation(np_2D_coord_vector).value
 
-    data = [
-        ([1.0, 0.0], 0.0),
-        ([0.0, 1.0], 1.0),
-        ([-1.0, 0.0], 2.0),
-        ([0.0, -1.0], 3.0),
-        ([2.0, 0.0], 0.0),
-        ([0.0, 2.0], 1.0),
-        ([-2.0, 0.0], 2.0),
-        ([0.0, -2.0], 3.0),
-    ]
-
-    proto_test_case(data, value_test_fct)
-
-    def test_inside_polygon(self):
-        # TODO more detailed test. edges with slopes... also in timezonefinder
-
-        for border_value in [True, False]:
-
-            def test_fct(input):
-                polygon_test_case = np.array([(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)])
-                x, y = input
-
-                return inside_polygon(x, y, polygon_test_case, border_value)
-
-            p_test_cases = [
-                # (x,y),
-                # inside
-                (0.0, 0.0),
-                # # outside
-                (-2.0, 2.0),
-                (0, 2.0),
-                (2.0, 2.0),
-                (-2.0, 0),
-                (2.0, 0),
-                (-2.0, -2.0),
-                (0, -2.0),
-                (2.0, -2.0),
-                # on the line test cases
-                (-1.0, -1.0),
-                (1.0, -1.0),
-                (1.0, 1.0),
-                (-1.0, 1.0),
-                (0.0, 1),
-                (0, -1),
-                (1, 0),
-                (-1, 0),
-            ]
-            expected_results = [
-                True,
-                False,
-                False,
-                False,
-                False,
-                False,
-                False,
-                False,
-                False,
-                # on the line test cases
-                border_value,
-                border_value,
-                border_value,
-                border_value,
-                border_value,
-                border_value,
-                border_value,
-                border_value,
-            ]
-
-            proto_test_case(list(zip(p_test_cases, expected_results)), test_fct)
-
-    def test_clockwise_numering(self):
-        def clockwise_test_fct(input):
-            return has_clockwise_numbering(np.array(input))
-
-        data = [
-            # clockwise numbering!
-            ([(3.0, 7.0), (5.0, 9.0), (5.0, 7.0)], True),
-            ([(3.0, 7.0), (5.0, 9.0), (4.5, 7.0), (5.0, 4.0)], True),
-            ([(0.0, 0.0), (0.0, 1.0), (1.0, 0.0)], True),
-            # # counter clockwise edge numbering!
-            ([(0.0, 0.0), (1.0, 0.0), (0.0, 1.0)], False),
-            ([(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)], False),
-            ([(0.0, 0.0), (10.0, 0.0), (10.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False),
-            ([(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False),
+def test_inside_polygon():
+    for border_value in [True, False]:
+
+        def test_fct(input):
+            polygon_test_case = np.array([(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)])
+            x, y = input
+
+            return inside_polygon(x, y, polygon_test_case, border_value)
+
+        p_test_cases = [
+            # (x,y),
+            # inside
+            (0.0, 0.0),
+            # # outside
+            (-2.0, 2.0),
+            (0, 2.0),
+            (2.0, 2.0),
+            (-2.0, 0),
+            (2.0, 0),
+            (-2.0, -2.0),
+            (0, -2.0),
+            (2.0, -2.0),
+            # on the line test cases
+            (-1.0, -1.0),
+            (1.0, -1.0),
+            (1.0, 1.0),
+            (-1.0, 1.0),
+            (0.0, 1),
+            (0, -1),
+            (1, 0),
+            (-1, 0),
+        ]
+        expected_results = [
+            True,
+            False,
+            False,
+            False,
+            False,
+            False,
+            False,
+            False,
+            False,
+            # on the line test cases
+            border_value,
+            border_value,
+            border_value,
+            border_value,
+            border_value,
+            border_value,
+            border_value,
+            border_value,
         ]
-        proto_test_case(data, clockwise_test_fct)
 
-    def test_read_json(self):
-        path2json_file = abspath(join(__file__, pardir, pardir, "example.json"))
-        boundary_coordinates, list_of_holes = read_json(path2json_file)
-        assert len(boundary_coordinates) == 5
-        assert len(boundary_coordinates[0]) == 2
-        assert len(list_of_holes) == 2
-        first_hole = list_of_holes[0]
-        assert len(first_hole) == 4
-        assert len(first_hole[0]) == 2
-        environment = PolygonEnvironment()
-        environment.store(boundary_coordinates, list_of_holes, validate=True)
+        proto_test_case(list(zip(p_test_cases, expected_results)), test_fct)
+
+
+def test_read_json():
+    path2json_file = abspath(join(__file__, pardir, pardir, "example.json"))
+    boundary_coordinates, list_of_holes = read_json(path2json_file)
+    assert len(boundary_coordinates) == 5
+    assert len(boundary_coordinates[0]) == 2
+    assert len(list_of_holes) == 2
+    first_hole = list_of_holes[0]
+    assert len(first_hole) == 4
+    assert len(first_hole[0]) == 2
+    environment = PolygonEnvironment()
+    environment.store(boundary_coordinates, list_of_holes, validate=True)
 
 
 @pytest.mark.parametrize(
@@ -141,4 +113,139 @@ def test_clean_visible_idxs(
     assert res == expected
 
 
-# TODO test if relation is really bidirectional (y in find_visible(x,y) <=> x in find_visible(y,x))
+@pytest.mark.parametrize(
+    "coords, expected",
+    [
+        (
+            [
+                (0, 0),
+                (10, 0),
+                (9, 5),
+                (10, 10),
+                (0, 10),
+            ],
+            {2},
+        ),
+        (
+            [
+                (0, 0),
+                (10, 0),
+                (10, 10),
+                (0, 10),
+            ],
+            set(),
+        ),
+        (
+            [
+                (0, 0),
+                (-2, -2),
+                (-3, -2.5),
+                (-3, -4),
+                (2, -3),
+                (1, 2.5),
+                (0, -1),
+            ],
+            {6, 1},
+        ),
+    ],
+)
+def test_compute_extremity_idxs(coords, expected):
+    coords = np.array(coords)
+    res = compute_extremity_idxs(coords)
+    assert set(res) == expected
+
+
+@pytest.mark.parametrize(
+    "input, expected",
+    [
+        # clockwise numbering!
+        ([(3.0, 7.0), (5.0, 9.0), (5.0, 7.0)], True),
+        ([(3.0, 7.0), (5.0, 9.0), (4.5, 7.0), (5.0, 4.0)], True),
+        ([(0.0, 0.0), (0.0, 1.0), (1.0, 0.0)], True),
+        # # counter clockwise edge numbering!
+        ([(0.0, 0.0), (1.0, 0.0), (0.0, 1.0)], False),
+        ([(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)], False),
+        ([(0.0, 0.0), (10.0, 0.0), (10.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False),
+        ([(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False),
+    ],
+)
+def test_clockwise_numering(input, expected):
+    def clockwise_test_fct(input):
+        return has_clockwise_numbering(np.array(input))
+
+    assert clockwise_test_fct(input) == expected
+
+
+@pytest.mark.parametrize(
+    "input, expected",
+    [
+        ([0.0, -5.0], (3.0, 5.0)),
+        ([-5.0, 0.0], (2.0, 5.0)),
+        ([-1.0, 0.0], (2.0, 1.0)),
+        ([1.0, 0.0], (0.0, 1.0)),
+        ([0.0, 1.0], (1.0, 1.0)),
+        ([-6.0, -5.0], (2.64018439966448, 7.810249675906654)),
+        ([-5.0, -6.0], (2.768221279597376, 7.810249675906654)),
+    ],
+)
+def test_compute_repr_n_dist(input, expected):
+    def test_fct(input):
+        return compute_repr_n_dist(np.array(input))
+
+    assert test_fct(input) == expected
+
+
+@pytest.mark.parametrize(
+    "input, expected",
+    [
+        ([1.0, 0.0], 0.0),
+        ([0.0, 1.0], 1.0),
+        ([-1.0, 0.0], 2.0),
+        ([0.0, -1.0], 3.0),
+        ([2.0, 0.0], 0.0),
+        ([0.0, 2.0], 1.0),
+        ([-2.0, 0.0], 2.0),
+        ([0.0, -2.0], 3.0),
+    ],
+)
+def test_angle_representation(input, expected):
+    def func(input):
+        repr, dist = compute_repr_n_dist(np.array(input))
+        return repr
+
+    assert func(input) == expected
+
+
+@pytest.mark.parametrize(
+    "input, expected",
+    [
+        ([1.0, 0.0], 0.0),
+        ([0.0, 1.0], 1.0),
+        ([-1.0, 0.0], 2.0),
+        ([0.0, -1.0], 3.0),
+        ([2.0, 0.0], 0.0),
+        ([0.0, 2.0], 1.0),
+        ([-2.0, 0.0], 2.0),
+        ([0.0, -2.0], 3.0),
+        ([1.0, 1.0], 0.0),
+        ([-1.0, 1.0], 1.0),
+        ([-1.0, -1.0], 2.0),
+        ([1.0, -1.0], 3.0),
+        ([1.0, 0.00001], 0.0),
+        ([0.00001, 1.0], 0.0),
+        ([-1.0, 0.00001], 1.0),
+        ([0.00001, -1.0], 3.0),
+        ([1.0, -0.00001], 3.0),
+        ([-0.00001, 1.0], 1.0),
+        ([-1.0, -0.00001], 2.0),
+        ([-0.00001, -1.0], 2.0),
+    ],
+)
+def test_angle_repr_quadrant(input, expected):
+    def func(input):
+        repr, dist = compute_repr_n_dist(np.array(input))
+        return repr
+
+    res = func(input)
+    assert res >= expected
+    assert res < expected + 1

From e566de5015043e25f6fb1e268b84ae8bac8d9507 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 18:40:12 +0200
Subject: [PATCH 35/44] misc refactoring

---
 CHANGELOG.rst                              |   1 +
 extremitypathfinder/extremitypathfinder.py |   4 +-
 extremitypathfinder/helper_fcts.py         | 145 +++++----
 poetry.lock                                | 353 +++++----------------
 pyproject.toml                             |   6 +
 tests/helper_fcts_test.py                  |   5 +-
 6 files changed, 174 insertions(+), 340 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 57ad3a4..6278668 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -3,6 +3,7 @@ Changelog
 
 
 TODO pending major release: remove separate prepare step?! initialise in one step during initialisation
+TODO Numba JIT compilation of utils
 
 
 2.3.0 (2022-xx)
diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 4cb8305..7377746 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -232,8 +232,8 @@ def within_map(self, coords: InputCoords):
         """
         boundary = self.boundary_polygon
         holes = self.holes
-        x, y = coords
-        return is_within_map(x, y, boundary, holes)
+        p = np.array(coords, dtype=float)
+        return is_within_map(p, boundary, holes)
 
     def find_shortest_path(
         self,
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 9aada34..742f74b 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,5 +1,6 @@
 # TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
+import math
 from itertools import combinations
 from typing import Dict, Iterable, List, Optional, Set, Tuple
 
@@ -10,6 +11,60 @@
 from extremitypathfinder.helper_classes import DirectedHeuristicGraph
 
 
+def compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]:
+    """computing representation for the angle from the origin to a given vector
+
+    value in [0.0 : 4.0[
+    every quadrant contains angle measures from 0.0 to 1.0
+    there are 4 quadrants (counter clockwise numbering)
+    0 / 360 degree -> 0.0
+    90 degree -> 1.0
+    180 degree -> 2.0
+    270 degree -> 3.0
+    ...
+    Useful for comparing angles without actually computing expensive trigonometrical functions
+    This representation does not grow directly proportional to its represented angle,
+    but it its bijective and monotonous:
+    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
+    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
+    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
+    with (0,0)' being the vector representing the origin
+
+    :param np_vector:
+    :return:
+    """
+    dx, dy = np_vector
+    distance = math.sqrt(dx**2 + dy**2)  # l-2 norm
+    if distance == 0.0:
+        angle_measure = np.nan
+    else:
+        # 2D vector: (dx, dy) = np_vector
+        dx_positive = dx >= 0
+        dy_positive = dy >= 0
+
+        if dx_positive and dy_positive:
+            quadrant = 0.0
+            angle_measure = dy
+
+        elif not dx_positive and dy_positive:
+            quadrant = 1.0
+            angle_measure = -dx
+
+        elif not dx_positive and not dy_positive:
+            quadrant = 2.0
+            angle_measure = -dy
+
+        else:
+            quadrant = 3.0
+            angle_measure = dx
+
+        # normalise angle measure to [0; 1]
+        angle_measure /= distance
+        angle_measure += quadrant
+
+    return angle_measure, distance
+
+
 def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
     coords_orig = coords[orig_idx]
     coords_translated = coords - coords_orig
@@ -17,15 +72,15 @@ def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
     return repr_n_dists.T
 
 
-def inside_polygon(x, y, coords, border_value):
+def inside_polygon(p: np.ndarray, coords: np.ndarray, border_value: bool) -> bool:
     # should return the border value for point equal to any polygon vertex
     # TODO overflow possible with large values when comparing slopes, change procedure
+    x, y = p
     for c in coords[:]:
-        if np.all(c == [x, y]):
+        if np.array_equal(c, p):
             return border_value
 
     # and if the point p lies on any polygon edge
-    p = np.array([x, y])
     p1 = coords[-1, :]
     for p2 in coords[:]:
         rep_p1_p, _ = compute_repr_n_dist(p1 - p)
@@ -72,11 +127,11 @@ def inside_polygon(x, y, coords, border_value):
     return contained
 
 
-def is_within_map(x, y, boundary, holes):
-    if not inside_polygon(x, y, boundary, border_value=True):
+def is_within_map(p: np.ndarray, boundary: np.ndarray, holes: Iterable[np.ndarray]) -> bool:
+    if not inside_polygon(p, boundary, border_value=True):
         return False
     for hole in holes:
-        if inside_polygon(x, y, hole, border_value=False):
+        if inside_polygon(p, hole, border_value=False):
             return False
     return True
 
@@ -85,7 +140,7 @@ def no_identical_consequent_vertices(coords):
     p1 = coords[-1]
     for p2 in coords:
         # TODO adjust allowed difference: rtol, atol
-        if np.allclose(p1, p2):
+        if np.array_equal(p1, p2):
             return False
         p1 = p2
 
@@ -179,7 +234,7 @@ def no_self_intersection(coords):
     return True
 
 
-def has_clockwise_numbering(coords):
+def has_clockwise_numbering(coords: np.ndarray) -> bool:
     """tests if a polygon has clockwise vertex numbering
     approach: Sum over the edges, (x2 − x1)(y2 + y1). If the result is positive the curve is clockwise.
     from:
@@ -762,7 +817,7 @@ def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: boo
         if cntr_clockwise_wanted:
             # make edge numbering counter clockwise!
             edge_list.reverse()
-        return np.array(edge_list)
+        return np.array(edge_list, dtype=float)
 
     # build the boundary polygon
     # start at the lowest and leftmost unblocked grid cell
@@ -779,9 +834,17 @@ def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: boo
     # just the obstacles inside the boundary polygon are part of holes
     # shift coordinates by +(0.5,0.5) for correct detection
     # the border value does not matter here
-    unchecked_obstacles = [
-        o for o in obstacle_iter if inside_polygon(o[0] + 0.5, o[1] + 0.5, boundary_edges, border_value=True)
-    ]
+
+    def get_unchecked_obstacles(obstacles: Iterable, poly: np.ndarray, required_val: bool = True) -> List:
+        unchecked_obstacles = []
+        for o in obstacles:
+            p = o + 0.5
+            if inside_polygon(p, poly, border_value=True) == required_val:
+                unchecked_obstacles.append(o)
+
+        return unchecked_obstacles
+
+    unchecked_obstacles = get_unchecked_obstacles(obstacle_iter, boundary_edges)
 
     hole_list = []
     while len(unchecked_obstacles) > 0:
@@ -790,9 +853,7 @@ def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: boo
 
         # detect which of the obstacles still do not belong to any hole:
         # delete the obstacles which are included in the just constructed hole
-        unchecked_obstacles = [
-            o for o in unchecked_obstacles if not inside_polygon(o[0] + 0.5, o[1] + 0.5, hole, border_value=True)
-        ]
+        unchecked_obstacles = get_unchecked_obstacles(unchecked_obstacles, hole, required_val=False)
 
         if simplify:
             # TODO
@@ -850,57 +911,3 @@ def compute_extremity_idxs(coordinates: np.ndarray) -> List[int]:
         p1 = p2
         p2 = p3
     return extr_idxs
-
-
-def compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]:
-    """computing representation for the angle from the origin to a given vector
-
-    value in [0.0 : 4.0[
-    every quadrant contains angle measures from 0.0 to 1.0
-    there are 4 quadrants (counter clockwise numbering)
-    0 / 360 degree -> 0.0
-    90 degree -> 1.0
-    180 degree -> 2.0
-    270 degree -> 3.0
-    ...
-    Useful for comparing angles without actually computing expensive trigonometrical functions
-    This representation does not grow directly proportional to its represented angle,
-    but it its bijective and monotonous:
-    rep(p1) > rep(p2) <=> angle(p1) > angle(p2)
-    rep(p1) = rep(p2) <=> angle(p1) = angle(p2)
-    angle(p): counter clockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p
-    with (0,0)' being the vector representing the origin
-
-    :param np_vector:
-    :return:
-    """
-    distance = np.linalg.norm(np_vector, ord=2)
-    if distance == 0.0:
-        angle_measure = np.nan
-    else:
-        # 2D vector: (dx, dy) = np_vector
-        dx, dy = np_vector
-        dx_positive = dx >= 0
-        dy_positive = dy >= 0
-
-        if dx_positive and dy_positive:
-            quadrant = 0.0
-            angle_measure = dy
-
-        elif not dx_positive and dy_positive:
-            quadrant = 1.0
-            angle_measure = -dx
-
-        elif not dx_positive and not dy_positive:
-            quadrant = 2.0
-            angle_measure = -dy
-
-        else:
-            quadrant = 3.0
-            angle_measure = dx
-
-        # normalise angle measure to [0; 1]
-        angle_measure /= distance
-        angle_measure += quadrant
-
-    return angle_measure, distance
diff --git a/poetry.lock b/poetry.lock
index 83247b6..7943694 100644
--- a/poetry.lock
+++ b/poetry.lock
@@ -16,17 +16,17 @@ python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
 
 [[package]]
 name = "attrs"
-version = "21.4.0"
+version = "22.1.0"
 description = "Classes Without Boilerplate"
 category = "dev"
 optional = false
-python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*, !=3.4.*"
+python-versions = ">=3.5"
 
 [package.extras]
-dev = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "six", "mypy", "pytest-mypy-plugins", "zope.interface", "furo", "sphinx", "sphinx-notfound-page", "pre-commit", "cloudpickle"]
+dev = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "mypy (>=0.900,!=0.940)", "pytest-mypy-plugins", "zope.interface", "furo", "sphinx", "sphinx-notfound-page", "pre-commit", "cloudpickle"]
 docs = ["furo", "sphinx", "zope.interface", "sphinx-notfound-page"]
-tests = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "six", "mypy", "pytest-mypy-plugins", "zope.interface", "cloudpickle"]
-tests_no_zope = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "six", "mypy", "pytest-mypy-plugins", "cloudpickle"]
+tests = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "mypy (>=0.900,!=0.940)", "pytest-mypy-plugins", "zope.interface", "cloudpickle"]
+tests_no_zope = ["coverage[toml] (>=5.0.2)", "hypothesis", "pympler", "pytest (>=4.3.0)", "mypy (>=0.900,!=0.940)", "pytest-mypy-plugins", "cloudpickle"]
 
 [[package]]
 name = "babel"
@@ -84,7 +84,7 @@ python-versions = ">=3.6"
 
 [[package]]
 name = "distlib"
-version = "0.3.4"
+version = "0.3.5"
 description = "Distribution utilities"
 category = "dev"
 optional = false
@@ -100,19 +100,19 @@ python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*, !=3.4.*"
 
 [[package]]
 name = "filelock"
-version = "3.7.1"
+version = "3.8.0"
 description = "A platform independent file lock."
 category = "dev"
 optional = false
 python-versions = ">=3.7"
 
 [package.extras]
-docs = ["furo (>=2021.8.17b43)", "sphinx (>=4.1)", "sphinx-autodoc-typehints (>=1.12)"]
-testing = ["covdefaults (>=1.2.0)", "coverage (>=4)", "pytest (>=4)", "pytest-cov", "pytest-timeout (>=1.4.2)"]
+docs = ["furo (>=2022.6.21)", "sphinx (>=5.1.1)", "sphinx-autodoc-typehints (>=1.19.1)"]
+testing = ["covdefaults (>=2.2)", "coverage (>=6.4.2)", "pytest (>=7.1.2)", "pytest-cov (>=3)", "pytest-timeout (>=2.1)"]
 
 [[package]]
 name = "fonttools"
-version = "4.34.4"
+version = "4.36.0"
 description = "Tools to manipulate font files"
 category = "dev"
 optional = false
@@ -134,7 +134,7 @@ woff = ["zopfli (>=0.1.4)", "brotlicffi (>=0.8.0)", "brotli (>=1.0.1)"]
 
 [[package]]
 name = "identify"
-version = "2.5.1"
+version = "2.5.3"
 description = "File identification library for Python"
 category = "dev"
 optional = false
@@ -199,7 +199,7 @@ i18n = ["Babel (>=2.7)"]
 
 [[package]]
 name = "kiwisolver"
-version = "1.4.3"
+version = "1.4.4"
 description = "A fast implementation of the Cassowary constraint solver"
 category = "dev"
 optional = false
@@ -215,7 +215,7 @@ python-versions = ">=3.7"
 
 [[package]]
 name = "matplotlib"
-version = "3.5.2"
+version = "3.5.3"
 description = "Python plotting package"
 category = "dev"
 optional = false
@@ -230,7 +230,7 @@ packaging = ">=20.0"
 pillow = ">=6.2.0"
 pyparsing = ">=2.2.1"
 python-dateutil = ">=2.7"
-setuptools_scm = ">=4"
+setuptools_scm = ">=4,<7"
 
 [[package]]
 name = "nodeenv"
@@ -242,7 +242,7 @@ python-versions = ">=2.7,!=3.0.*,!=3.1.*,!=3.2.*,!=3.3.*,!=3.4.*,!=3.5.*,!=3.6.*
 
 [[package]]
 name = "numpy"
-version = "1.23.1"
+version = "1.23.2"
 description = "NumPy is the fundamental package for array computing with Python."
 category = "main"
 optional = false
@@ -292,12 +292,12 @@ optional = false
 python-versions = ">=3.6"
 
 [package.extras]
-dev = ["pre-commit", "tox"]
-testing = ["pytest", "pytest-benchmark"]
+testing = ["pytest-benchmark", "pytest"]
+dev = ["tox", "pre-commit"]
 
 [[package]]
 name = "pre-commit"
-version = "2.19.0"
+version = "2.20.0"
 description = "A framework for managing and maintaining multi-language pre-commit hooks."
 category = "dev"
 optional = false
@@ -321,12 +321,15 @@ python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*, !=3.4.*"
 
 [[package]]
 name = "pygments"
-version = "2.12.0"
+version = "2.13.0"
 description = "Pygments is a syntax highlighting package written in Python."
 category = "dev"
 optional = false
 python-versions = ">=3.6"
 
+[package.extras]
+plugins = ["importlib-metadata"]
+
 [[package]]
 name = "pyparsing"
 version = "3.0.9"
@@ -372,7 +375,7 @@ six = ">=1.5"
 
 [[package]]
 name = "pytz"
-version = "2022.1"
+version = "2022.2.1"
 description = "World timezone definitions, modern and historical"
 category = "dev"
 optional = false
@@ -406,16 +409,15 @@ use_chardet_on_py3 = ["chardet (>=3.0.2,<6)"]
 
 [[package]]
 name = "setuptools-scm"
-version = "7.0.4"
+version = "6.4.2"
 description = "the blessed package to manage your versions by scm tags"
 category = "dev"
 optional = false
-python-versions = ">=3.7"
+python-versions = ">=3.6"
 
 [package.dependencies]
 packaging = ">=20.0"
 tomli = ">=1.0.0"
-typing-extensions = "*"
 
 [package.extras]
 test = ["pytest (>=6.2)", "virtualenv (>20)"]
@@ -482,7 +484,7 @@ docutils = "<0.18"
 sphinx = ">=1.6"
 
 [package.extras]
-dev = ["transifex-client", "sphinxcontrib-httpdomain", "bump2version"]
+dev = ["bump2version", "sphinxcontrib-httpdomain", "transifex-client"]
 
 [[package]]
 name = "sphinxcontrib-applehelp"
@@ -493,8 +495,8 @@ optional = false
 python-versions = ">=3.5"
 
 [package.extras]
-lint = ["flake8", "mypy", "docutils-stubs"]
 test = ["pytest"]
+lint = ["docutils-stubs", "mypy", "flake8"]
 
 [[package]]
 name = "sphinxcontrib-devhelp"
@@ -505,8 +507,8 @@ optional = false
 python-versions = ">=3.5"
 
 [package.extras]
-lint = ["flake8", "mypy", "docutils-stubs"]
 test = ["pytest"]
+lint = ["docutils-stubs", "mypy", "flake8"]
 
 [[package]]
 name = "sphinxcontrib-htmlhelp"
@@ -517,8 +519,8 @@ optional = false
 python-versions = ">=3.6"
 
 [package.extras]
-lint = ["flake8", "mypy", "docutils-stubs"]
-test = ["pytest", "html5lib"]
+test = ["html5lib", "pytest"]
+lint = ["docutils-stubs", "mypy", "flake8"]
 
 [[package]]
 name = "sphinxcontrib-jsmath"
@@ -529,7 +531,7 @@ optional = false
 python-versions = ">=3.5"
 
 [package.extras]
-test = ["pytest", "flake8", "mypy"]
+test = ["mypy", "flake8", "pytest"]
 
 [[package]]
 name = "sphinxcontrib-qthelp"
@@ -540,8 +542,8 @@ optional = false
 python-versions = ">=3.5"
 
 [package.extras]
-lint = ["flake8", "mypy", "docutils-stubs"]
 test = ["pytest"]
+lint = ["docutils-stubs", "mypy", "flake8"]
 
 [[package]]
 name = "sphinxcontrib-serializinghtml"
@@ -552,8 +554,8 @@ optional = false
 python-versions = ">=3.5"
 
 [package.extras]
-lint = ["flake8", "mypy", "docutils-stubs"]
 test = ["pytest"]
+lint = ["docutils-stubs", "mypy", "flake8"]
 
 [[package]]
 name = "toml"
@@ -593,17 +595,9 @@ virtualenv = ">=16.0.0,<20.0.0 || >20.0.0,<20.0.1 || >20.0.1,<20.0.2 || >20.0.2,
 docs = ["pygments-github-lexers (>=0.0.5)", "sphinx (>=2.0.0)", "sphinxcontrib-autoprogram (>=0.1.5)", "towncrier (>=18.5.0)"]
 testing = ["flaky (>=3.4.0)", "freezegun (>=0.3.11)", "pytest (>=4.0.0)", "pytest-cov (>=2.5.1)", "pytest-mock (>=1.10.0)", "pytest-randomly (>=1.0.0)", "psutil (>=5.6.1)", "pathlib2 (>=2.3.3)"]
 
-[[package]]
-name = "typing-extensions"
-version = "4.3.0"
-description = "Backported and Experimental Type Hints for Python 3.7+"
-category = "dev"
-optional = false
-python-versions = ">=3.7"
-
 [[package]]
 name = "urllib3"
-version = "1.26.10"
+version = "1.26.11"
 description = "HTTP library with thread-safe connection pooling, file post, and more."
 category = "dev"
 optional = false
@@ -616,269 +610,96 @@ socks = ["PySocks (>=1.5.6,!=1.5.7,<2.0)"]
 
 [[package]]
 name = "virtualenv"
-version = "20.15.1"
+version = "20.16.3"
 description = "Virtual Python Environment builder"
 category = "dev"
 optional = false
-python-versions = "!=3.0.*,!=3.1.*,!=3.2.*,!=3.3.*,!=3.4.*,>=2.7"
+python-versions = ">=3.6"
 
 [package.dependencies]
-distlib = ">=0.3.1,<1"
-filelock = ">=3.2,<4"
-platformdirs = ">=2,<3"
-six = ">=1.9.0,<2"
+distlib = ">=0.3.5,<1"
+filelock = ">=3.4.1,<4"
+platformdirs = ">=2.4,<3"
 
 [package.extras]
-docs = ["proselint (>=0.10.2)", "sphinx (>=3)", "sphinx-argparse (>=0.2.5)", "sphinx-rtd-theme (>=0.4.3)", "towncrier (>=21.3)"]
-testing = ["coverage (>=4)", "coverage-enable-subprocess (>=1)", "flaky (>=3)", "pytest (>=4)", "pytest-env (>=0.6.2)", "pytest-freezegun (>=0.4.1)", "pytest-mock (>=2)", "pytest-randomly (>=1)", "pytest-timeout (>=1)", "packaging (>=20.0)"]
+docs = ["proselint (>=0.13)", "sphinx (>=5.1.1)", "sphinx-argparse (>=0.3.1)", "sphinx-rtd-theme (>=1)", "towncrier (>=21.9)"]
+testing = ["coverage (>=6.2)", "coverage-enable-subprocess (>=1)", "flaky (>=3.7)", "packaging (>=21.3)", "pytest (>=7.0.1)", "pytest-env (>=0.6.2)", "pytest-freezegun (>=0.4.2)", "pytest-mock (>=3.6.1)", "pytest-randomly (>=3.10.3)", "pytest-timeout (>=2.1)"]
 
 [[package]]
 name = "zipp"
-version = "3.8.0"
+version = "3.8.1"
 description = "Backport of pathlib-compatible object wrapper for zip files"
 category = "dev"
 optional = false
 python-versions = ">=3.7"
 
 [package.extras]
-docs = ["sphinx", "jaraco.packaging (>=9)", "rst.linker (>=1.9)"]
-testing = ["pytest (>=6)", "pytest-checkdocs (>=2.4)", "pytest-flake8", "pytest-cov", "pytest-enabler (>=1.0.1)", "jaraco.itertools", "func-timeout", "pytest-black (>=0.3.7)", "pytest-mypy (>=0.9.1)"]
+docs = ["sphinx", "jaraco.packaging (>=9)", "rst.linker (>=1.9)", "jaraco.tidelift (>=1.4)"]
+testing = ["pytest (>=6)", "pytest-checkdocs (>=2.4)", "pytest-flake8", "pytest-cov", "pytest-enabler (>=1.3)", "jaraco.itertools", "func-timeout", "pytest-black (>=0.3.7)", "pytest-mypy (>=0.9.1)"]
+
+[extras]
+docs = []
+numba = []
+plot = []
+test = []
 
 [metadata]
 lock-version = "1.1"
 python-versions = "^3.8"
-content-hash = "16ab09eb70efbe3fc8d946786d03da08acc22c6a1fc2101838e6f04ea27a4c73"
+content-hash = "b17494c876d4a16b89fcbd219cd9c8b5c5eb2b1af590f6db5bf40add8a826263"
 
 [metadata.files]
-alabaster = [
-    {file = "alabaster-0.7.12-py2.py3-none-any.whl", hash = "sha256:446438bdcca0e05bd45ea2de1668c1d9b032e1a9154c2c259092d77031ddd359"},
-    {file = "alabaster-0.7.12.tar.gz", hash = "sha256:a661d72d58e6ea8a57f7a86e37d86716863ee5e92788398526d58b26a4e4dc02"},
-]
+alabaster = []
 atomicwrites = []
-attrs = [
-    {file = "attrs-21.4.0-py2.py3-none-any.whl", hash = "sha256:2d27e3784d7a565d36ab851fe94887c5eccd6a463168875832a1be79c82828b4"},
-    {file = "attrs-21.4.0.tar.gz", hash = "sha256:626ba8234211db98e869df76230a137c4c40a12d72445c45d5f5b716f076e2fd"},
-]
+attrs = []
 babel = []
-certifi = [
-    {file = "certifi-2022.6.15-py3-none-any.whl", hash = "sha256:fe86415d55e84719d75f8b69414f6438ac3547d2078ab91b67e779ef69378412"},
-    {file = "certifi-2022.6.15.tar.gz", hash = "sha256:84c85a9078b11105f04f3036a9482ae10e4621616db313fe045dd24743a0820d"},
-]
-cfgv = [
-    {file = "cfgv-3.3.1-py2.py3-none-any.whl", hash = "sha256:c6a0883f3917a037485059700b9e75da2464e6c27051014ad85ba6aaa5884426"},
-    {file = "cfgv-3.3.1.tar.gz", hash = "sha256:f5a830efb9ce7a445376bb66ec94c638a9787422f96264c98edc6bdeed8ab736"},
-]
+certifi = []
+cfgv = []
 charset-normalizer = []
 colorama = []
 cycler = []
-distlib = [
-    {file = "distlib-0.3.4-py2.py3-none-any.whl", hash = "sha256:6564fe0a8f51e734df6333d08b8b94d4ea8ee6b99b5ed50613f731fd4089f34b"},
-    {file = "distlib-0.3.4.zip", hash = "sha256:e4b58818180336dc9c529bfb9a0b58728ffc09ad92027a3f30b7cd91e3458579"},
-]
-docutils = [
-    {file = "docutils-0.17.1-py2.py3-none-any.whl", hash = "sha256:cf316c8370a737a022b72b56874f6602acf974a37a9fba42ec2876387549fc61"},
-    {file = "docutils-0.17.1.tar.gz", hash = "sha256:686577d2e4c32380bb50cbb22f575ed742d58168cee37e99117a854bcd88f125"},
-]
-filelock = [
-    {file = "filelock-3.7.1-py3-none-any.whl", hash = "sha256:37def7b658813cda163b56fc564cdc75e86d338246458c4c28ae84cabefa2404"},
-    {file = "filelock-3.7.1.tar.gz", hash = "sha256:3a0fd85166ad9dbab54c9aec96737b744106dc5f15c0b09a6744a445299fcf04"},
-]
+distlib = []
+docutils = []
+filelock = []
 fonttools = []
-identify = [
-    {file = "identify-2.5.1-py2.py3-none-any.whl", hash = "sha256:0dca2ea3e4381c435ef9c33ba100a78a9b40c0bab11189c7cf121f75815efeaa"},
-    {file = "identify-2.5.1.tar.gz", hash = "sha256:3d11b16f3fe19f52039fb7e39c9c884b21cb1b586988114fbe42671f03de3e82"},
-]
-idna = [
-    {file = "idna-3.3-py3-none-any.whl", hash = "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff"},
-    {file = "idna-3.3.tar.gz", hash = "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d"},
-]
+identify = []
+idna = []
 imagesize = []
 importlib-metadata = []
-iniconfig = [
-    {file = "iniconfig-1.1.1-py2.py3-none-any.whl", hash = "sha256:011e24c64b7f47f6ebd835bb12a743f2fbe9a26d4cecaa7f53bc4f35ee9da8b3"},
-    {file = "iniconfig-1.1.1.tar.gz", hash = "sha256:bc3af051d7d14b2ee5ef9969666def0cd1a000e121eaea580d4a313df4b37f32"},
-]
-jinja2 = [
-    {file = "Jinja2-3.1.2-py3-none-any.whl", hash = "sha256:6088930bfe239f0e6710546ab9c19c9ef35e29792895fed6e6e31a023a182a61"},
-    {file = "Jinja2-3.1.2.tar.gz", hash = "sha256:31351a702a408a9e7595a8fc6150fc3f43bb6bf7e319770cbc0db9df9437e852"},
-]
+iniconfig = []
+jinja2 = []
 kiwisolver = []
-markupsafe = [
-    {file = "MarkupSafe-2.1.1-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:86b1f75c4e7c2ac2ccdaec2b9022845dbb81880ca318bb7a0a01fbf7813e3812"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:f121a1420d4e173a5d96e47e9a0c0dcff965afdf1626d28de1460815f7c4ee7a"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a49907dd8420c5685cfa064a1335b6754b74541bbb3706c259c02ed65b644b3e"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:10c1bfff05d95783da83491be968e8fe789263689c02724e0c691933c52994f5"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:b7bd98b796e2b6553da7225aeb61f447f80a1ca64f41d83612e6139ca5213aa4"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:b09bf97215625a311f669476f44b8b318b075847b49316d3e28c08e41a7a573f"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-musllinux_1_1_i686.whl", hash = "sha256:694deca8d702d5db21ec83983ce0bb4b26a578e71fbdbd4fdcd387daa90e4d5e"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:efc1913fd2ca4f334418481c7e595c00aad186563bbc1ec76067848c7ca0a933"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-win32.whl", hash = "sha256:4a33dea2b688b3190ee12bd7cfa29d39c9ed176bda40bfa11099a3ce5d3a7ac6"},
-    {file = "MarkupSafe-2.1.1-cp310-cp310-win_amd64.whl", hash = "sha256:dda30ba7e87fbbb7eab1ec9f58678558fd9a6b8b853530e176eabd064da81417"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:671cd1187ed5e62818414afe79ed29da836dde67166a9fac6d435873c44fdd02"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3799351e2336dc91ea70b034983ee71cf2f9533cdff7c14c90ea126bfd95d65a"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:e72591e9ecd94d7feb70c1cbd7be7b3ebea3f548870aa91e2732960fa4d57a37"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:6fbf47b5d3728c6aea2abb0589b5d30459e369baa772e0f37a0320185e87c980"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-musllinux_1_1_aarch64.whl", hash = "sha256:d5ee4f386140395a2c818d149221149c54849dfcfcb9f1debfe07a8b8bd63f9a"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-musllinux_1_1_i686.whl", hash = "sha256:bcb3ed405ed3222f9904899563d6fc492ff75cce56cba05e32eff40e6acbeaa3"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-musllinux_1_1_x86_64.whl", hash = "sha256:e1c0b87e09fa55a220f058d1d49d3fb8df88fbfab58558f1198e08c1e1de842a"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-win32.whl", hash = "sha256:8dc1c72a69aa7e082593c4a203dcf94ddb74bb5c8a731e4e1eb68d031e8498ff"},
-    {file = "MarkupSafe-2.1.1-cp37-cp37m-win_amd64.whl", hash = "sha256:97a68e6ada378df82bc9f16b800ab77cbf4b2fada0081794318520138c088e4a"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:e8c843bbcda3a2f1e3c2ab25913c80a3c5376cd00c6e8c4a86a89a28c8dc5452"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:0212a68688482dc52b2d45013df70d169f542b7394fc744c02a57374a4207003"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:8e576a51ad59e4bfaac456023a78f6b5e6e7651dcd383bcc3e18d06f9b55d6d1"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4b9fe39a2ccc108a4accc2676e77da025ce383c108593d65cc909add5c3bd601"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:96e37a3dc86e80bf81758c152fe66dbf60ed5eca3d26305edf01892257049925"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:6d0072fea50feec76a4c418096652f2c3238eaa014b2f94aeb1d56a66b41403f"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-musllinux_1_1_i686.whl", hash = "sha256:089cf3dbf0cd6c100f02945abeb18484bd1ee57a079aefd52cffd17fba910b88"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:6a074d34ee7a5ce3effbc526b7083ec9731bb3cbf921bbe1d3005d4d2bdb3a63"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-win32.whl", hash = "sha256:421be9fbf0ffe9ffd7a378aafebbf6f4602d564d34be190fc19a193232fd12b1"},
-    {file = "MarkupSafe-2.1.1-cp38-cp38-win_amd64.whl", hash = "sha256:fc7b548b17d238737688817ab67deebb30e8073c95749d55538ed473130ec0c7"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:e04e26803c9c3851c931eac40c695602c6295b8d432cbe78609649ad9bd2da8a"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:b87db4360013327109564f0e591bd2a3b318547bcef31b468a92ee504d07ae4f"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:99a2a507ed3ac881b975a2976d59f38c19386d128e7a9a18b7df6fff1fd4c1d6"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:56442863ed2b06d19c37f94d999035e15ee982988920e12a5b4ba29b62ad1f77"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:3ce11ee3f23f79dbd06fb3d63e2f6af7b12db1d46932fe7bd8afa259a5996603"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:33b74d289bd2f5e527beadcaa3f401e0df0a89927c1559c8566c066fa4248ab7"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-musllinux_1_1_i686.whl", hash = "sha256:43093fb83d8343aac0b1baa75516da6092f58f41200907ef92448ecab8825135"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:8e3dcf21f367459434c18e71b2a9532d96547aef8a871872a5bd69a715c15f96"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-win32.whl", hash = "sha256:d4306c36ca495956b6d568d276ac11fdd9c30a36f1b6eb928070dc5360b22e1c"},
-    {file = "MarkupSafe-2.1.1-cp39-cp39-win_amd64.whl", hash = "sha256:46d00d6cfecdde84d40e572d63735ef81423ad31184100411e6e3388d405e247"},
-    {file = "MarkupSafe-2.1.1.tar.gz", hash = "sha256:7f91197cc9e48f989d12e4e6fbc46495c446636dfc81b9ccf50bb0ec74b91d4b"},
-]
+markupsafe = []
 matplotlib = []
 nodeenv = []
 numpy = []
-packaging = [
-    {file = "packaging-21.3-py3-none-any.whl", hash = "sha256:ef103e05f519cdc783ae24ea4e2e0f508a9c99b2d4969652eed6a2e1ea5bd522"},
-    {file = "packaging-21.3.tar.gz", hash = "sha256:dd47c42927d89ab911e606518907cc2d3a1f38bbd026385970643f9c5b8ecfeb"},
-]
+packaging = []
 pillow = []
-platformdirs = [
-    {file = "platformdirs-2.5.2-py3-none-any.whl", hash = "sha256:027d8e83a2d7de06bbac4e5ef7e023c02b863d7ea5d079477e722bb41ab25788"},
-    {file = "platformdirs-2.5.2.tar.gz", hash = "sha256:58c8abb07dcb441e6ee4b11d8df0ac856038f944ab98b7be6b27b2a3c7feef19"},
-]
-pluggy = [
-    {file = "pluggy-1.0.0-py2.py3-none-any.whl", hash = "sha256:74134bbf457f031a36d68416e1509f34bd5ccc019f0bcc952c7b909d06b37bd3"},
-    {file = "pluggy-1.0.0.tar.gz", hash = "sha256:4224373bacce55f955a878bf9cfa763c1e360858e330072059e10bad68531159"},
-]
-pre-commit = [
-    {file = "pre_commit-2.19.0-py2.py3-none-any.whl", hash = "sha256:10c62741aa5704faea2ad69cb550ca78082efe5697d6f04e5710c3c229afdd10"},
-    {file = "pre_commit-2.19.0.tar.gz", hash = "sha256:4233a1e38621c87d9dda9808c6606d7e7ba0e087cd56d3fe03202a01d2919615"},
-]
-py = [
-    {file = "py-1.11.0-py2.py3-none-any.whl", hash = "sha256:607c53218732647dff4acdfcd50cb62615cedf612e72d1724fb1a0cc6405b378"},
-    {file = "py-1.11.0.tar.gz", hash = "sha256:51c75c4126074b472f746a24399ad32f6053d1b34b68d2fa41e558e6f4a98719"},
-]
-pygments = [
-    {file = "Pygments-2.12.0-py3-none-any.whl", hash = "sha256:dc9c10fb40944260f6ed4c688ece0cd2048414940f1cea51b8b226318411c519"},
-    {file = "Pygments-2.12.0.tar.gz", hash = "sha256:5eb116118f9612ff1ee89ac96437bb6b49e8f04d8a13b514ba26f620208e26eb"},
-]
-pyparsing = [
-    {file = "pyparsing-3.0.9-py3-none-any.whl", hash = "sha256:5026bae9a10eeaefb61dab2f09052b9f4307d44aee4eda64b309723d8d206bbc"},
-    {file = "pyparsing-3.0.9.tar.gz", hash = "sha256:2b020ecf7d21b687f219b71ecad3631f644a47f01403fa1d1036b0c6416d70fb"},
-]
-pytest = [
-    {file = "pytest-6.2.5-py3-none-any.whl", hash = "sha256:7310f8d27bc79ced999e760ca304d69f6ba6c6649c0b60fb0e04a4a77cacc134"},
-    {file = "pytest-6.2.5.tar.gz", hash = "sha256:131b36680866a76e6781d13f101efb86cf674ebb9762eb70d3082b6f29889e89"},
-]
-python-dateutil = [
-    {file = "python-dateutil-2.8.2.tar.gz", hash = "sha256:0123cacc1627ae19ddf3c27a5de5bd67ee4586fbdd6440d9748f8abb483d3e86"},
-    {file = "python_dateutil-2.8.2-py2.py3-none-any.whl", hash = "sha256:961d03dc3453ebbc59dbdea9e4e11c5651520a876d0f4db161e8674aae935da9"},
-]
-pytz = [
-    {file = "pytz-2022.1-py2.py3-none-any.whl", hash = "sha256:e68985985296d9a66a881eb3193b0906246245294a881e7c8afe623866ac6a5c"},
-    {file = "pytz-2022.1.tar.gz", hash = "sha256:1e760e2fe6a8163bc0b3d9a19c4f84342afa0a2affebfaa84b01b978a02ecaa7"},
-]
-pyyaml = [
-    {file = "PyYAML-6.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:d4db7c7aef085872ef65a8fd7d6d09a14ae91f691dec3e87ee5ee0539d516f53"},
-    {file = "PyYAML-6.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:9df7ed3b3d2e0ecfe09e14741b857df43adb5a3ddadc919a2d94fbdf78fea53c"},
-    {file = "PyYAML-6.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:77f396e6ef4c73fdc33a9157446466f1cff553d979bd00ecb64385760c6babdc"},
-    {file = "PyYAML-6.0-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:a80a78046a72361de73f8f395f1f1e49f956c6be882eed58505a15f3e430962b"},
-    {file = "PyYAML-6.0-cp310-cp310-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:f84fbc98b019fef2ee9a1cb3ce93e3187a6df0b2538a651bfb890254ba9f90b5"},
-    {file = "PyYAML-6.0-cp310-cp310-win32.whl", hash = "sha256:2cd5df3de48857ed0544b34e2d40e9fac445930039f3cfe4bcc592a1f836d513"},
-    {file = "PyYAML-6.0-cp310-cp310-win_amd64.whl", hash = "sha256:daf496c58a8c52083df09b80c860005194014c3698698d1a57cbcfa182142a3a"},
-    {file = "PyYAML-6.0-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:897b80890765f037df3403d22bab41627ca8811ae55e9a722fd0392850ec4d86"},
-    {file = "PyYAML-6.0-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:50602afada6d6cbfad699b0c7bb50d5ccffa7e46a3d738092afddc1f9758427f"},
-    {file = "PyYAML-6.0-cp36-cp36m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:48c346915c114f5fdb3ead70312bd042a953a8ce5c7106d5bfb1a5254e47da92"},
-    {file = "PyYAML-6.0-cp36-cp36m-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:98c4d36e99714e55cfbaaee6dd5badbc9a1ec339ebfc3b1f52e293aee6bb71a4"},
-    {file = "PyYAML-6.0-cp36-cp36m-win32.whl", hash = "sha256:0283c35a6a9fbf047493e3a0ce8d79ef5030852c51e9d911a27badfde0605293"},
-    {file = "PyYAML-6.0-cp36-cp36m-win_amd64.whl", hash = "sha256:07751360502caac1c067a8132d150cf3d61339af5691fe9e87803040dbc5db57"},
-    {file = "PyYAML-6.0-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:819b3830a1543db06c4d4b865e70ded25be52a2e0631ccd2f6a47a2822f2fd7c"},
-    {file = "PyYAML-6.0-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:473f9edb243cb1935ab5a084eb238d842fb8f404ed2193a915d1784b5a6b5fc0"},
-    {file = "PyYAML-6.0-cp37-cp37m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:0ce82d761c532fe4ec3f87fc45688bdd3a4c1dc5e0b4a19814b9009a29baefd4"},
-    {file = "PyYAML-6.0-cp37-cp37m-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:231710d57adfd809ef5d34183b8ed1eeae3f76459c18fb4a0b373ad56bedcdd9"},
-    {file = "PyYAML-6.0-cp37-cp37m-win32.whl", hash = "sha256:c5687b8d43cf58545ade1fe3e055f70eac7a5a1a0bf42824308d868289a95737"},
-    {file = "PyYAML-6.0-cp37-cp37m-win_amd64.whl", hash = "sha256:d15a181d1ecd0d4270dc32edb46f7cb7733c7c508857278d3d378d14d606db2d"},
-    {file = "PyYAML-6.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:0b4624f379dab24d3725ffde76559cff63d9ec94e1736b556dacdfebe5ab6d4b"},
-    {file = "PyYAML-6.0-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:213c60cd50106436cc818accf5baa1aba61c0189ff610f64f4a3e8c6726218ba"},
-    {file = "PyYAML-6.0-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:9fa600030013c4de8165339db93d182b9431076eb98eb40ee068700c9c813e34"},
-    {file = "PyYAML-6.0-cp38-cp38-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:277a0ef2981ca40581a47093e9e2d13b3f1fbbeffae064c1d21bfceba2030287"},
-    {file = "PyYAML-6.0-cp38-cp38-win32.whl", hash = "sha256:d4eccecf9adf6fbcc6861a38015c2a64f38b9d94838ac1810a9023a0609e1b78"},
-    {file = "PyYAML-6.0-cp38-cp38-win_amd64.whl", hash = "sha256:1e4747bc279b4f613a09eb64bba2ba602d8a6664c6ce6396a4d0cd413a50ce07"},
-    {file = "PyYAML-6.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:055d937d65826939cb044fc8c9b08889e8c743fdc6a32b33e2390f66013e449b"},
-    {file = "PyYAML-6.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:e61ceaab6f49fb8bdfaa0f92c4b57bcfbea54c09277b1b4f7ac376bfb7a7c174"},
-    {file = "PyYAML-6.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d67d839ede4ed1b28a4e8909735fc992a923cdb84e618544973d7dfc71540803"},
-    {file = "PyYAML-6.0-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:cba8c411ef271aa037d7357a2bc8f9ee8b58b9965831d9e51baf703280dc73d3"},
-    {file = "PyYAML-6.0-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:40527857252b61eacd1d9af500c3337ba8deb8fc298940291486c465c8b46ec0"},
-    {file = "PyYAML-6.0-cp39-cp39-win32.whl", hash = "sha256:b5b9eccad747aabaaffbc6064800670f0c297e52c12754eb1d976c57e4f74dcb"},
-    {file = "PyYAML-6.0-cp39-cp39-win_amd64.whl", hash = "sha256:b3d267842bf12586ba6c734f89d1f5b871df0273157918b0ccefa29deb05c21c"},
-    {file = "PyYAML-6.0.tar.gz", hash = "sha256:68fb519c14306fec9720a2a5b45bc9f0c8d1b9c72adf45c37baedfcd949c35a2"},
-]
+platformdirs = []
+pluggy = []
+pre-commit = []
+py = []
+pygments = []
+pyparsing = []
+pytest = []
+python-dateutil = []
+pytz = []
+pyyaml = []
 requests = []
 setuptools-scm = []
-six = [
-    {file = "six-1.16.0-py2.py3-none-any.whl", hash = "sha256:8abb2f1d86890a2dfb989f9a77cfcfd3e47c2a354b01111771326f8aa26e0254"},
-    {file = "six-1.16.0.tar.gz", hash = "sha256:1e61c37477a1626458e36f7b1d82aa5c9b094fa4802892072e49de9c60c4c926"},
-]
-snowballstemmer = [
-    {file = "snowballstemmer-2.2.0-py2.py3-none-any.whl", hash = "sha256:c8e1716e83cc398ae16824e5572ae04e0d9fc2c6b985fb0f900f5f0c96ecba1a"},
-    {file = "snowballstemmer-2.2.0.tar.gz", hash = "sha256:09b16deb8547d3412ad7b590689584cd0fe25ec8db3be37788be3810cbf19cb1"},
-]
-sphinx = [
-    {file = "Sphinx-4.5.0-py3-none-any.whl", hash = "sha256:ebf612653238bcc8f4359627a9b7ce44ede6fdd75d9d30f68255c7383d3a6226"},
-    {file = "Sphinx-4.5.0.tar.gz", hash = "sha256:7bf8ca9637a4ee15af412d1a1d9689fec70523a68ca9bb9127c2f3eeb344e2e6"},
-]
-sphinx-rtd-theme = [
-    {file = "sphinx_rtd_theme-1.0.0-py2.py3-none-any.whl", hash = "sha256:4d35a56f4508cfee4c4fb604373ede6feae2a306731d533f409ef5c3496fdbd8"},
-    {file = "sphinx_rtd_theme-1.0.0.tar.gz", hash = "sha256:eec6d497e4c2195fa0e8b2016b337532b8a699a68bcb22a512870e16925c6a5c"},
-]
-sphinxcontrib-applehelp = [
-    {file = "sphinxcontrib-applehelp-1.0.2.tar.gz", hash = "sha256:a072735ec80e7675e3f432fcae8610ecf509c5f1869d17e2eecff44389cdbc58"},
-    {file = "sphinxcontrib_applehelp-1.0.2-py2.py3-none-any.whl", hash = "sha256:806111e5e962be97c29ec4c1e7fe277bfd19e9652fb1a4392105b43e01af885a"},
-]
-sphinxcontrib-devhelp = [
-    {file = "sphinxcontrib-devhelp-1.0.2.tar.gz", hash = "sha256:ff7f1afa7b9642e7060379360a67e9c41e8f3121f2ce9164266f61b9f4b338e4"},
-    {file = "sphinxcontrib_devhelp-1.0.2-py2.py3-none-any.whl", hash = "sha256:8165223f9a335cc1af7ffe1ed31d2871f325254c0423bc0c4c7cd1c1e4734a2e"},
-]
-sphinxcontrib-htmlhelp = [
-    {file = "sphinxcontrib-htmlhelp-2.0.0.tar.gz", hash = "sha256:f5f8bb2d0d629f398bf47d0d69c07bc13b65f75a81ad9e2f71a63d4b7a2f6db2"},
-    {file = "sphinxcontrib_htmlhelp-2.0.0-py2.py3-none-any.whl", hash = "sha256:d412243dfb797ae3ec2b59eca0e52dac12e75a241bf0e4eb861e450d06c6ed07"},
-]
-sphinxcontrib-jsmath = [
-    {file = "sphinxcontrib-jsmath-1.0.1.tar.gz", hash = "sha256:a9925e4a4587247ed2191a22df5f6970656cb8ca2bd6284309578f2153e0c4b8"},
-    {file = "sphinxcontrib_jsmath-1.0.1-py2.py3-none-any.whl", hash = "sha256:2ec2eaebfb78f3f2078e73666b1415417a116cc848b72e5172e596c871103178"},
-]
-sphinxcontrib-qthelp = [
-    {file = "sphinxcontrib-qthelp-1.0.3.tar.gz", hash = "sha256:4c33767ee058b70dba89a6fc5c1892c0d57a54be67ddd3e7875a18d14cba5a72"},
-    {file = "sphinxcontrib_qthelp-1.0.3-py2.py3-none-any.whl", hash = "sha256:bd9fc24bcb748a8d51fd4ecaade681350aa63009a347a8c14e637895444dfab6"},
-]
-sphinxcontrib-serializinghtml = [
-    {file = "sphinxcontrib-serializinghtml-1.1.5.tar.gz", hash = "sha256:aa5f6de5dfdf809ef505c4895e51ef5c9eac17d0f287933eb49ec495280b6952"},
-    {file = "sphinxcontrib_serializinghtml-1.1.5-py2.py3-none-any.whl", hash = "sha256:352a9a00ae864471d3a7ead8d7d79f5fc0b57e8b3f95e9867eb9eb28999b92fd"},
-]
-toml = [
-    {file = "toml-0.10.2-py2.py3-none-any.whl", hash = "sha256:806143ae5bfb6a3c6e736a764057db0e6a0e05e338b5630894a5f779cabb4f9b"},
-    {file = "toml-0.10.2.tar.gz", hash = "sha256:b3bda1d108d5dd99f4a20d24d9c348e91c4db7ab1b749200bded2f839ccbe68f"},
-]
-tomli = [
-    {file = "tomli-2.0.1-py3-none-any.whl", hash = "sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc"},
-    {file = "tomli-2.0.1.tar.gz", hash = "sha256:de526c12914f0c550d15924c62d72abc48d6fe7364aa87328337a31007fe8a4f"},
-]
+six = []
+snowballstemmer = []
+sphinx = []
+sphinx-rtd-theme = []
+sphinxcontrib-applehelp = []
+sphinxcontrib-devhelp = []
+sphinxcontrib-htmlhelp = []
+sphinxcontrib-jsmath = []
+sphinxcontrib-qthelp = []
+sphinxcontrib-serializinghtml = []
+toml = []
+tomli = []
 tox = []
-typing-extensions = []
 urllib3 = []
 virtualenv = []
-zipp = [
-    {file = "zipp-3.8.0-py3-none-any.whl", hash = "sha256:c4f6e5bbf48e74f7a38e7cc5b0480ff42b0ae5178957d564d18932525d5cf099"},
-    {file = "zipp-3.8.0.tar.gz", hash = "sha256:56bf8aadb83c24db6c4b577e13de374ccfb67da2078beba1d037c17980bf43ad"},
-]
+zipp = []
diff --git a/pyproject.toml b/pyproject.toml
index 7b21b03..a1944a5 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -51,6 +51,12 @@ Sphinx = "^4.3.1"
 sphinx-rtd-theme = "^1.0.0"
 matplotlib = "^3.5.2"
 
+[tool.poetry.extras]
+numba = ["numba"]
+plot = ["matplotlib"]
+test = ["pytest", "tox"]
+docs = ["Sphinx", "sphinx-rtd-theme"]
+
 [build-system]
 requires = ["poetry-core>=1.0.7", "poetry==1.1.11"]
 build-backend = "poetry.core.masonry.api"
diff --git a/tests/helper_fcts_test.py b/tests/helper_fcts_test.py
index 1baa49d..39682cb 100755
--- a/tests/helper_fcts_test.py
+++ b/tests/helper_fcts_test.py
@@ -26,9 +26,8 @@ def test_inside_polygon():
 
         def test_fct(input):
             polygon_test_case = np.array([(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)])
-            x, y = input
-
-            return inside_polygon(x, y, polygon_test_case, border_value)
+            p = np.array(input, dtype=float)
+            return inside_polygon(p, polygon_test_case, border_value)
 
         p_test_cases = [
             # (x,y),

From 2125b1b06c1bc30a65ea9e83666a32807462faa3 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 18:50:33 +0200
Subject: [PATCH 36/44] refactor inside_polygon

---
 extremitypathfinder/helper_fcts.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 742f74b..4fd4198 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -75,20 +75,21 @@ def get_repr_n_dists(orig_idx: int, coords: np.ndarray) -> np.ndarray:
 def inside_polygon(p: np.ndarray, coords: np.ndarray, border_value: bool) -> bool:
     # should return the border value for point equal to any polygon vertex
     # TODO overflow possible with large values when comparing slopes, change procedure
-    x, y = p
-    for c in coords[:]:
-        if np.array_equal(c, p):
-            return border_value
-
     # and if the point p lies on any polygon edge
     p1 = coords[-1, :]
     for p2 in coords[:]:
+        if np.array_equal(p2, p):
+            return border_value
         rep_p1_p, _ = compute_repr_n_dist(p1 - p)
         rep_p2_p, _ = compute_repr_n_dist(p2 - p)
         if abs(rep_p1_p - rep_p2_p) == 2.0:
             return border_value
         p1 = p2
 
+    # regular point in polygon algorithm
+    # TODO use optimised implementation
+    x, y = p
+
     contained = False
     # the edge from the last to the first point is checked first
     i = -1

From 5954322879372c049aa35c3158272f5ad167134f Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:00:32 +0200
Subject: [PATCH 37/44] add test case

---
 CHANGELOG.rst                         | 3 ++-
 docs/conf.py                          | 2 +-
 extremitypathfinder/helper_classes.py | 4 ----
 extremitypathfinder/helper_fcts.py    | 9 ++++-----
 tests/main_test.py                    | 6 +++++-
 5 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 6278668..505ebe9 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -3,7 +3,8 @@ Changelog
 
 
 TODO pending major release: remove separate prepare step?! initialise in one step during initialisation
-TODO Numba JIT compilation of utils
+TODO Numba JIT compilation of utils. line speed profiling for highest impact of refactoring
+TODO improve A* implementation (away from OOP)
 
 
 2.3.0 (2022-xx)
diff --git a/docs/conf.py b/docs/conf.py
index 50a65ad..bfdef07 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -110,7 +110,7 @@
 # so a file named "default.css" will overwrite the builtin "default.css".
 html_static_path = ["_static"]
 
-# TODO https://github.com/adamchainz/django-mysql/blob/master/docs/conf.py
+# https://github.com/adamchainz/django-mysql/blob/master/docs/conf.py
 # -- Options for LaTeX output ------------------------------------------
 
 # -- Options for manual page output ------------------------------------
diff --git a/extremitypathfinder/helper_classes.py b/extremitypathfinder/helper_classes.py
index 199870c..c439df1 100644
--- a/extremitypathfinder/helper_classes.py
+++ b/extremitypathfinder/helper_classes.py
@@ -10,7 +10,6 @@ class SearchState(object):
     def __init__(self, node, distance, neighbour_generator, path, cost_so_far, cost_estim):
         self.node = node
         self.distance = distance
-        # TODO
         self.neighbours = neighbour_generator
         self.path = path
         self.cost_so_far: float = cost_so_far
@@ -45,7 +44,6 @@ def get_distance_to_origin(coords_origin: np.ndarray, coords_v: np.ndarray) -> f
 NodeId = int
 
 
-# TODO often empty sets in self.neighbours
 class DirectedHeuristicGraph(object):
     __slots__ = ["all_nodes", "distances", "goal_coords", "heuristic", "neighbours", "coord_map", "merged_id_mapping"]
 
@@ -62,8 +60,6 @@ def __init__(self, coord_map: Optional[Dict[NodeId, np.ndarray]] = None):
         self.coord_map: Dict[NodeId, np.ndarray] = coord_map
         self.merged_id_mapping: Dict[NodeId, NodeId] = {}
 
-        # TODO use same set as extremities of env, but different for copy!
-
         # the heuristic must NEVER OVERESTIMATE the actual cost (here: actual shortest distance)
         # <=> must always be lowest for node with the POSSIBLY lowest cost
         # <=> heuristic is LOWER BOUND for the cost
diff --git a/extremitypathfinder/helper_fcts.py b/extremitypathfinder/helper_fcts.py
index 4fd4198..ea86cbf 100644
--- a/extremitypathfinder/helper_fcts.py
+++ b/extremitypathfinder/helper_fcts.py
@@ -1,4 +1,3 @@
-# TODO numba precompilation of some parts possible?! do line speed profiling first! speed impact
 import json
 import math
 from itertools import combinations
@@ -152,7 +151,7 @@ def get_intersection_status(p1, p2, q1, q2):
     # return:
     #   0: no intersection
     #   1: intersection in ]p1;p2[
-    # TODO 4 different possibilities
+    # TODO support 2 remaining possibilities
     #   2: intersection directly in p1 or p2
     #   3: intersection directly in q1 or q2
     # solve the set of equations
@@ -276,9 +275,9 @@ def check_data_requirements(boundary_coords: np.ndarray, list_hole_coords: List[
             * outer boundary polygon: counter clockwise
             * holes: clockwise
 
-    # TODO test
-    # todo - polygons must not intersect each other
-    # TODO data rectification
+    TODO test
+    todo - polygons must not intersect each other
+    TODO data rectification
 
     :param boundary_coords:
     :param list_hole_coords:
diff --git a/tests/main_test.py b/tests/main_test.py
index e8851a0..7d4997f 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -6,7 +6,6 @@
 from extremitypathfinder.extremitypathfinder import PolygonEnvironment
 from extremitypathfinder.plotting import PlottingEnvironment
 
-# TODO
 # PLOT_TEST_RESULTS = True
 PLOT_TEST_RESULTS = False
 TEST_PLOT_OUTPUT_FOLDER = "plots"
@@ -19,6 +18,7 @@
     ENVIRONMENT_CLASS = PolygonEnvironment
     CONSTRUCTION_KWARGS = {}
 
+# TODO pytest parameterize
 # size_x, size_y, obstacle_iter
 GRID_ENV_PARAMS = (
     19,
@@ -297,6 +297,10 @@
             138.23115155299263,
         ),
     ),
+    (
+        ((2, 38), (45, 45)),
+        ([(2.0, 38.0), (9.5, 45.5), (10.0, 45.5), (45.0, 45.0)], 46.11017296417249),
+    ),
 ]
 
 SEPARATED_ENV = (

From 04c15fd416c345c91bdffd70f80c6939cf4b775c Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:04:48 +0200
Subject: [PATCH 38/44] refactored main tests

---
 tests/__init__.py   |   0
 tests/main_test.py  | 470 +++++++++-----------------------------------
 tests/test_cases.py | 299 ++++++++++++++++++++++++++++
 3 files changed, 388 insertions(+), 381 deletions(-)
 create mode 100644 tests/__init__.py
 create mode 100644 tests/test_cases.py

diff --git a/tests/__init__.py b/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/main_test.py b/tests/main_test.py
index 7d4997f..8c9e925 100755
--- a/tests/main_test.py
+++ b/tests/main_test.py
@@ -1,10 +1,18 @@
-import unittest
-from math import sqrt
-
 import pytest
 
 from extremitypathfinder.extremitypathfinder import PolygonEnvironment
 from extremitypathfinder.plotting import PlottingEnvironment
+from tests.test_cases import (
+    GRID_ENV_PARAMS,
+    INVALID_DESTINATION_DATA,
+    OVERLAP_POLY_ENV_PARAMS,
+    POLY_ENV_PARAMS,
+    SEPARATED_ENV,
+    TEST_DATA_GRID_ENV,
+    TEST_DATA_OVERLAP_POLY_ENV,
+    TEST_DATA_POLY_ENV,
+    TEST_DATA_SEPARATE_ENV,
+)
 
 # PLOT_TEST_RESULTS = True
 PLOT_TEST_RESULTS = False
@@ -18,304 +26,8 @@
     ENVIRONMENT_CLASS = PolygonEnvironment
     CONSTRUCTION_KWARGS = {}
 
-# TODO pytest parameterize
-# size_x, size_y, obstacle_iter
-GRID_ENV_PARAMS = (
-    19,
-    10,
-    [
-        # (x,y),
-        # obstacles changing boundary
-        (0, 1),
-        (1, 1),
-        (2, 1),
-        (3, 1),
-        (17, 9),
-        (17, 8),
-        (17, 7),
-        (17, 5),
-        (17, 4),
-        (17, 3),
-        (17, 2),
-        (17, 1),
-        (17, 0),
-        # hole 1
-        (5, 5),
-        (5, 6),
-        (6, 6),
-        (6, 7),
-        (7, 7),
-        # hole 2
-        (7, 5),
-    ],
-)
-
-INVALID_DESTINATION_DATA = [
-    # outside of map region
-    ((-1, 5.0), (17, 0.5)),
-    ((17, 0.5), (-1, 5.0)),
-    ((20, 5.0), (17, 0.5)),
-    ((17, 0.5), (20, 5.0)),
-    ((1, -5.0), (17, 0.5)),
-    ((17, 0.5), (1, -5.0)),
-    ((1, 11.0), (17, 0.5)),
-    ((17, 0.5), (1, 11.0)),
-    # outside boundary polygon
-    ((17.5, 5.0), (17, 0.5)),
-    ((17, 0.5), (17.5, 5.0)),
-    ((1, 1.5), (17, 0.5)),
-    ((17, 0.5), (1, 1.5)),
-    # inside hole
-    ((6.5, 6.5), (17, 0.5)),
-    ((17, 0.5), (6.5, 6.5)),
-]
-
-TEST_DATA_GRID_ENV = [
-    # ((start,goal),(path,distance))
-    # shortest paths should be distinct (reverse will automatically be tested)
-    # identical nodes
-    (((15, 5), (15, 5)), ([(15, 5), (15, 5)], 0.0)),
-    # directly reachable
-    (((15, 5), (15, 6)), ([(15, 5), (15, 6)], 1.0)),
-    (((15, 5), (16, 6)), ([(15, 5), (16, 6)], sqrt(2))),
-    # points on the polygon edges (vertices) should be accepted!
-    # on edge
-    (((15, 0), (15, 6)), ([(15, 0), (15, 6)], 6.0)),
-    (((17, 5), (16, 5)), ([(17, 5), (16, 5)], 1.0)),
-    # on edge of hole
-    (((7, 8), (7, 9)), ([(7, 8), (7, 9)], 1.0)),
-    # on vertex
-    (((4, 2), (4, 3)), ([(4, 2), (4, 3)], 1.0)),
-    # on vertex of hole
-    (((6, 8), (6, 9)), ([(6, 8), (6, 9)], 1.0)),
-    # on two vertices
-    # coinciding with edge (direct neighbour)
-    (((4, 2), (4, 1)), ([(4, 2), (4, 1)], 1.0)),
-    (((5, 5), (5, 7)), ([(5, 5), (5, 7)], 2.0)),
-    # should have direct connection to all visible extremities! connected in graph
-    (((6, 8), (5, 7)), ([(6, 8), (5, 7)], sqrt(2))),
-    (((4, 1), (5, 7)), ([(4, 1), (5, 7)], sqrt(1**2 + 6**2))),
-    # should have direct connection to all visible extremities! even if not connected in graph!
-    (((4, 2), (5, 7)), ([(4, 2), (5, 7)], sqrt(1**2 + 5**2))),
-    # mix of edges and vertices, directly visible
-    (((2, 2), (5, 7)), ([(2, 2), (5, 7)], sqrt(3**2 + 5**2))),
-    # also regular points should have direct connection to all visible extremities!
-    (((10, 3), (17, 6)), ([(10, 3), (17, 6)], sqrt(7**2 + 3**2))),
-    (((10, 3), (8, 8)), ([(10, 3), (8, 8)], sqrt(2**2 + 5**2))),
-    # even if the query point lies in front of an extremity! (test if new query vertices are being created!)
-    (((10, 3), (8, 5)), ([(10, 3), (8, 5)], sqrt(2**2 + 2**2))),
-    # using a* graph search:
-    # directly reachable through a single vertex (does not change distance!)
-    (((5, 1), (3, 3)), ([(5, 1), (4, 2), (3, 3)], sqrt(2**2 + 2**2))),
-    # If two Polygons have vertices with identical coordinates (this is allowed),
-    #   paths through these vertices are theoretically possible!
-    (
-        ((6.5, 5.5), (7.5, 6.5)),
-        ([(6.5, 5.5), (7, 6), (7.5, 6.5)], sqrt(1**2 + 1**2)),
-    ),
-    # distance should stay the same even if multiple extremities lie on direct path
-    # test if path is skipping passed extremities
-    (((8, 4), (8, 8)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8)], 4)),
-    (((8, 4), (8, 9)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9)], 5)),
-    # regular examples
-    (
-        ((0.5, 6), (18.5, 0.5)),
-        (
-            [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (17, 6), (18, 6), (18.5, 0.5)],
-            23.18783787537749,
-        ),
-    ),
-    (
-        ((0.5, 6), (9, 5.5)),
-        ([(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (9.0, 5.5)], 8.727806217396338),
-    ),
-    (
-        ((0.5, 6), (18.5, 9)),
-        (
-            [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (18, 7), (18.5, 9.0)],
-            19.869364068640845,
-        ),
-    ),
-    (
-        ((6.9, 4), (7, 9)),
-        ([(6.9, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.830925564196269),
-    ),
-    (
-        ((6.5, 4), (7, 9)),
-        ([(6.5, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.889979937555021),
-    ),
-    # symmetric around the lower boundary obstacle
-    (
-        ((0.5, 0.5), (0.5, 2.5)),
-        ([(0.5, 0.5), (4, 1), (4, 2), (0.5, 2.5)], 8.071067811865476),
-    ),
-    # symmetric around the lower right boundary obstacle
-    (
-        ((16.5, 0.5), (18.5, 0.5)),
-        ([(16.5, 0.5), (17, 6), (18, 6), (18.5, 0.5)], 12.045361017187261),
-    ),
-    # symmetric around the top right boundary obstacle
-    (
-        ((16.5, 9.5), (18.5, 9.5)),
-        ([(16.5, 9.5), (17, 7), (18, 7), (18.5, 9.5)], 6.0990195135927845),
-    ),
-]
-
-POLY_ENV_PARAMS = (
-    # boundary_coordinates
-    [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)],
-    # list_of_holes
-    [
-        [
-            (3.0, 7.0),
-            (5.0, 9.0),
-            (4.6, 7.0),
-            (5.0, 4.0),
-        ],
-    ],
-)
-
-TEST_DATA_POLY_ENV = [
-    # ((start,goal),(path,distance))
-    # identical nodes
-    (((1, 1), (1, 1)), ([(1, 1), (1, 1)], 0.0)),
-    # directly reachable
-    (((1, 1), (1, 2)), ([(1, 1), (1, 2)], 1.0)),
-    (((1, 1), (2, 1)), ([(1, 1), (2, 1)], 1.0)),
-    # points on the polygon edges (vertices) should be accepted!
-    # on edge (boundary polygon)
-    (((1, 0), (1, 1)), ([(1, 0), (1, 1)], 1.0)),
-    (((9.5, 2.5), (8.5, 2.5)), ([(9.5, 2.5), (8.5, 2.5)], 1.0)),
-    (((0, 2), (0, 1)), ([(0, 2), (0, 1)], 1.0)),  # both
-    (((1, 0), (5, 0)), ([(1, 0), (5, 0)], 4.0)),  # both
-    # on edge of hole
-    (((4, 8), (3, 8)), ([(4, 8), (3, 8)], 1.0)),
-    (((4, 8), (4.1, 8.1)), ([(4, 8), (4.1, 8.1)], sqrt(2 * (0.1**2)))),  # both
-    # on vertex
-    (((9, 5), (8, 5)), ([(9, 5), (8, 5)], 1.0)),
-    # on vertex of hole
-    (((3, 7), (2, 7)), ([(3, 7), (2, 7)], 1.0)),
-    # on two vertices
-    # coinciding with edge (direct neighbour)
-    (((3, 7), (5, 9)), ([(3, 7), (5, 9)], sqrt(8))),
-    (((4.6, 7), (5, 9)), ([(4.6, 7), (5, 9)], sqrt((0.4**2) + (2**2)))),
-    # should have direct connection to all visible extremities! connected in graph
-    (((5, 4), (5, 9)), ([(5, 4), (5, 9)], 5)),
-    # should have a direct connection to all visible extremities! even if not connected in graph!
-    (((9, 5), (5, 9)), ([(9, 5), (5, 9)], sqrt(2 * (4**2)))),
-    # using a* graph search:
-    # directly reachable through a single vertex (does not change distance!)
-    (((9, 4), (9, 6)), ([(9, 4), (9, 5), (9, 6)], 2)),
-    # slightly indented, path must go through right boundary extremity
-    (((9.1, 4), (9.1, 6)), ([(9.1, 4.0), (9.0, 5.0), (9.1, 6.0)], 2.009975124224178)),
-    # path must go through lower hole extremity
-    (((4, 4.5), (6, 4.5)), ([(4.0, 4.5), (5.0, 4.0), (6.0, 4.5)], 2.23606797749979)),
-    # path must go through top hole extremity
-    (((4, 8.5), (6, 8.5)), ([(4.0, 8.5), (5.0, 9.0), (6.0, 8.5)], 2.23606797749979)),
-]
-
-OVERLAP_POLY_ENV_PARAMS = (
-    # boundary_coordinates
-    [
-        (9.5, 10.5),
-        (25.5, 10.5),
-        (25.5, 0.5),
-        (49.5, 0.5),
-        (49.5, 49.5),
-        (0.5, 49.5),
-        (0.5, 16.5),
-        (9.5, 16.5),
-        (9.5, 45.5),
-        (10.0, 45.5),
-        (10.0, 30.5),
-        (35.5, 30.5),
-        (35.5, 14.5),
-        (0.5, 14.5),
-        (0.5, 0.5),
-        (9.5, 0.5),
-    ],
-    # list_of_holes
-    [
-        [
-            (40.5, 4.5),
-            (29.5, 4.5),
-            (29.5, 15.0),
-            (40.5, 15.0),
-        ],
-        [
-            (45.4, 14.5),
-            (44.6, 14.5),
-            (43.4, 20.5),
-            (46.6, 20.5),
-        ],
-        # slightly right of the top boundary obstacle
-        # goal: create an obstacle that obstructs two very close extremities
-        # to check if visibility is correctly blocked in such cases
-        [
-            (40, 34),
-            (10.5, 34),
-            (10.5, 40),
-            (40, 40),
-        ],
-        # on the opposite site close to top boundary obstacle
-        [
-            (9, 34),
-            (5, 34),
-            (5, 40),
-            (9, 40),
-        ],
-        [
-            (31.5, 5.390098048839718),
-            (31.5, 10.909901951439679),
-            (42.5, 13.109901951160282),
-            (42.5, 7.590098048560321),
-        ],
-    ],
-)
-
-TEST_DATA_OVERLAP_POLY_ENV = [
-    # ((start,goal),(path,distance))
-    # TODO add more
-    (
-        ((1, 1), (5, 20)),
-        (
-            [
-                (1, 1),
-                (9.5, 10.5),
-                (25.5, 10.5),
-                (29.5, 4.5),
-                (40.5, 4.5),
-                (42.5, 7.590098048560321),
-                (42.5, 13.109901951160282),
-                (35.5, 30.5),
-                (10.5, 34.0),
-                (10.0, 45.5),
-                (9.5, 45.5),
-                (9, 34),
-                (5, 20),
-            ],
-            138.23115155299263,
-        ),
-    ),
-    (
-        ((2, 38), (45, 45)),
-        ([(2.0, 38.0), (9.5, 45.5), (10.0, 45.5), (45.0, 45.0)], 46.11017296417249),
-    ),
-]
-
-SEPARATED_ENV = (
-    [(5, 5), (-5, 5), (-5, -5), (5, -5)],
-    [[(-5.1, 1), (-5.1, 2), (5.1, 2), (5.1, 1)]],  # intersecting polygons -> no path possible
-    # [[(-5, 1), (-5, 2), (5, 2), (5, 1)]], # hole lies on the edges -> path possible
-)
-
-TEST_DATA_SEPARATE_ENV = [
-    # ((start,goal),(path,distance))
-    (((0, 0), (0, 4)), ([], None)),  # unreachable
-]
-
 
-# ((start,goal),(path,distance))
+# TODO pytest parameterize
 
 
 def try_test_cases(environment, test_cases):
@@ -344,84 +56,80 @@ def validate(start_coordinates, goal_coordinates, expected_output):
         validate(goal_coordinates, start_coordinates, expected_output_reversed)
 
 
-class MainTest(unittest.TestCase):
-    def test_grid_env(self):
-        grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
-
-        grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False)
-        nr_extremities = len(grid_env.all_extremities)
-        assert nr_extremities == 17, "extremities do not get detected correctly!"
-        grid_env.prepare()
-        nr_graph_nodes = len(grid_env.graph.all_nodes)
-        assert nr_graph_nodes == 16, "identical nodes should get joined in the graph!"
-
-        # test if points outside the map are being rejected
-        for start_coordinates, goal_coordinates in INVALID_DESTINATION_DATA:
-            with pytest.raises(ValueError):
-                grid_env.find_shortest_path(start_coordinates, goal_coordinates)
-
-        print("testing grid environment")
-        try_test_cases(grid_env, TEST_DATA_GRID_ENV)
-
-        # when the deep copy mechanism works correctly
-        # even after many queries the internal graph should have the same structure as before
-        # otherwise the temporarily added vertices during a query stay stored
-        nr_graph_nodes = len(grid_env.graph.all_nodes)
-        # TODO
-        # assert nr_graph_nodes == 16, "the graph should stay unchanged by shortest path queries!"
-
-        nr_nodes_env1_old = len(grid_env.graph.all_nodes)
-
-    def test_poly_env(self):
-        poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
-        poly_env.store(*POLY_ENV_PARAMS, validate=True)
-        NR_EXTR_POLY_ENV = 4
-        assert (
-            len(list(poly_env.all_extremities)) == NR_EXTR_POLY_ENV
-        ), f"the environment should detect all {NR_EXTR_POLY_ENV} extremities!"
-        poly_env.prepare()
-        nr_nodes_env2 = len(poly_env.graph.all_nodes)
-        assert nr_nodes_env2 == NR_EXTR_POLY_ENV, (
-            f"the visibility graph should store all {NR_EXTR_POLY_ENV} extremities {list(poly_env.all_extremities)}!"
-            f"\n found: {poly_env.graph.all_nodes}"
-        )
-
-        # nr_nodes_env1_new = len(grid_env.graph.all_nodes)
-        # assert (
-        #     nr_nodes_env1_new == nr_nodes_env1_old
-        # ), "node amount of an grid_env should not change by creating another grid_env!"
-        # assert grid_env.graph is not poly_env.graph, "different environments share the same graph object"
-        # assert (
-        #     grid_env.graph.all_nodes is not poly_env.graph.all_nodes
-        # ), "different environments share the same set of nodes"
-
-        print("\ntesting polygon environment")
-        try_test_cases(poly_env, TEST_DATA_POLY_ENV)
-
-        # TODO test: When the paths should be blocked, use a single polygon with multiple identical
-        #   vertices instead (also allowed?! change data requirements in doc!).
-
-        # TODO test graph construction
-        # when two nodes have the same angle representation there should only be an edge to the closer node!
-        # test if property 1 is being properly exploited
-        # (extremities lying in front of each other need not be connected)
-
-    def test_overlapping_polygon(self):
-        overlap_poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
-        overlap_poly_env.store(*OVERLAP_POLY_ENV_PARAMS)
-        overlap_poly_env.prepare()
-        print("\ntesting polygon environment with overlapping polygons")
-        try_test_cases(overlap_poly_env, TEST_DATA_OVERLAP_POLY_ENV)
-
-    def test_separated_environment(self):
-        env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
-        env.store(*SEPARATED_ENV)
-        env.prepare()
-        print("\ntesting polygon environment with two separated areas")
-        try_test_cases(env, TEST_DATA_SEPARATE_ENV)
-
-
-if __name__ == "__main__":
-    suite = unittest.TestLoader().loadTestsFromTestCase(MainTest)
-    unittest.TextTestRunner(verbosity=2).run(suite)
-    unittest.main()
+def test_grid_env():
+    grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
+
+    grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False)
+    nr_extremities = len(grid_env.all_extremities)
+    assert nr_extremities == 17, "extremities do not get detected correctly!"
+    grid_env.prepare()
+    nr_graph_nodes = len(grid_env.graph.all_nodes)
+    assert nr_graph_nodes == 16, "identical nodes should get joined in the graph!"
+
+    # test if points outside the map are being rejected
+    for start_coordinates, goal_coordinates in INVALID_DESTINATION_DATA:
+        with pytest.raises(ValueError):
+            grid_env.find_shortest_path(start_coordinates, goal_coordinates)
+
+    print("testing grid environment")
+    try_test_cases(grid_env, TEST_DATA_GRID_ENV)
+
+    # when the deep copy mechanism works correctly
+    # even after many queries the internal graph should have the same structure as before
+    # otherwise the temporarily added vertices during a query stay stored
+    nr_graph_nodes = len(grid_env.graph.all_nodes)
+    # TODO
+    # assert nr_graph_nodes == 16, "the graph should stay unchanged by shortest path queries!"
+
+    nr_nodes_env1_old = len(grid_env.graph.all_nodes)
+
+
+def test_poly_env():
+    poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
+    poly_env.store(*POLY_ENV_PARAMS, validate=True)
+    NR_EXTR_POLY_ENV = 4
+    assert (
+        len(list(poly_env.all_extremities)) == NR_EXTR_POLY_ENV
+    ), f"the environment should detect all {NR_EXTR_POLY_ENV} extremities!"
+    poly_env.prepare()
+    nr_nodes_env2 = len(poly_env.graph.all_nodes)
+    assert nr_nodes_env2 == NR_EXTR_POLY_ENV, (
+        f"the visibility graph should store all {NR_EXTR_POLY_ENV} extremities {list(poly_env.all_extremities)}!"
+        f"\n found: {poly_env.graph.all_nodes}"
+    )
+
+    # nr_nodes_env1_new = len(grid_env.graph.all_nodes)
+    # assert (
+    #     nr_nodes_env1_new == nr_nodes_env1_old
+    # ), "node amount of an grid_env should not change by creating another grid_env!"
+    # assert grid_env.graph is not poly_env.graph, "different environments share the same graph object"
+    # assert (
+    #     grid_env.graph.all_nodes is not poly_env.graph.all_nodes
+    # ), "different environments share the same set of nodes"
+
+    print("\ntesting polygon environment")
+    try_test_cases(poly_env, TEST_DATA_POLY_ENV)
+
+    # TODO test: When the paths should be blocked, use a single polygon with multiple identical
+    #   vertices instead (also allowed?! change data requirements in doc!).
+
+    # TODO test graph construction
+    # when two nodes have the same angle representation there should only be an edge to the closer node!
+    # test if property 1 is being properly exploited
+    # (extremities lying in front of each other need not be connected)
+
+
+def test_overlapping_polygon():
+    overlap_poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
+    overlap_poly_env.store(*OVERLAP_POLY_ENV_PARAMS)
+    overlap_poly_env.prepare()
+    print("\ntesting polygon environment with overlapping polygons")
+    try_test_cases(overlap_poly_env, TEST_DATA_OVERLAP_POLY_ENV)
+
+
+def test_separated_environment():
+    env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS)
+    env.store(*SEPARATED_ENV)
+    env.prepare()
+    print("\ntesting polygon environment with two separated areas")
+    try_test_cases(env, TEST_DATA_SEPARATE_ENV)
diff --git a/tests/test_cases.py b/tests/test_cases.py
new file mode 100644
index 0000000..18a7998
--- /dev/null
+++ b/tests/test_cases.py
@@ -0,0 +1,299 @@
+# size_x, size_y, obstacle_iter
+from math import sqrt
+
+GRID_ENV_PARAMS = (
+    19,
+    10,
+    [
+        # (x,y),
+        # obstacles changing boundary
+        (0, 1),
+        (1, 1),
+        (2, 1),
+        (3, 1),
+        (17, 9),
+        (17, 8),
+        (17, 7),
+        (17, 5),
+        (17, 4),
+        (17, 3),
+        (17, 2),
+        (17, 1),
+        (17, 0),
+        # hole 1
+        (5, 5),
+        (5, 6),
+        (6, 6),
+        (6, 7),
+        (7, 7),
+        # hole 2
+        (7, 5),
+    ],
+)
+
+INVALID_DESTINATION_DATA = [
+    # outside of map region
+    ((-1, 5.0), (17, 0.5)),
+    ((17, 0.5), (-1, 5.0)),
+    ((20, 5.0), (17, 0.5)),
+    ((17, 0.5), (20, 5.0)),
+    ((1, -5.0), (17, 0.5)),
+    ((17, 0.5), (1, -5.0)),
+    ((1, 11.0), (17, 0.5)),
+    ((17, 0.5), (1, 11.0)),
+    # outside boundary polygon
+    ((17.5, 5.0), (17, 0.5)),
+    ((17, 0.5), (17.5, 5.0)),
+    ((1, 1.5), (17, 0.5)),
+    ((17, 0.5), (1, 1.5)),
+    # inside hole
+    ((6.5, 6.5), (17, 0.5)),
+    ((17, 0.5), (6.5, 6.5)),
+]
+
+TEST_DATA_GRID_ENV = [
+    # ((start,goal),(path,distance))
+    # shortest paths should be distinct (reverse will automatically be tested)
+    # identical nodes
+    (((15, 5), (15, 5)), ([(15, 5), (15, 5)], 0.0)),
+    # directly reachable
+    (((15, 5), (15, 6)), ([(15, 5), (15, 6)], 1.0)),
+    (((15, 5), (16, 6)), ([(15, 5), (16, 6)], sqrt(2))),
+    # points on the polygon edges (vertices) should be accepted!
+    # on edge
+    (((15, 0), (15, 6)), ([(15, 0), (15, 6)], 6.0)),
+    (((17, 5), (16, 5)), ([(17, 5), (16, 5)], 1.0)),
+    # on edge of hole
+    (((7, 8), (7, 9)), ([(7, 8), (7, 9)], 1.0)),
+    # on vertex
+    (((4, 2), (4, 3)), ([(4, 2), (4, 3)], 1.0)),
+    # on vertex of hole
+    (((6, 8), (6, 9)), ([(6, 8), (6, 9)], 1.0)),
+    # on two vertices
+    # coinciding with edge (direct neighbour)
+    (((4, 2), (4, 1)), ([(4, 2), (4, 1)], 1.0)),
+    (((5, 5), (5, 7)), ([(5, 5), (5, 7)], 2.0)),
+    # should have direct connection to all visible extremities! connected in graph
+    (((6, 8), (5, 7)), ([(6, 8), (5, 7)], sqrt(2))),
+    (((4, 1), (5, 7)), ([(4, 1), (5, 7)], sqrt(1**2 + 6**2))),
+    # should have direct connection to all visible extremities! even if not connected in graph!
+    (((4, 2), (5, 7)), ([(4, 2), (5, 7)], sqrt(1**2 + 5**2))),
+    # mix of edges and vertices, directly visible
+    (((2, 2), (5, 7)), ([(2, 2), (5, 7)], sqrt(3**2 + 5**2))),
+    # also regular points should have direct connection to all visible extremities!
+    (((10, 3), (17, 6)), ([(10, 3), (17, 6)], sqrt(7**2 + 3**2))),
+    (((10, 3), (8, 8)), ([(10, 3), (8, 8)], sqrt(2**2 + 5**2))),
+    # even if the query point lies in front of an extremity! (test if new query vertices are being created!)
+    (((10, 3), (8, 5)), ([(10, 3), (8, 5)], sqrt(2**2 + 2**2))),
+    # using a* graph search:
+    # directly reachable through a single vertex (does not change distance!)
+    (((5, 1), (3, 3)), ([(5, 1), (4, 2), (3, 3)], sqrt(2**2 + 2**2))),
+    # If two Polygons have vertices with identical coordinates (this is allowed),
+    #   paths through these vertices are theoretically possible!
+    (
+        ((6.5, 5.5), (7.5, 6.5)),
+        ([(6.5, 5.5), (7, 6), (7.5, 6.5)], sqrt(1**2 + 1**2)),
+    ),
+    # distance should stay the same even if multiple extremities lie on direct path
+    # test if path is skipping passed extremities
+    (((8, 4), (8, 8)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8)], 4)),
+    (((8, 4), (8, 9)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9)], 5)),
+    # regular examples
+    (
+        ((0.5, 6), (18.5, 0.5)),
+        (
+            [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (17, 6), (18, 6), (18.5, 0.5)],
+            23.18783787537749,
+        ),
+    ),
+    (
+        ((0.5, 6), (9, 5.5)),
+        ([(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (9.0, 5.5)], 8.727806217396338),
+    ),
+    (
+        ((0.5, 6), (18.5, 9)),
+        (
+            [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (18, 7), (18.5, 9.0)],
+            19.869364068640845,
+        ),
+    ),
+    (
+        ((6.9, 4), (7, 9)),
+        ([(6.9, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.830925564196269),
+    ),
+    (
+        ((6.5, 4), (7, 9)),
+        ([(6.5, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.889979937555021),
+    ),
+    # symmetric around the lower boundary obstacle
+    (
+        ((0.5, 0.5), (0.5, 2.5)),
+        ([(0.5, 0.5), (4, 1), (4, 2), (0.5, 2.5)], 8.071067811865476),
+    ),
+    # symmetric around the lower right boundary obstacle
+    (
+        ((16.5, 0.5), (18.5, 0.5)),
+        ([(16.5, 0.5), (17, 6), (18, 6), (18.5, 0.5)], 12.045361017187261),
+    ),
+    # symmetric around the top right boundary obstacle
+    (
+        ((16.5, 9.5), (18.5, 9.5)),
+        ([(16.5, 9.5), (17, 7), (18, 7), (18.5, 9.5)], 6.0990195135927845),
+    ),
+]
+
+POLY_ENV_PARAMS = (
+    # boundary_coordinates
+    [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)],
+    # list_of_holes
+    [
+        [
+            (3.0, 7.0),
+            (5.0, 9.0),
+            (4.6, 7.0),
+            (5.0, 4.0),
+        ],
+    ],
+)
+
+TEST_DATA_POLY_ENV = [
+    # ((start,goal),(path,distance))
+    # identical nodes
+    (((1, 1), (1, 1)), ([(1, 1), (1, 1)], 0.0)),
+    # directly reachable
+    (((1, 1), (1, 2)), ([(1, 1), (1, 2)], 1.0)),
+    (((1, 1), (2, 1)), ([(1, 1), (2, 1)], 1.0)),
+    # points on the polygon edges (vertices) should be accepted!
+    # on edge (boundary polygon)
+    (((1, 0), (1, 1)), ([(1, 0), (1, 1)], 1.0)),
+    (((9.5, 2.5), (8.5, 2.5)), ([(9.5, 2.5), (8.5, 2.5)], 1.0)),
+    (((0, 2), (0, 1)), ([(0, 2), (0, 1)], 1.0)),  # both
+    (((1, 0), (5, 0)), ([(1, 0), (5, 0)], 4.0)),  # both
+    # on edge of hole
+    (((4, 8), (3, 8)), ([(4, 8), (3, 8)], 1.0)),
+    (((4, 8), (4.1, 8.1)), ([(4, 8), (4.1, 8.1)], sqrt(2 * (0.1**2)))),  # both
+    # on vertex
+    (((9, 5), (8, 5)), ([(9, 5), (8, 5)], 1.0)),
+    # on vertex of hole
+    (((3, 7), (2, 7)), ([(3, 7), (2, 7)], 1.0)),
+    # on two vertices
+    # coinciding with edge (direct neighbour)
+    (((3, 7), (5, 9)), ([(3, 7), (5, 9)], sqrt(8))),
+    (((4.6, 7), (5, 9)), ([(4.6, 7), (5, 9)], sqrt((0.4**2) + (2**2)))),
+    # should have direct connection to all visible extremities! connected in graph
+    (((5, 4), (5, 9)), ([(5, 4), (5, 9)], 5)),
+    # should have a direct connection to all visible extremities! even if not connected in graph!
+    (((9, 5), (5, 9)), ([(9, 5), (5, 9)], sqrt(2 * (4**2)))),
+    # using a* graph search:
+    # directly reachable through a single vertex (does not change distance!)
+    (((9, 4), (9, 6)), ([(9, 4), (9, 5), (9, 6)], 2)),
+    # slightly indented, path must go through right boundary extremity
+    (((9.1, 4), (9.1, 6)), ([(9.1, 4.0), (9.0, 5.0), (9.1, 6.0)], 2.009975124224178)),
+    # path must go through lower hole extremity
+    (((4, 4.5), (6, 4.5)), ([(4.0, 4.5), (5.0, 4.0), (6.0, 4.5)], 2.23606797749979)),
+    # path must go through top hole extremity
+    (((4, 8.5), (6, 8.5)), ([(4.0, 8.5), (5.0, 9.0), (6.0, 8.5)], 2.23606797749979)),
+]
+
+OVERLAP_POLY_ENV_PARAMS = (
+    # boundary_coordinates
+    [
+        (9.5, 10.5),
+        (25.5, 10.5),
+        (25.5, 0.5),
+        (49.5, 0.5),
+        (49.5, 49.5),
+        (0.5, 49.5),
+        (0.5, 16.5),
+        (9.5, 16.5),
+        (9.5, 45.5),
+        (10.0, 45.5),
+        (10.0, 30.5),
+        (35.5, 30.5),
+        (35.5, 14.5),
+        (0.5, 14.5),
+        (0.5, 0.5),
+        (9.5, 0.5),
+    ],
+    # list_of_holes
+    [
+        [
+            (40.5, 4.5),
+            (29.5, 4.5),
+            (29.5, 15.0),
+            (40.5, 15.0),
+        ],
+        [
+            (45.4, 14.5),
+            (44.6, 14.5),
+            (43.4, 20.5),
+            (46.6, 20.5),
+        ],
+        # slightly right of the top boundary obstacle
+        # goal: create an obstacle that obstructs two very close extremities
+        # to check if visibility is correctly blocked in such cases
+        [
+            (40, 34),
+            (10.5, 34),
+            (10.5, 40),
+            (40, 40),
+        ],
+        # on the opposite site close to top boundary obstacle
+        [
+            (9, 34),
+            (5, 34),
+            (5, 40),
+            (9, 40),
+        ],
+        [
+            (31.5, 5.390098048839718),
+            (31.5, 10.909901951439679),
+            (42.5, 13.109901951160282),
+            (42.5, 7.590098048560321),
+        ],
+    ],
+)
+
+TEST_DATA_OVERLAP_POLY_ENV = [
+    # ((start,goal),(path,distance))
+    # TODO add more
+    (
+        ((1, 1), (5, 20)),
+        (
+            [
+                (1, 1),
+                (9.5, 10.5),
+                (25.5, 10.5),
+                (29.5, 4.5),
+                (40.5, 4.5),
+                (42.5, 7.590098048560321),
+                (42.5, 13.109901951160282),
+                (35.5, 30.5),
+                (10.5, 34.0),
+                (10.0, 45.5),
+                (9.5, 45.5),
+                (9, 34),
+                (5, 20),
+            ],
+            138.23115155299263,
+        ),
+    ),
+    (
+        ((2, 38), (45, 45)),
+        ([(2.0, 38.0), (9.5, 45.5), (10.0, 45.5), (45.0, 45.0)], 46.11017296417249),
+    ),
+]
+
+SEPARATED_ENV = (
+    [(5, 5), (-5, 5), (-5, -5), (5, -5)],
+    [[(-5.1, 1), (-5.1, 2), (5.1, 2), (5.1, 1)]],  # intersecting polygons -> no path possible
+    # [[(-5, 1), (-5, 2), (5, 2), (5, 1)]], # hole lies on the edges -> path possible
+)
+
+TEST_DATA_SEPARATE_ENV = [
+    # ((start,goal),(path,distance))
+    (((0, 0), (0, 4)), ([], None)),  # unreachable
+]
+
+
+# ((start,goal),(path,distance))

From d8b70ba436fe427bb164b5a8ec1d514897787cf1 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:17:36 +0200
Subject: [PATCH 39/44] Update plotting.py

---
 extremitypathfinder/plotting.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/extremitypathfinder/plotting.py b/extremitypathfinder/plotting.py
index 75c4f07..9610a84 100644
--- a/extremitypathfinder/plotting.py
+++ b/extremitypathfinder/plotting.py
@@ -34,7 +34,7 @@ def export_plot(fig, file_name):
 
 def mark_points(vertex_iter, **kwargs):
     try:
-        coordinates = [v.coordinates.tolist() for v in vertex_iter]
+        coordinates = [v.tolist() for v in vertex_iter]
     except AttributeError:
         coordinates = list(vertex_iter)
     coords_zipped = list(zip(*coordinates))
@@ -57,9 +57,9 @@ def draw_polygon(ax, coords, **kwargs):
 def draw_boundaries(map, ax):
     # TODO outside dark grey
     # TODO fill holes light grey
-    draw_polygon(ax, map.boundary_polygon.coordinates)
+    draw_polygon(ax, map.boundary_polygon)
     for h in map.holes:
-        draw_polygon(ax, h.coordinates, facecolor="grey", fill=True)
+        draw_polygon(ax, h, facecolor="grey", fill=True)
 
     mark_points(map.all_vertices, c="black", s=15)
     mark_points(map.all_extremities, c="red", s=50)
@@ -77,14 +77,14 @@ def draw_internal_graph(map: PolygonEnvironment, ax):
 def set_limits(map, ax):
     ax.set_xlim(
         (
-            min(map.boundary_polygon.coordinates[:, 0]) - 1,
-            max(map.boundary_polygon.coordinates[:, 0]) + 1,
+            min(map.boundary_polygon[:, 0]) - 1,
+            max(map.boundary_polygon[:, 0]) + 1,
         )
     )
     ax.set_ylim(
         (
-            min(map.boundary_polygon.coordinates[:, 1]) - 1,
-            max(map.boundary_polygon.coordinates[:, 1]) + 1,
+            min(map.boundary_polygon[:, 1]) - 1,
+            max(map.boundary_polygon[:, 1]) + 1,
         )
     )
 

From 6136e9c73c906fe5bc793c82660cf17fd4ecb620 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:17:49 +0200
Subject: [PATCH 40/44] Update test_cases.py

---
 tests/test_cases.py | 51 +++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 49 insertions(+), 2 deletions(-)

diff --git a/tests/test_cases.py b/tests/test_cases.py
index 18a7998..3e8ff54 100644
--- a/tests/test_cases.py
+++ b/tests/test_cases.py
@@ -256,7 +256,6 @@
 
 TEST_DATA_OVERLAP_POLY_ENV = [
     # ((start,goal),(path,distance))
-    # TODO add more
     (
         ((1, 1), (5, 20)),
         (
@@ -282,6 +281,55 @@
         ((2, 38), (45, 45)),
         ([(2.0, 38.0), (9.5, 45.5), (10.0, 45.5), (45.0, 45.0)], 46.11017296417249),
     ),
+    (
+        ((2, 38), (45, 2)),
+        (
+            [
+                (2.0, 38.0),
+                (9.5, 45.5),
+                (10.0, 45.5),
+                (10.5, 34.0),
+                (35.5, 30.5),
+                (42.5, 13.109901951160282),
+                (45.0, 2.0),
+            ],
+            77.99506635830616,
+        ),
+    ),
+    (
+        ((2, 38), (38, 2)),
+        (
+            [
+                (2.0, 38.0),
+                (9.5, 45.5),
+                (10.0, 45.5),
+                (10.5, 34.0),
+                (35.5, 30.5),
+                (42.5, 13.109901951160282),
+                (42.5, 7.590098048560321),
+                (40.5, 4.5),
+                (38.0, 2.0),
+            ],
+            79.34355163003127,
+        ),
+    ),
+    (
+        ((2, 38), (28, 2)),
+        (
+            [
+                (2.0, 38.0),
+                (9.5, 45.5),
+                (10.0, 45.5),
+                (10.5, 34.0),
+                (35.5, 30.5),
+                (42.5, 13.109901951160282),
+                (42.5, 7.590098048560321),
+                (40.5, 4.5),
+                (28.0, 2.0),
+            ],
+            88.55556650808049,
+        ),
+    ),
 ]
 
 SEPARATED_ENV = (
@@ -295,5 +343,4 @@
     (((0, 0), (0, 4)), ([], None)),  # unreachable
 ]
 
-
 # ((start,goal),(path,distance))

From a90f598e0868c086f5c1f858fcfa05e5f2dd3768 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:18:01 +0200
Subject: [PATCH 41/44] Update extremitypathfinder.py

---
 extremitypathfinder/extremitypathfinder.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/extremitypathfinder/extremitypathfinder.py b/extremitypathfinder/extremitypathfinder.py
index 7377746..7991048 100644
--- a/extremitypathfinder/extremitypathfinder.py
+++ b/extremitypathfinder/extremitypathfinder.py
@@ -1,6 +1,6 @@
 import pickle
 from copy import deepcopy
-from typing import Dict, List, Tuple
+from typing import Dict, List, Optional, Tuple
 
 import numpy as np
 
@@ -51,7 +51,7 @@ class PolygonEnvironment:
     extremity_indices: List[int]
     reprs_n_distances: Dict[int, np.ndarray]
     graph: DirectedHeuristicGraph
-    temp_graph: DirectedHeuristicGraph  # for storing and plotting the graph during a query
+    temp_graph: Optional[DirectedHeuristicGraph] = None  # for storing and plotting the graph during a query
     boundary_polygon: np.ndarray
     coords: np.ndarray
     edge_vertex_idxs: np.ndarray
@@ -367,8 +367,8 @@ def find_shortest_path(
 
         # clean up
         # TODO re-use the same graph
-        graph.remove_node(idx_start)
-        graph.remove_node(idx_goal)
+        # graph.remove_node(idx_start)
+        # graph.remove_node(idx_goal)
         if free_space_after:
             del graph  # free the memory
 

From ff13d2f315e61defa15c0eb8c311a34746155128 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:18:12 +0200
Subject: [PATCH 42/44] Update helper_fcts_test.py

---
 tests/helper_fcts_test.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/helper_fcts_test.py b/tests/helper_fcts_test.py
index 39682cb..eb08e34 100755
--- a/tests/helper_fcts_test.py
+++ b/tests/helper_fcts_test.py
@@ -8,7 +8,6 @@
 
 import numpy as np
 import pytest
-from helpers import proto_test_case
 
 from extremitypathfinder import PolygonEnvironment
 from extremitypathfinder.helper_fcts import (
@@ -19,6 +18,7 @@
     inside_polygon,
     read_json,
 )
+from tests.helpers import proto_test_case
 
 
 def test_inside_polygon():

From 8222701c65603ab0bc5e06e212e26cdbdc2eb40e Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:19:42 +0200
Subject: [PATCH 43/44] Update CHANGELOG.rst

---
 CHANGELOG.rst | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 505ebe9..31b5a20 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -7,10 +7,10 @@ TODO Numba JIT compilation of utils. line speed profiling for highest impact of
 TODO improve A* implementation (away from OOP)
 
 
-2.3.0 (2022-xx)
+2.3.0 (2022-08-18)
 -------------------
 
-* major overhaul of all functionality from OOP to functional/numpy based
+* major overhaul of all functionality from OOP to functional programming/numpy based
 
 internal:
 

From 6c5c5d542f10f78d34888dbf1531a5272c0827d9 Mon Sep 17 00:00:00 2001
From: MrMinimal64 <github@michelfe.it>
Date: Thu, 18 Aug 2022 19:20:05 +0200
Subject: [PATCH 44/44] Update pyproject.toml

---
 pyproject.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/pyproject.toml b/pyproject.toml
index a1944a5..7a44ea7 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "extremitypathfinder"
-version = "2.2.3"
+version = "2.3.0"
 license = "MIT"
 readme = "README.rst"
 repository = "https://github.com/jannikmi/extremitypathfinder"