fmm.hpp

#ifndef FMM_HPP_
#define FMM_HPP_

#include <iostream>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <array>

#include <fast_methods/fm/eikonalsolver.hpp>

#include <fast_methods/ndgridmap/fmcell.h>
#include <fast_methods/datastructures/fmdaryheap.hpp>

#include <fast_methods/ndgridmap/ndgridmap.hpp>
#include <fast_methods/console/console.h>

enum HeurStrategy {NOHEUR = 0, TIME, DISTANCE};

template < class grid_t, class heap_t = FMDaryHeap<FMCell> >  class FMM : public EikonalSolver<grid_t> {

    public:
        FMM(HeurStrategy h = NOHEUR) : EikonalSolver<grid_t>("FMM"), heurStrategy_(h), precomputed_(false) {
            //if (static_cast<FMFibHeap>(heap_t))
             //   name_ = "FMMFib";
        }

        FMM(const char * name, HeurStrategy h = NOHEUR) : EikonalSolver<grid_t>(name), heurStrategy_(h), precomputed_(false) {}

        virtual ~FMM() { clear(); }

        virtual void setup
        () {
            EikonalSolver<grid_t>::setup();
            narrow_band_.setMaxSize(grid_->size());
            setHeuristics(heurStrategy_); // Redundant, but safe.

            if (int(goal_idx_) == -1 && heurStrategy_ != NOHEUR) {
                console::warning("FMM/SFMM: Heuristics set with no goal point. Deactivating heuristics.");
                heurStrategy_ = NOHEUR;
            }
        }

        virtual void computeInternal
        () {
            if (!setup_)
                setup();

            unsigned int j = 0;
            unsigned int n_neighs = 0;
            bool stopWavePropagation = false;

            // Algorithm initialization
            for (unsigned int &i: init_points_) { // For each initial point
                grid_->getCell(i).setArrivalTime(0);
                // Include heuristics if necessary.
                if (heurStrategy_ == TIME)
                    grid_->getCell(i).setHeuristicTime( getPrecomputedDistance(i)/grid_->getCell(i).getVelocity() );
                else if (heurStrategy_ == DISTANCE)
                    grid_->getCell(i).setHeuristicTime( getPrecomputedDistance(i) );
                narrow_band_.push( &(grid_->getCell(i)) );
            }

            // Main loop.
            unsigned int idxMin = 0;
            while (!stopWavePropagation && !narrow_band_.empty()) {
                idxMin = narrow_band_.popMinIdx();
                n_neighs = grid_->getNeighbors(idxMin, neighbors_);
                grid_->getCell(idxMin).setState(FMState::FROZEN);
                for (unsigned int s = 0; s < n_neighs; ++s) {
                    j = neighbors_[s];
                    if ((grid_->getCell(j).getState() == FMState::FROZEN) || grid_->getCell(j).isOccupied())
                        continue;
                    else {
                        double new_arrival_time = solveEikonal(j);

                        // Include heuristics if necessary.
                        if (heurStrategy_ == TIME)
                            grid_->getCell(j).setHeuristicTime( getPrecomputedDistance(j)/grid_->getCell(j).getVelocity() );
                        else if (heurStrategy_ == DISTANCE)
                            grid_->getCell(j).setHeuristicTime( getPrecomputedDistance(j) );

                        // Updating narrow band if necessary.
                        if (grid_->getCell(j).getState() == FMState::NARROW) {
                            if (utils::isTimeBetterThan(new_arrival_time, grid_->getCell(j).getArrivalTime())) {
                                grid_->getCell(j).setArrivalTime(new_arrival_time);
                                narrow_band_.increase( &(grid_->getCell(j)) );
                            }
                        }
                        else {
                            grid_->getCell(j).setState(FMState::NARROW);
                            grid_->getCell(j).setArrivalTime(new_arrival_time);
                            narrow_band_.push( &(grid_->getCell(j)) );
                        } // neighbors_ open.
                    } // neighbors_ not frozen.
                } // For each neighbor.

                if (idxMin == goal_idx_)
                    stopWavePropagation = true;
            } // while narrow band not empty
        }

        void setHeuristics
        (HeurStrategy h) {
            if (h && int(goal_idx_)!=-1) {
                heurStrategy_ = h;
                grid_->idx2coord(goal_idx_, heur_coord_);
                if (!precomputed_)
                    precomputeDistances();
            }
        }

        HeurStrategy getHeuristics
        () const {
            return heurStrategy_;
        }

        virtual void clear
        () {
            narrow_band_.clear();
            distances_.clear();
            precomputed_ = false;
        }

        virtual void reset
        () {
            EikonalSolver<grid_t>::reset();
            narrow_band_.clear();
        }

        virtual void precomputeDistances
        () {
            distances_.reserve(grid_->size());
            std::array <unsigned int, grid_t::getNDims()> coords;
            double dist = 0;

            for (size_t i = 0; i < grid_->size(); ++i)
            {
                dist = 0;
                grid_->idx2coord(i, coords);

                for (size_t j = 0; j < coords.size(); ++j)
                    dist += coords[j]*coords[j];

                distances_[i] = std::sqrt(dist);
            }
            precomputed_ = true;
        }

        virtual double getPrecomputedDistance
        (const unsigned int idx) {
            std::array <unsigned int, grid_t::getNDims()> position, distance;
            grid_->idx2coord(idx, position);

            for (unsigned int i = 0; i < grid_t::getNDims(); ++i)
                distance[i] = utils::absUI(position[i] - heur_coord_[i]);

            unsigned int idx_dist;
            grid_->coord2idx(distance, idx_dist);

            return distances_[idx_dist];
        }

        virtual void printRunInfo
        () const {
            console::info("Fast Marching Method");
            std::cout << '\t' << name_ << '\n'
                      << '\t' << "Heuristic type: " << heurStrategy_ << '\n'
                      << '\t' << "Elapsed time: " << time_ << " ms\n";
        }


    protected:
        using EikonalSolver<grid_t>::grid_;
        using EikonalSolver<grid_t>::init_points_;
        using EikonalSolver<grid_t>::goal_idx_;
        using EikonalSolver<grid_t>::setup_;
        using EikonalSolver<grid_t>::name_;
        using EikonalSolver<grid_t>::time_;
        using EikonalSolver<grid_t>::solveEikonal;
        using EikonalSolver<grid_t>::neighbors_;

    private:
        heap_t                                          narrow_band_;

        HeurStrategy                                    heurStrategy_;

        std::vector<double>                             distances_;
        
        bool                                            precomputed_;

        std::array <unsigned int, grid_t::getNDims()>   heur_coord_;
};

#endif /* FMM_HPP_*/