solver.hpp

#ifndef SOLVER_H_
#define SOLVER_H_

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

#include <boost/concept_check.hpp>

#include <fast_methods/console/console.h>

template <class grid_t>
class Solver {

    public:
        Solver() :name_("GenericSolver"), setup_(false) {}

        Solver(const std::string& name) : name_(name), setup_(false) {}

        virtual ~Solver() { clear(); }

        virtual void setEnvironment
        (grid_t * g) {
            grid_ = g;
            grid_->clean();
        }

        virtual void setInitialAndGoalPoints
        (const std::vector<unsigned int> & init_points, unsigned int goal_idx) {
            init_points_ = init_points;
            goal_idx_ = goal_idx;
        }

        virtual void setInitialPoints
        (const std::vector<unsigned int> & init_points)
        {
            setInitialAndGoalPoints(init_points, -1);
        }

        virtual void setInitialAndGoalPoints
        (const std::array<unsigned int, grid_t::getNDims()> & init_coord, const std::array<unsigned int, grid_t::getNDims()> & goal_coord) {
            std::vector<unsigned int> init_points;
            unsigned int idx;
            grid_->coord2idx(init_coord, idx);
            init_points.push_back(idx);
            grid_->coord2idx(goal_coord, idx);
            setInitialAndGoalPoints(init_points, idx);
        }

        virtual void setInitialPoints
        (const std::array<unsigned int, grid_t::getNDims()> & init_coord)
        {
            std::vector<unsigned int> init_points;
            unsigned int idx;
            grid_->coord2idx(init_coord, idx);
            init_points.push_back(idx);
            setInitialAndGoalPoints(init_points, -1);
        }

        virtual void setup
        () {
            const int err = sanityChecks();
            if (err)
            {
                console::error("Global sanity checks not successful: ");
                switch(err) {
                    case 1:
                        console::error("No grid map set.");
                        break;
                    case 2:
                        console::error("Grid map set is not clean.");
                        break;
                    case 3:
                        console::error("Initial points were not set.");
                        break;
                    case 4:
                        console::error("A init point is in a obstacle.");
                        break;
                    case 5:
                        console::error("A goal point is in a obstacle.");
                        break;
                    case 6:
                        console::error("A start is equal to a goal point.");
                        break;
                    default:
                        console::error("Uknown error.");
                }
                exit(1);
            }
            grid_->setClean(false);
            setup_ = true;
        }

        void compute
        () {
            start_ = std::chrono::steady_clock::now();
            computeInternal();
            end_ = std::chrono::steady_clock::now();
            time_ = std::chrono::duration_cast<std::chrono::milliseconds>(end_-start_).count();
        }

        virtual void computeInternal() = 0;

        template<class T>
        T* as
        () {
            // OMPL-inspired function.
            /* Make sure the type we are casting to is a solver */
            BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Solver*>));
            return static_cast<T*>(this);
        }

        template<class T>
        const T* as
        () const {
            // OMPL-inspired function.
            /* Make sure the type we are casting to is a solver */
            BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Solver*>));
            return static_cast<const T*>(this);
        }


        const std::string& getName() const
        {
            return name_;
        }

        virtual void clear
        () {
            init_points_.clear();
            goal_idx_ = -1;
            setup_ = false;
        }

        virtual void reset
        () {
            setup_ = false;
            grid_->clean();
        }

        grid_t* getGrid() const
        {
            return grid_;
        }

        virtual double getTime
        () const {
            return time_;
        }

        virtual void printRunInfo
        () const {
            console::warning("No run info available.");
        }

    protected:
        int sanityChecks
        () {
            if (grid_ == NULL) return 1;
            if (!grid_->isClean()) return 2;
            if (init_points_.empty()) return 3;

            // When more that 1 initial point is given, this check is ommitted
            // since it could be FM2-like velocities map computation.
            if (init_points_.size() == 1 &&
                grid_->getCell(init_points_[0]).isOccupied()) return 4;

            if(int(goal_idx_) != -1 && grid_->getCell(goal_idx_).isOccupied()) return 5;

            for (int ip : init_points_)
                if(int(goal_idx_) == ip) return 6;

            return 0;
        }

        grid_t*                     grid_;

        std::string                 name_;

        bool                        setup_;

        std::vector<unsigned int>   init_points_;

        unsigned int                goal_idx_;

        std::chrono::time_point<std::chrono::steady_clock> start_, end_;

        double                      time_;
};

#endif /* SOLVER_H_*/