fm2.hpp

#ifndef FM2_HPP_
#define FM2_HPP_

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

#include <fast_methods/fm/fmm.hpp>
#include <fast_methods/gradientdescent/gradientdescent.hpp>

//template < class grid_t, class solver_t = FMM<grid_t> > class FM2 : public Solver<grid_t> {

template < class grid_t, class heap_t = FMDaryHeap<FMCell> > class FM2 : public Solver<grid_t> {
    public:
    
        typedef std::vector< std::array<double, grid_t::getNDims()> > path_t;

        FM2
        (double maxDistance = -1) : Solver<grid_t>("FM2"), maxDistance_(maxDistance) {
            solver_ = new FMM<grid_t, heap_t> ();
        }

        FM2
        (const char * name, double maxDistance = -1) : Solver<grid_t>(name), maxDistance_(maxDistance) {
            solver_ = new FMM<grid_t, heap_t> ();
        }

        virtual ~FM2 () { clear(); }

        virtual void setEnvironment
        (grid_t * g) {
            Solver<grid_t>::setEnvironment(g);
            grid_->getOccupiedCells(fm2_sources_);
            solver_->setEnvironment(grid_);
        }

        virtual void setup
        () {
            Solver<grid_t>::setup();

            if(init_points_.size() > 1) {
                console::error("FM2-based solvers currently allow only 1 initial point.");
                exit(1);
            }

            if (fm2_sources_.empty()) {
                console::error("Map has no obstacles. FM2-based solver is not running.");
                exit(1);
            }
        }

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

            computeVelocitiesMap();

            // Reset time counter so that time_ returns the time of the second wave.
            start_ = std::chrono::steady_clock::now();

            // According to the theoretical basis the wave is expanded from the goal point to the initial point.
            std::vector <unsigned int> wave_init;
            wave_init.push_back(goal_idx_);
            unsigned int wave_goal = init_points_[0];

            solver_->setInitialAndGoalPoints(wave_init, wave_goal);
            solver_->compute();
            // Restore the actual grid status.
            grid_->setClean(false);
        }

        void computeVelocitiesMap
        () {
            // Forces not to clean the grid.
            grid_->setClean(true);
            solver_->setInitialPoints(fm2_sources_);
            solver_->compute();
            time_vels_ = solver_->getTime();
            start_ = std::chrono::steady_clock::now();
            // Rescaling and saturating to relative velocities: [0,1]
            double maxValue = grid_->getMaxValue();
            double maxVelocity = 0;

            if (maxDistance_ != -1)
                maxVelocity = maxDistance_ / grid_->getLeafSize();

            for (unsigned int i = 0; i < grid_->size(); ++i) {
                double vel = grid_->getCell(i).getValue() / maxValue;

                if (maxDistance_ != -1)
                    if (vel < maxVelocity)
                        grid_->getCell(i).setVelocity(vel / maxVelocity);
                    else
                        grid_->getCell(i).setVelocity(1);
                else
                    grid_->getCell(i).setVelocity(vel);

                // Restarting grid values for second wave expasion.
                grid_->getCell(i).setValue(std::numeric_limits<double>::infinity());
                grid_->getCell(i).setState(FMState::OPEN);
                grid_->setClean(true);
            }
            end_ = std::chrono::steady_clock::now();
            time_vels_ += std::chrono::duration_cast<std::chrono::milliseconds>(end_-start_).count();
        }

        virtual void computePath
        (path_t * p, std::vector <double> * path_velocity, double step = 1) {
            path_t* path_ = p;
            GradientDescent< nDGridMap<FMCell, grid_t::getNDims()> > grad;
            grad.apply(*grid_,init_points_[0],*path_, *path_velocity, step);
        }

        virtual void clear
        () {
            Solver<grid_t>::clear();
            fm2_sources_.clear();
            maxDistance_ = -1;
            delete solver_;
        }

        virtual void reset
        () {
            Solver<grid_t>::reset();
            solver_->reset();
        }

        virtual double getTimeVelocities
        () const {
            return time_vels_;
        }

    protected:
        using Solver<grid_t>::grid_;
        using Solver<grid_t>::init_points_;
        using Solver<grid_t>::goal_idx_;
        using Solver<grid_t>::setup_;
        using Solver<grid_t>::time_;
        using Solver<grid_t>::start_;
        using Solver<grid_t>::end_;

        std::vector<unsigned int>   fm2_sources_;
        
        FMM<grid_t, heap_t> *       solver_;
        
        double                      maxDistance_;

        double                      time_vels_;
};

#endif /* FM2_H_*/