Generalised FOCAL search

[dharabor]

The process of FOCAL search is a generic one which we should capture. For this we need a new type of queue. The queue has 3 parameters:

1. Containers to store nodes (e.g., open, focal)
2. Sorting functions for each container
3. A new function (in addition to ::push, ::pop), which is ::repair
4. Boolean function (repair_policy) that drives the main loop (where to pop from) and calls the repair function if the policy returns false.