// EuclideanDistance.hpp
//
//     Created on: 15. 03. 2018
//         Author: Jan Uhlik
//    Modified by:
//
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library

#ifndef ALIB2_EUCLIDEANDISTANCE_HPP
#define ALIB2_EUCLIDEANDISTANCE_HPP

#include <cmath>
#include <alib/pair>
#include <functional>

namespace graph {

namespace heuristic {

class EuclideanDistance {
// ---------------------------------------------------------------------------------------------------------------------

 public:
  template<typename TCoordinate>
  static double euclideanDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
                                  const ext::pair<TCoordinate, TCoordinate> &node);

// ---------------------------------------------------------------------------------------------------------------------

  template<typename TCoordinate>
  static std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
                              const ext::pair<TCoordinate, TCoordinate> &)> euclideanDistanceFunction();

// ---------------------------------------------------------------------------------------------------------------------

};

// =====================================================================================================================

template<typename TCoordinate>
double EuclideanDistance::euclideanDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
                                            const ext::pair<TCoordinate, TCoordinate> &node) {
  return std::sqrt(
      (node.first - goal.first) * (node.first - goal.first) +
          (node.second - goal.second) * (node.second - goal.second));
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate>
std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
                     const ext::pair<TCoordinate, TCoordinate> &)> EuclideanDistance::euclideanDistanceFunction() {
  return (double (*)(const ext::pair<TCoordinate, TCoordinate> &,
                     const ext::pair<TCoordinate, TCoordinate> &)) euclideanDistance;
}

// ---------------------------------------------------------------------------------------------------------------------

} // namespace heuristic

} // namespace graph

#endif //ALIB2_EUCLIDEANDISTANCE_HPP
