.. _tutorial-first-solver:

Writing Your First Solver
=========================

This tutorial walks you through building a working motion-planning solver from
scratch. We will implement a solver named ``RandomSolver``. Although not so practical in real life usages, it serves as a great example of how to use the framework to build a solver. 


What Is A solver?
-----------------

A solver is a Python class that implements a motion-planning algorithm. It takes a scene as input and produces a path for each robot as output.
Meaning its whole responsibility is to answer the question: *given a scene
full of robots and obstacles, how should each robot get from its starting
position to its goal?*


The solver base class contract
------------------------------

Every solver in DiscoPygal inherits from :class:`Solver`. The base class defines
the lifecycle and API that Solver Viewer and the rest of the framework rely on.
It's built in a way that handles all the interaction with the framework, so you can focus on the algorithm itself.
The only method required to implement is ``_solve``, which computes the paths for all robots. 
The rest of the methods are optional overrides that let you customize the solver behavior in different ways.

Let's first break down the solvers properties initialized in init:

.. code-block:: python

    self.scene = None
    self.writer = None
    self.verbose = kwargs.get("verbose", False)
    self.bounding_margin_width_factor = bounding_margin_width_factor
    self._bounding_box = None
    self.operations_counter = OperationsCounter
    self.operations_counter.clear()

``self.scene`` is where the loaded scene is stored. You can access it in ``_solve`` and any other method after the scene is loaded.

``self.writer`` is a utility for writing logs and metrics. It handles output formatting and file management, so you can just call ``self.writer.write(...)`` to record whatever you want.

``self.verbose`` is a flag that controls whether the solver should print logs to the console. You can set it when initializing your solver, and then use it in your code to conditionally print debug information.

``self.bounding_margin_width_factor`` is a parameter that affects how the framework computes bounding boxes for collision checking. The default value is 0.0, which means no extra margin. User can adjust this value in the solver viewer UI

``self._bounding_box`` is where the computed bounding box of the scene will be stored after loading. This value should be used when creating the roadmap to make sure the sampled points are within the scene boundaries

``self.operations_counter`` is a utility for counting how many times certain operations occur during solving (like collision checks). It's initialized here and cleared at the start of each solve.

After understanding what properties the base class provides, let's look at the methods you can implement to create your solver:

**Must implement**

1. ``_solve(self) -> PathCollection``

    This is the only abstract method in the base class, so every concrete solver
    must implement it. It should compute paths for the loaded scene and return a
    :class:`PathCollection`.

    Important: do not override ``solve``. The base ``solve`` method handles scene
    loading (when provided), calls ``_solve``, and runs optional analysis/logging.

**Strongly recommended to implement**

1. ``get_arguments(cls)`` (class method)

    The default base implementation only exposes
    ``bounding_margin_width_factor``. In practice, each solver should override
    this method so Solver Viewer can render the correct input fields for your
    solver parameters.

    It should return a dictionary in this format:

    .. code-block:: python

        {
             'arg_name': ('Label shown in UI', default_value, type),
        }

    The dictionary keys should match your ``__init__`` parameters.

2. ``__init__(...)``

    Add the parameters your algorithm needs, and always call
    ``super().__init__(**kwargs)`` so base solver state (verbosity, bounding box
    behavior, counters) is initialized correctly.

**Optional overrides (based on your algorithm)**

1. ``load_scene(self, scene)``

    Override when you need preprocessing. Always call ``super().load_scene(scene)``
    first so ``self.scene`` and the internal bounding box are initialized.

2. ``get_graph(self) -> nx.Graph``

    Return your roadmap/graph for visualization in Solver Viewer. If you do not
    use a graph, keep the default ``None``.

3. ``get_arrangement(self) -> Arrangement_2``

    Return an arrangement object if your solver works with arrangements and you
    want Viewer support for arrangement visualization.

4. ``on_gui_load(self, gui, layout)``

    If you want to add custom widgets to the Solver Viewer UI, implement this
    method to receive the GUI and layout objects. You can then add buttons,
    sliders, or any other PyQt widgets to the layout.


Setting up
----------

Create a new file called ``RandomSolver.py``. We'll fill it in together,
section by section.


Imports: assembling your toolkit
---------------------------------

Start with this at the top of the file:

.. code-block:: python

   import random
   import networkx as nx

   from discopygal.solvers_infra import Path, PathPoint, PathCollection
   from discopygal.solvers_infra.Solver import Solver
   from discopygal.geometry_utils.bounding_boxes import calc_scene_bounding_box
   from discopygal.geometry_utils.collision_detection import ObjectCollisionDetection
   from discopygal.bindings import FT, Point_2

You might recognize ``random`` and ``networkx`` packages from previous python work.
Random is a standard library module for generating random numbers, and ``networkx`` is a popular library for working with graphs.
Tutorials on using those packages are out of scope for this guide. Documentation of the ``random`` library can be found at https://docs.python.org/3/library/random.html ,and documentation of ``networkx`` library can be found at https://networkx.org/documentation/stable/index.html
In our code ``networkx`` is being used gives us a graph we can store points and connections in, and then query for shortest paths.

Next we have some discopygal imports:

``Path``, ``PathPoint``, and ``PathCollection`` objects used to store the robots paths.
``PathPoint`` represents a single point in a robot's path, while ``Path`` is a sequence of those points for one robot. ``PathCollection`` is a container that holds the paths for all robots in the scene.

More can be read about these classes in their documentation: 

``Solver`` is the base class your solver inherits from, as we discussed above


``calc_scene_bounding_box`` is a helper function that computes the bounding box of the scene, which is neccessary for sampling points within the scene boundaries.

``ObjectCollisionDetection`` is a utility class that checks whether a point is valid (collision-free) for a given robot, based on the scene's obstacles. We will use it to ensure our random points are actually reachable.

Finally, ``FT`` and ``Point_2`` are DiscoPygal's geometry primitives. ``Point_2``
is a 2D point; ``FT`` wraps a float so it plays nicely with the underlying CGAL
geometry library. Whenever you create a point, you'll write
``Point_2(FT(x), FT(y))``.

More can be read about these classes in their documentation: 



Defining the class
------------------

Now let's define the solver itself:

.. code-block:: python

   class RandomSolver(Solver):
       def __init__(self, num_landmarks, num_connections, **kwargs):
           super().__init__(**kwargs)

           self.num_landmarks = num_landmarks
           self.num_connections = num_connections

           self._collision_detection = {}
           self._roadmap = None

As you can see, we inherit from ``Solver`` and call ``super().__init__(**kwargs)`` right away this ensures that the base class's own setup (like verbosity and counters) is initialized correctly.
``num_landmarks`` and ``num_connections`` are this solver's two parameters. They control respectively: how many random sample points we scatter through the scene, and how many random edges we draw between them
Later, we'll expose those variables to the Solver Viewer UI so users can adjust them.


The instance variables starting with ``_`` are internal state.
``_collision_detection`` will become a dictionary mapping each robot to its own
collision detector. ``_roadmap`` will hold the graph. 

Loading the scene
-----------------

To ensure full understanding of the code flow we should implement the ``load_scene`` method first.
This method is called by the framework when a scene is loaded, and it's where you can do any preprocessing you need before solving.
For our solver, we need to compute the bounding box and set up collision detection for each robot.

This method is called once, when your solver first receives a scene to work with:

.. code-block:: python

   def load_scene(self, scene):
       super().load_scene(scene)
       self._x_min, self._x_max, self._y_min, self._y_max = calc_scene_bounding_box(self.scene)

       for robot in self.scene.robots:
           self._collision_detection[robot] = ObjectCollisionDetection(scene.obstacles, robot)

In this methods we use two of our imported utilities: ``calc_scene_bounding_box`` and ``ObjectCollisionDetection``.
``calc_scene_bounding_box`` takes the scene and computes the minimum and maximum x and y coordinates that encompass all the obstacles and robots. This gives us the boundaries for sampling random points later on.

Then for each robot ``ObjectCollisionDetection`` instance for it, passing in the scene's obstacles and the robot itself.
This class handles collision detection of a single object with obstacles, in our case each robot with the obstacles.
The collision detector builds a CGAL arrangement representing the scene and allows to (quickly) query the arrangement for collisions.
We will use this collision arrangement to check whether the random points we generate are valid positions for the robots (i.e., not colliding with any obstacles).

Note that all those methods are strictly related to the scene itself and not to the solving process. In addition calculation 
boundary is necessary for writing the ``_solve`` method itself, so in the general case its advisable to implement all things that are strictly related only to the scene under this function.

Sampling random points
----------------------

Before we can build a roadmap, we need to be able to generate random points
inside the scene. Here's a small helper that does just that:

.. code-block:: python

   def _sample_random_point(self):
       x = random.uniform(self._x_min, self._x_max)
       y = random.uniform(self._y_min, self._y_max)
       return Point_2(FT(x), FT(y))

This solver is a Sampling Solver, and in general every sampling-based solver needs a way to generate random points in the scene.
Having the raw sampling isolated in its own method is useful, if you ever want to swap in a smarter distribution/
Our implementation picks a random (x, y) within the scene boundaries and wraps it in a
``Point_2`` (which is a CGAL-compatible point class — more on that in
:doc:`../tutorials/cgalpy`).



Making sure points are actually reachable
-----------------------------------------

A roadmap with unreachable points is not very useful. So we need a way to check the validity of each sample point.
This methods keeps generating random points until it finds one that is valid for all robots. 
Although it seems like this brute-force approach might run forever if the scene is very crowded, 
in runs usually fast enough for our use cases.

.. code-block:: python

   def _create_random_point(self, robots):
       is_valid_point = False
       while not is_valid_point:
           point = self._sample_random_point()
           is_valid_point = all([
               self._collision_detection[robot].is_point_valid(point)
               for robot in robots
           ])
       return point

    
Telling the UI which parameters to expose
------------------------------------------
As we mentioned above, we want users to be able to adjust ``num_landmarks`` and ``num_connections`` 
from the Solver Viewer UI. To do that, we need to implement the ``get_arguments`` class method:


.. code-block:: python

   @classmethod
   def get_arguments(cls):
       return {
           'num_landmarks': ('Number of points', 5, int),
           'num_connections': ('Number of connections', 5, int),
       }

This is what bridges your Python code to the Solver Viewer UI. Each key in the
dictionary is a parameter name that must match exactly what your ``__init__``
expects. Each value is a tuple of ``(label shown in the UI, default value,
Python type)``.
In this case, we tell the UI to show two integer fields labeled "Number of points" and "Number of connections", both defaulting to 5.
When the user changes these values in the UI, they will be passed to your solver's ``__init__`` when it is instantiated.



Building the roadmap
--------------------

No we have the basis for building a roadmap: we can generate random points and check whether they're valid.
Now we just need to connect them together and anchor the robots' start and goal positions.

.. code-block:: python

   def _create_random_roadmap(self, robots):
       roadmap = nx.Graph()

       # Scatter collision-free sample points through the scene
       random_points = []
       for _ in range(self.num_landmarks):
           point = self._create_random_point(robots)
           random_points.append(point)

       roadmap.add_nodes_from(random_points)

       # Connect random pairs of those points
       for _ in range(self.num_connections):
           v, u = random.sample(random_points, 2)
           roadmap.add_edge(v, u, weight=1)

       # Anchor each robot's start and goal into the graph
       for robot in robots:
            # Add starting point of robot to the graph
            roadmap.add_node(robot.start)
            # Connect start to a random point
            roadmap.add_edge(robot.start, *random.sample(random_points, 1), weight=1)
            # Add ending point of robot to the graph
            roadmap.add_node(robot.end)
            # Connect to end to a random point
            roadmap.add_edge(robot.end, *random.sample(random_points, 1), weight=1)
       return roadmap

This method builds the graph in three passes:

The **first pass** scatters ``num_landmarks`` collision-free points and adds them
as nodes. These are the stepping stones our robots will use.

The **second pass** draws ``num_connections`` random edges between those stepping
stones, giving robots routes to follow.

The **third pass** anchors each robot's start and goal positions into the graph by connecting them to a random sample point.
This ensures that the graph includes paths from the robots' actual start and end locations.




Notice that the connections between robots' start/goal positions and the random
points are chosen randomly too. A more sophisticated solver would pick connections more carefully, finding the
*nearest* point, or checking whether the edge itself is collision-free.




Letting the viewer see your roadmap
------------------------------------

To be able to visualize the roadmap in Solver Viewer,we have to implement the ``get_graph`` method.
This method returns ``self._roadmap``, which would store the graph that was built using ``_create_random_roadmap`` (more on that later).

.. code-block:: python

   def get_graph(self):
       return self._roadmap





The main event: _solve
-----------------------

Until now we prepared all the building blocks for our solver, but we haven't actually implemented the ``_solve`` method that computes the paths.
As mentioned above this is the heart of the solver, where the main logic of the algorithms resides.

.. code-block:: python

   def _solve(self):
       self.log("Solving...")
       self._roadmap = self._create_random_roadmap(self.scene.robots)

       path_collection = PathCollection()

       for i, robot in enumerate(self.scene.robots):
           self.log(f"Robot {i}")

           if not nx.algorithms.has_path(self._roadmap, robot.start, robot.end):
               self.log(f"No path found for robot {i}")
               return PathCollection()

           found_path = nx.algorithms.shortest_path(self._roadmap, robot.start, robot.end)
           points = [PathPoint(point) for point in found_path]
           path = Path(points)
           path_collection.add_robot_path(robot, path)

       return path_collection

Let's break this code down:

First we create random roadmap by calling ``_create_random_roadmap`` and store it in ``self._roadmap`` so that it can be visualized in the UI.


Then we create an empty ``PathCollection`` to hold the paths for all robots

For each robot, we check whether the graph has path between the robot's start and end positions.
If not, we log that no path was found and return an empty ``PathCollection`` to indicate failure.
If a path exists, we use ``nx.algorithms.shortest_path`` to find the shortest sequence of nodes from start to goal.
We convert each node into a ``PathPoint``, bundle them into a ``Path``, and add that path to the collection under this robot's name.

After processing all robots, we return the complete ``PathCollection`` containing the paths for all robots.
This is the output that Solver Viewer will use to animate the solution.



Running it
----------

With the file saved, launch Solver Viewer like this:

.. code-block:: bash

   solver_viewer -sl RandomSolver -sf RandomSolver.py -sc [your_sample_scene].json

You should see the scene load, the parameter fields for "Number of points" and
"Number of connections" appear in the panel. Click solve to run the solver and wait for the calculation to finish.
If a path is found, you can validate it and play the robot movement animation.

You could try to change ``num_landmarks`` and ``num_connections`` how it affects the ability of the solver to find a path and the quality of the found path. In general, more points and connections should lead to better paths, but it also increases the runtime, so you might want to find a good balance for your scene.

Full Code
---------

.. collapse:: Solver Class code

    .. literalinclude:: ../../examples/basic_examples/RandomSolver.py
        :caption: RandomSolver.py