//* Potential field-based path finding algorithm class - header file *
#ifndef _NAV_MAP_POTENTIAL
#define _NAV_MAP_POTENTIAL

#include "../Aria/include/Aria.h"
#include "nav_map_baset.h"
#include "nav_map.h"
#include <vector>


class NAV_FIELD_Potential {
public:
    NAV_FIELD_Potential  (NAV_Map *RobotMap, const ArPose &goal=ArPose(), double resol=100, double robotradius=200)
                        : MAPS(0),
                          rradius(robotradius), cgoal(goal), ugoal(true),
                          myMutex(), VAL_CLOSE(2+static_cast<double>(3*robotradius/resol)),
                          Katr(1), Kreprel(1E4), Rrep(10*rradius), Rrep_inv(1/Rrep),
                          cache_is_valid(false)
                        { MAPS.push_back(RobotMap); recalculate(); }

public:
    void        register_map(NAV_Map *RobotMap);
    bool        unregister_map(NAV_Map *RobotMap);
    void        setGoal(const ArPose &goal)             { cgoal=goal; recalculate(); } 
    void        recalculate()                           { cache_is_valid=false; /* otherwise, do nothing */ }
    void        setUseGoal(bool use)                    { cache_is_valid=(ugoal==use); ugoal=use; }

public:
    double      getResolution() const                   { return(MAPS[0]->getResolution()); }
    const ArPose& getGoal   () const                    { return(cgoal); }

public:
    // ArPose interface
//  bool        is_reachable(const ArPose &pos)  const  { return(???); }
    bool        is_goal     (const ArPose &pos)  const  { return( cgoal.findDistanceTo(pos)<=MAPS[0]->getResolution() ); }
    bool        is_close    (const ArPose &pos)  const  { return( cgoal.findDistanceTo(pos)<=VAL_CLOSE ); }
    double      getValue    (const ArPose &pos)         { return( getValue(pos.getX(), pos.getY()) ); }
    double      getHeading  (const ArPose &pos)         { return( getHeading(pos.getX(), pos.getY()) ); }
    double      getOmegaCoef(const ArPose &pos)         { return( getOmegaCoef(pos.getX(), pos.getY(),pos.getTh()) ); }
    double      getVelCoef  (const ArPose &pos)         { return( getVelCoef(pos.getX(), pos.getY()) ); }

    // direct coordiantes interface
//  bool        is_reachable(double x, double y) const  { return( ??? ) }
    bool        is_goal     (double x, double y) const  { return( is_goal(ArPose(x,y)) ); }
    bool        is_close    (double x, double y) const  { return( is_close(ArPose(x,y)) ); }
    double      getValue    (double x, double y); 
    double      getHeading  (double x, double y);
    double      getVelCoef  (double x, double y);
    double      getOmegaCoef(double x, double y, double theta);

protected:
    void        calculate_at(double x, double y);   // this actually does all calculations and updates the cached result

public: // you must lock the map before using it and unlock after you are done with it if using in multithreaded application
    void        lock        () { myMutex.lock();    }
    void        unlock      () { myMutex.unlock();  }

protected:
    std::vector<NAV_Map*>   MAPS;   // pointer to at least one or possibly more maps kept (and updated) by the robot
    bool    ugoal;                  // use goal feature and attraction (true) or not (false)
    ArPose  cgoal;                  // current goal used for navigation
    double  rradius;                // current robot radius to enlarge obstacles, set to 0 to disable
    ArMutex myMutex;                // lock/unlock for multithreading
    double  VAL_CLOSE;              // number of grid units to activate "robot close to" behavior
    double  Katr, Kreprel;          // coefficients for attraction and repulsion fields
    double  Rrep, Rrep_inv;         // maximum distance for repulsion to be experienced

    // cached last calculated value
    bool    cache_is_valid;
    double  cache_x, cache_y;
    double  cache_e, cache_dx, cache_dy;
};

#endif