// DiagonalDistance.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_DIAGONALDISTANCE_HPP
#define ALIB2_DIAGONALDISTANCE_HPP

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

namespace graph {

namespace heuristic {

class DiagonalDistance {
// ---------------------------------------------------------------------------------------------------------------------
 public:
  template<typename TCoordinate>
  static double diagonalDistance(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> &)> diagonalDistanceFunction();

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

};

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

template<typename TCoordinate>
double DiagonalDistance::diagonalDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
                                          const ext::pair<TCoordinate, TCoordinate> &node) {
  TCoordinate d_max = std::max(std::abs(node.first - goal.first), std::abs(node.second - goal.second));
  TCoordinate d_min = std::min(std::abs(node.first - goal.first), std::abs(node.second - goal.second));

  return M_SQRT2 * d_min + (d_max - d_min);
}

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

template<typename TCoordinate>
std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
                     const ext::pair<TCoordinate, TCoordinate> &)> DiagonalDistance::diagonalDistanceFunction() {
  return (double (*)(const ext::pair<TCoordinate, TCoordinate> &,
                     const ext::pair<TCoordinate, TCoordinate> &)) diagonalDistance;
}

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

} // namespace heuristic

} // namespace graph

#endif //ALIB2_DIAGONALDISTANCE_HPP
