/**
 * MULTIDRONE Project:
 *
 * Geographic to Cartesian coordinate conversion and vice versa, with an intermediate conversion to UTM.
 *
 * This library suppose that the path of the robot cannot cross more than 2 different UTM zones.
 *
 */

#ifndef GEOGRAPHIC_TO_CARTESIAN
#define GEOGRAPHIC_TO_CARTESIAN

#include <math.h>
#include <algorithm>
#include <stdlib.h>
#include <geometry_msgs/Point32.h>
#include <geographic_msgs/GeoPoint.h>

#include <geodesy/utm.h>                // IMPORTANT TO INSTALL GEODESY: sudo apt-get install ros-kinetic-geographic-info

// #define USING_IN_PYTHON

// #ifdef USING_IN_PYTHON
// #include <boost/python.hpp>
// #endif

namespace multidrone {

inline geometry_msgs::Point32 geographic_to_cartesian (const geographic_msgs::GeoPoint& _actual_coordinate_geo, const geographic_msgs::GeoPoint& _origin_geo) {

    geodesy::UTMPoint actual_coordinate_UTM(_actual_coordinate_geo); // Conversion from geographic coordinates to UTM.
    geodesy::UTMPoint origin_UTM(_origin_geo);                       // Conversion from geographic coordinates to UTM.

    geometry_msgs::Point32 actual_coordinate_cartesian;              // Cartesian coordinate that this function will return.

    // The problem with UTM is that if there are coordinates in more than one zone, it's difficult to merge the coordinates of the different zones.
    // This is because each zone has 6 degrees of longitude, and the width in meters of the zone is variable from equator to the poles.
    // Something similar when the coordinates are in different hemispheres, separated by the equator. Each hemisphere has a different origin for the y axis.
    // These are the reasons why the x and y assignation for the Cartesian conversion isn't a simple UTM substraction.

    // Assignating "actual_coordinate_cartesian.x" is tricky when the actual coordinate and the origin of the Cartesian coordinates are on different zones.
    if ( int(actual_coordinate_UTM.zone)==60 && origin_UTM.zone==1 ) {         // Coordinate and origin separated by the +-180º longitude
        geographic_msgs::GeoPoint geo_180_w;
        geographic_msgs::GeoPoint geo_180_e;
        geo_180_w.longitude = 179.9999999;
        geo_180_w.latitude = _actual_coordinate_geo.latitude;
        geo_180_w.altitude = _actual_coordinate_geo.altitude;
        geo_180_e.longitude = -179.9999999;
        geo_180_e.latitude = _actual_coordinate_geo.latitude;
        geo_180_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_180_w(geo_180_w);
        geodesy::UTMPoint utm_180_e(geo_180_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_w.easting + utm_180_e.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
    } else if ( origin_UTM.zone==60 && int(actual_coordinate_UTM.zone)==1 ) {  // Coordinate and origin separated by the +-180º longitude
        geographic_msgs::GeoPoint geo_180_w;
        geographic_msgs::GeoPoint geo_180_e;
        geo_180_w.longitude = 179.9999999;
        geo_180_w.latitude = _actual_coordinate_geo.latitude;
        geo_180_w.altitude = _actual_coordinate_geo.altitude;
        geo_180_e.longitude = -179.9999999;
        geo_180_e.latitude = _actual_coordinate_geo.latitude;
        geo_180_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_180_w(geo_180_w);
        geodesy::UTMPoint utm_180_e(geo_180_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_e.easting + utm_180_w.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
    } else if ( int(actual_coordinate_UTM.zone) < origin_UTM.zone ) {
        int quotient_from_int_division = (int) ( std::max(std::abs(_actual_coordinate_geo.longitude),std::abs(_origin_geo.longitude))/6 );                    // int division of the max longitude (absolute, without sign)
        double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(_actual_coordinate_geo.longitude,_origin_geo.longitude)));   // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
        geographic_msgs::GeoPoint geo_w;
        geographic_msgs::GeoPoint geo_e;
        geo_w.longitude = border_longitude - 0.0000001;
        geo_w.latitude = _actual_coordinate_geo.latitude;
        geo_w.altitude = _actual_coordinate_geo.altitude;
        geo_e.longitude = border_longitude + 0.0000001;
        geo_e.latitude = _actual_coordinate_geo.latitude;
        geo_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_w(geo_w);
        geodesy::UTMPoint utm_e(geo_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_w.easting + utm_e.easting - origin_UTM.easting;         // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
    } else if ( origin_UTM.zone < int(actual_coordinate_UTM.zone) ) {
        int quotient_from_int_division = (int) ( std::max(std::abs(_actual_coordinate_geo.longitude),std::abs(_origin_geo.longitude))/6 );                    // int division of the max longitude (absolute, without sign)
        double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(_actual_coordinate_geo.longitude,_origin_geo.longitude)));   // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
        geographic_msgs::GeoPoint geo_w;
        geographic_msgs::GeoPoint geo_e;
        geo_w.longitude = border_longitude - 0.0000001;
        geo_w.latitude = _actual_coordinate_geo.latitude;
        geo_w.altitude = _actual_coordinate_geo.altitude;
        geo_e.longitude = border_longitude + 0.0000001;
        geo_e.latitude = _actual_coordinate_geo.latitude;
        geo_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_w(geo_w);
        geodesy::UTMPoint utm_e(geo_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_e.easting + utm_w.easting - origin_UTM.easting;    // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
    } else {
        // The actual coordinate is in the same zone that the origin (the first station). This is the normal situation, the assigntation of the x axis value is trivial (simple subtraction).
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting -origin_UTM.easting;
    }

    // Assignating "actual_coordinate_cartesian.y" is also tricky when the actual coordinate and the origin of the Cartesian coordinates are on different hemispheres:
    if ( origin_UTM.band=='N' && actual_coordinate_UTM.band=='M' ) {         // Coordinate and origin separated by the 0º latitude
        geographic_msgs::GeoPoint geo_0_n;
        geographic_msgs::GeoPoint geo_0_s;
        geo_0_n.longitude = _actual_coordinate_geo.longitude;
        geo_0_n.latitude = 0.0000001;
        geo_0_n.altitude = _actual_coordinate_geo.altitude;
        geo_0_s.longitude = _actual_coordinate_geo.longitude;
        geo_0_s.latitude = -0.0000001;
        geo_0_s.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_0_n(geo_0_n);
        geodesy::UTMPoint utm_0_s(geo_0_s);
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_s.northing + utm_0_n.northing - origin_UTM.northing;    // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_s.northing + utm_0_n.northing " computes the y step in both sides of the equator.
    } else if ( actual_coordinate_UTM.band=='N' && origin_UTM.band=='M' ) {  // Coordinate and origin separated by the 0º latitude
        geographic_msgs::GeoPoint geo_0_n;
        geographic_msgs::GeoPoint geo_0_s;
        geo_0_n.longitude = _actual_coordinate_geo.longitude;
        geo_0_n.latitude = 0.0000001;
        geo_0_n.altitude = _actual_coordinate_geo.altitude;
        geo_0_s.longitude = _actual_coordinate_geo.longitude;
        geo_0_s.latitude = -0.0000001;
        geo_0_s.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_0_n(geo_0_n);
        geodesy::UTMPoint utm_0_s(geo_0_s);
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_n.northing + utm_0_s.northing - origin_UTM.northing;    // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_n.northing + utm_0_s.northing " computes the y step in both sides of the equator.
    } else {
        // The actual coordinate is in the same hemisphere that the origin (the first station). This is the normal situation, the assigntation of the y axis value is trivial (simple subtraction).
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing-origin_UTM.northing;
    }

    // Assignating "actual_coordinate_cartesian.z" is trivial always, simple subtraction.
    actual_coordinate_cartesian.z = actual_coordinate_UTM.altitude-origin_UTM.altitude;

    return actual_coordinate_cartesian;
}   // end geographic_to_cartesian

inline geographic_msgs::GeoPoint cartesian_to_geographic (const geometry_msgs::Point32& _actual_coordinate_cartesian, const geographic_msgs::GeoPoint& _origin_geo) {
    geodesy::UTMPoint actual_coordinate_aux(_origin_geo);                                               // Conversion from geographic coordinates to UTM.

    actual_coordinate_aux.easting  += _actual_coordinate_cartesian.x;
    actual_coordinate_aux.northing += _actual_coordinate_cartesian.y;
    actual_coordinate_aux.altitude += _actual_coordinate_cartesian.z;

    // Convert to geographic coordinates and return the coordinate. This conversion is easier as it doesn't matter if the points are in different zones or hemispheres, but in that case the error increases if the points are very far from each other.
    return geodesy::toMsg (actual_coordinate_aux);

}   // end cartesian_to_geographic

// #ifdef USING_IN_PYTHON
// BOOST_PYTHON_MODULE(geographic_to_cartesian_and_vice_verse) {
//     using namespace boost::python;
//     def("geographic_to_cartesian", geographic_to_cartesian);
//     def("cartesian_to_geographic", cartesian_to_geographic);
// }
// #endif

}   // end namespace multidrone

#endif // GEOGRAPHIC_TO_CARTESIAN


/**
 * \file RemezBasedFilter.cpp
 */

#include <vector>

#include <boost/math/constants/constants.hpp>

#include <Eigen/Dense>

#include <ATK/Core/Utilities.h>
#include <ATK/EQ/RemezBasedFilter.h>
#include <ATK/EQ/FIRFilter.h>

#include <ATK/Utility/FFT.h>

namespace
{
  template<class DataType>
  class RemezBuilder
  {
  public:
    using AlignedScalarVector = typename ATK::TypedBaseFilter<DataType>::AlignedScalarVector;
  private:
    const static gsl::index grid_size = 1024; // grid size, power of two better for FFT
    constexpr static DataType SN = 1e-8;

    gsl::index M;
    std::vector<DataType> grid;
    std::vector<std::pair<std::pair<DataType, DataType>, std::pair<DataType, DataType>> > target;

    /// Computed coefficients
    AlignedScalarVector coeffs;
    /// Selected indices
    std::vector<gsl::index> indices;
    /// Weight function on the grid
    std::vector<DataType> weights;
    /// Objective function on the grid
    std::vector<DataType> objective;
    /// Alternate signs
    std::vector<int> s;

    ATK::FFT<DataType> fft_processor;

  public:
    RemezBuilder(gsl::index order, const std::vector<std::pair<std::pair<DataType, DataType>, std::pair<DataType, DataType>> >& target)
    :M(order / 2), target(target)
    {
      grid.resize(grid_size);
      for(gsl::index i = 0; i < grid_size; ++i)
      {
        grid[i] = i * boost::math::constants::pi<DataType>() / grid_size;
      }
      fft_processor.set_size(2 * grid_size);
    }

    void init()
    {
      coeffs.assign(M * 2 + 1, 0);
      indices.assign(M + 2, -1);
      s.assign(M+2, 0);

      weights.assign(grid_size, 0);
      objective.assign(grid_size, 0);

      int current_template = 0;
      for(gsl::index i = 0; i < grid_size; ++i)
      {
        auto reduced_freq = grid[i] / boost::math::constants::pi<DataType>();
        if(reduced_freq > target[current_template].first.second && current_template + 1 < target.size())
        {
          ++current_template;
        }
        if (reduced_freq < target[current_template].first.first || reduced_freq > target[current_template].first.second)
        {
          weights[i] = 0;
          objective[i] = 0;
        }
        else
        {
          weights[i] = target[current_template].second.second;
          objective[i] = target[current_template].second.first;
        }
      }
      int flag = -1;
      for (gsl::index i = 0; i < M + 2; ++i)
      {
        s[i] = flag;
        flag = -flag;
      }
      indices = set_starting_conditions();
    }

    std::vector<gsl::index> set_starting_conditions() const
    {
      std::vector<gsl::index> indices;

      std::vector<gsl::index> valid_indices;
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        if (weights[i] != 0)
        {
          valid_indices.push_back(i);
        }
      }

      for (gsl::index i = 0; i < M + 2; ++i)
      {
        indices.push_back(valid_indices[std::lround(valid_indices.size() / (M + 4.) * (i + 1))]);
      }

      return indices;
    }

    AlignedScalarVector build()
    {
      if(target.empty())
      {
        coeffs.clear();
        return coeffs;
      }
      init();
      while(true)
      {
        Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> A(M + 2, M + 2);
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          for (gsl::index j = 0; j < M + 1; ++j)
          {
            A(i, j) = std::cos(grid[indices[i]] * j);
          }
        }
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          A(i, M + 1) = s[i] / weights[indices[i]];
        }

        Eigen::Matrix<DataType, Eigen::Dynamic, 1> b(M + 2, 1);
        for (gsl::index i = 0; i < M + 2; ++i)
        {
          b(i) = objective[indices[i]];
        }
        Eigen::Matrix<DataType, Eigen::Dynamic, 1> x = A.colPivHouseholderQr().solve(b);

        for (gsl::index i = 0; i < M; ++i)
        {
          coeffs[i] = coeffs[2 * M - i] = x[M - i] / 2;
        }
        coeffs[M] = x[0];
        auto delta = std::abs(x[M + 1]); // maximum cost

        auto newerr = compute_new_error();
        auto new_indices = locmax(newerr);

        filter_SN(delta, new_indices, newerr);
        filter_monotony(new_indices, newerr);

        DataType max = 0;
        for (auto indice : new_indices)
        {
          max = std::max(max, std::abs(newerr[indice]));
        }

        if ((max - delta) / delta < SN)
        {
          break;
        }

        indices = std::move(new_indices);
      }

      return coeffs;
    }

  private:
    /// Creates a spectral response for a given set of coeffs through an FFT
    std::vector<DataType> firamp() const
    {
      std::vector<std::complex<DataType>> output(grid_size * 2);
      fft_processor.process_forward(coeffs.data(), output.data(), coeffs.size());
      std::vector<DataType> amp(grid_size);
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        amp[i] = (std::complex<DataType>(std::cos(M * grid[i]), std::sin(M * grid[i])) * output[i]).real() * grid_size * 2;
      }
      return amp;
    }

    std::vector<DataType> compute_new_error() const
    {
      auto fir_result = firamp();
      std::vector<DataType> newerr(grid_size);
      for (gsl::index i = 0; i < grid_size; ++i)
      {
        newerr[i] = (fir_result[i] - objective[i]) * weights[i];
      }

      return newerr;
    }

    // Finds min and max
    std::vector<gsl::index> locmax(const std::vector<DataType>& data) const
    {
      std::vector<gsl::index> v;

      std::vector<DataType> temp1;
      std::vector<DataType> temp2;
      temp1.push_back(data[0] - 1);
      for (gsl::index i = 0; i < data.size() - 1; ++i)
      {
        temp1.push_back(data[i]);
        temp2.push_back(data[i + 1]);
      }
      temp2.push_back(data.back() - 1);

      for (gsl::index i = 0; i < data.size(); ++i)
      {
        if ((data[i] > temp1[i]) && (data[i] > temp2[i]))
        {
          v.push_back(i);
        }
      }

      temp1.clear();
      temp2.clear();
      temp1.push_back(-data[0] - 1);
      for (gsl::index i = 0; i < data.size() - 1; ++i)
      {
        temp1.push_back(-data[i]);
        temp2.push_back(-data[i + 1]);
      }
      temp2.push_back(-data.back() - 1);

      for (gsl::index i = 0; i < data.size(); ++i)
      {
        if ((-data[i] > temp1[i]) && (-data[i] > temp2[i]))
        {
          v.push_back(i);
        }
      }

      std::sort(v.begin(), v.end());

      return v;
    }

    void filter_SN(DataType delta, std::vector<gsl::index>& indices, const std::vector<DataType>& err) const
    {
      std::vector<gsl::index> new_indices;

      for (auto indice : indices)
      {
        if (std::abs(err[indice]) > (delta - SN))
        {
          new_indices.push_back(indice);
        }
      }

      indices = std::move(new_indices);
    }

    std::vector<gsl::index> etap(const std::vector<DataType>& data) const
    {
      std::vector<gsl::index> v;
      auto xe = data[0];
      gsl::index xv = 0;
      for (gsl::index i = 1; i < data.size(); ++i)
      {
        if (std::signbit(data[i]) == std::signbit(xe))
        {
          if (std::abs(data[i]) > std::abs(xe))
          {
            xe = data[i];
            xv = i;
          }
        }
        else
        {
          v.push_back(xv);
          xe = data[i];
          xv = i;
        }
      }
      v.push_back(xv);
      return v;
    }

    void filter_monotony(std::vector<gsl::index>& indices, const std::vector<DataType>& err) const
    {
      std::vector<DataType> filtered_err;
      for (auto indice : indices)
      {
        filtered_err.push_back(err[indice]);
      }

      auto selected_indices = etap(filtered_err);
      std::vector<gsl::index> new_indices;
      for (gsl::index i = 0; i < M + 2; ++i)
      {
        new_indices.push_back(indices[selected_indices[i]]);
      }
      indices = std::move(new_indices);
    }
  };
}

namespace ATK
{
  template<class DataType>
  RemezBasedCoefficients<DataType>::RemezBasedCoefficients(gsl::index nb_channels)
    :Parent(nb_channels, nb_channels)
  {
  }

  template<class DataType>
  RemezBasedCoefficients<DataType>::RemezBasedCoefficients(RemezBasedCoefficients&& other)
    :Parent(std::move(other)), target(std::move(other.target)), in_order(std::move(other.in_order)), coefficients_in(std::move(other.coefficients_in))
  {
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::set_template(const std::vector<std::pair<std::pair<CoeffDataType, CoeffDataType>, std::pair<CoeffDataType, CoeffDataType> > >& target)
  {
    this->target = target;
    setup();
  }

  template<class DataType>
  const std::vector<std::pair<std::pair<typename RemezBasedCoefficients<DataType>::CoeffDataType, typename RemezBasedCoefficients<DataType>::CoeffDataType>, std::pair<typename RemezBasedCoefficients<DataType>::CoeffDataType, typename RemezBasedCoefficients<DataType>::CoeffDataType> > >& RemezBasedCoefficients<DataType>::get_template() const
  {
    return target;
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::set_order(gsl::index order)
  {
    if(order % 2 == 1)
    {
      throw ATK::RuntimeError("Need an even filter order (considering order 0 has 1 coefficients)");
    }
    in_order = order;
    setup();
  }

  template<class DataType>
  void RemezBasedCoefficients<DataType>::setup()
  {
    Parent::setup();

    std::sort(target.begin(), target.end());
    for(gsl::index i = 0; i + 1 < target.size(); ++i)
    {
      if(target[i].first.second > target[i + 1].first.first)
      {
        target.clear();
        throw ATK::RuntimeError("Bad template");
      }
    }

    if (in_order > 0)
    {
      RemezBuilder<CoeffDataType> builder(in_order, target);
      coefficients_in = builder.build();
    }
  }

  template class ATK_EQ_EXPORT RemezBasedCoefficients<double>;
#if ATK_ENABLE_INSTANTIATION
  template class ATK_EQ_EXPORT RemezBasedCoefficients<std::complex<double> >;
#endif
}


#ifndef VECTORQUANTIZER_HPP
#define VECTORQUANTIZER_HPP

#include <Eigen/Dense>
#include "../iterator/iterator.hpp"
/**
 * @brief plain old vector quantizer
 */
template<typename T, uint D, uint C>
class vectorquantizer {

    typedef Eigen::Matrix < T, D, 1 > vec_t;

    static_assert( ((C % 2 == 0) || ( C == 1)), "vectorquantizer: cells = 0 mod 2 failed or C==1");

public:

    // ================================================================================
    // Methods
    // ================================================================================

    vectorquantizer(){
        _raw_centroids = new T[C * D];
        _centroids = Eigen::Map<Eigen::Matrix<T, C, D>>(_raw_centroids, C, D);

    }

    ~vectorquantizer() {
        delete[] _raw_centroids;

    }

    void getAssignment(iterator<T, D> &iter) {
        // for each vector ...
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            // find minimum
            uint bestIdx = 0;
            T bestDist = HUGE_VAL;
            vec_t vec = iter[n];
            // for each cluster
            for (uint c = 0, c_e = _step; c < c_e; ++c) {
                vec_t cec = _centroids.row(c);
                T curDist = (vec - cec).squaredNorm();
                if ( curDist  < bestDist ) {
                    bestDist = curDist;
                    bestIdx = c;
                }
            }
            _mapping[n] = bestIdx;
            _distances[n] = bestDist;
        }
    }

    void updateCentroids(iterator<T, D> &iter) {
        _centroids = Eigen::Matrix<T, C, D>::Zero(C, D);
        T centerCounter[C] = {0};
        // find mean
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            const uint c = _mapping[n];
            _centroids.row(c) += iter[n];
            ++centerCounter[c];
        }
        for (uint c = 0; c < C; ++c) {
            if (centerCounter[c] != 0)
                _centroids.row(c).array() /= centerCounter[c];

        }
    }

    void augmentCentroids() {
        for (uint i = 0; i < _step; ++i) {
            _centroids.row(i + _step) = _centroids.row(i).array() + 0.001;
            _centroids.row(i) = _centroids.row(i).array() - 0.001;
        }
        _step *= 2;
    }

    T loss(iterator<T, D> &iter) {
        Eigen::MatrixXf l = Eigen::Map<  Eigen::MatrixXf >(_distances, iter.num(), 1);
        return l.sum();
    }

    uint id(const vec_t &vec) {
        // compute all l1 distances (row=part)
        for (uint c = 0; c < C; ++c) {
            const Eigen::Matrix< T, D, 1> cec = _centroids.row(c);
            const Eigen::Matrix< T, D, 1> diff = vec - cec;
            _L1distances[c] = diff.squaredNorm();
            _L1order[c] = c;

        }

        // now sort them according their distances (sort each row)
        auto comparator = [&](const uint8_t &lhs, const uint8_t &rhs) -> bool {
            return _L1distances[lhs] < _L1distances[rhs];
        };

        std::sort(_L1order, _L1order + C, comparator);

        return _L1order[0];

    }

    void dist(const vec_t &vec) {
        // compute all l1 distances (row=part)
        for (uint c = 0; c < C; ++c) {
            const Eigen::Matrix< T, D, 1> cec = _centroids.row(c);
            const Eigen::Matrix< T, D, 1> diff = vec - cec;
            _L1distances[c] = diff.squaredNorm();
            _L1order[c] = c;

        }

    }

    void generate(iterator<T, D> &iter) {
        _step = 1;

        _mapping = new uint8_t[iter.num()]();
        _distances = new T[iter.num()]();

        _centroids.row(0) = iter.center();

        T currentLoss = 0.0;
        T lastLoss    = 0.0;

        if (C == 1) {
            getAssignment(iter);    // E step
            updateCentroids(iter);  // M step
            return;                  // do nothing
        }

        do {
            augmentCentroids();
            do {
                lastLoss = currentLoss;
                getAssignment(iter);    // E step
                updateCentroids(iter);  // M step
                currentLoss = loss(iter);
            } while ((abs(lastLoss - currentLoss) > 0.005));
        } while (_step < C );

        delete[] _mapping;
        delete[] _distances;
    }

    // ================================================================================
    // Variables
    // ================================================================================

    Eigen::Matrix<T, C, D> _centroids;
    T *_raw_centroids;
    uint _step;
    uint8_t *_mapping;
    T *_distances;

    T _L1distances[C];
    uint _L1order[C];

};

#endif


#include "teca_2d_component_area.h"

#include "teca_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh.h"

#include <algorithm>
#include <iostream>
#include <deque>
#include <set>
#define _USE_MATH_DEFINES
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

namespace {

template <typename component_t>
component_t get_max_component_id(unsigned long n, const component_t *labels)
{
    component_t max_component_id = std::numeric_limits<component_t>::lowest();
    for (unsigned long i = 0; i < n; ++i)
    {
        component_t label = labels[i];
        max_component_id = label > max_component_id ? label : max_component_id;
    }
    return max_component_id;
}

// visit each node in the mesh, the node is treated as a cell in
// the dual mesh defined by mid points between nodes. the area of
// the cell is added to the corresponding label. this formulation
// requires a layer of ghost nodes
//
// The exact area of the sperical rectangular patch A_i is given by:
//
// A_i = rho^2(cos(phi_0) - cos(phi_1))(theta_1 - theta_0)
//
//     = rho^2(sin(rad_lat_0) - sin(rad_lat_1))(theta_1 - theta_0)
//
// where
//
//   theta = deg_lon * pi/180
//   phi = pi/2 - rad_lat
//   rad_lat = deg_lat * pi/180
//   sin(rad_lat) = cos(pi/2 - rad_lat)
//
template<typename coord_t, typename component_t, typename container_t>
void component_area(unsigned long nlon, unsigned long nlat,
    const coord_t *deg_lon, const coord_t *deg_lat, const component_t *labels,
    container_t &area)
{
    // This calculation is sensative to floating point precision and
    // should be done in double precision
    using calc_t = double;

    calc_t R_e = 6378.1370; // km
    calc_t R_e_sq = R_e*R_e;
    calc_t rad_per_deg = M_PI/180.0;

    unsigned long nlonm1 = nlon - 1;
    unsigned long nlatm1 = nlat - 1;

    // convert to spherical coordinates in units of radians,
    // move to the dual mesh, and pre-compute factors in A
    calc_t *rho_sq_d_theta = (calc_t*)malloc(nlon*sizeof(calc_t));
    rho_sq_d_theta[0] = calc_t();
    for (unsigned long i = 1; i < nlonm1; ++i)
        rho_sq_d_theta[i] = R_e_sq*calc_t(0.5)*(deg_lon[i + 1] - deg_lon[i - 1])*rad_per_deg;
    rho_sq_d_theta[nlonm1] = calc_t();

    calc_t *rad_lat = (calc_t*)malloc(nlat*sizeof(calc_t));
    for (unsigned long j = 0; j < nlat; ++j)
        rad_lat[j] = deg_lat[j]*rad_per_deg;

    calc_t *d_cos_phi = (calc_t*)malloc(nlat*sizeof(calc_t));
    for (unsigned long j = 1; j < nlatm1; ++j)
    {
        calc_t cos_phi_1 = sin(calc_t(0.5)*(rad_lat[j - 1] + rad_lat[j]));
        calc_t cos_phi_0 = sin(calc_t(0.5)*(rad_lat[j] + rad_lat[j + 1]));
        d_cos_phi[j] = cos_phi_0 - cos_phi_1;
    }
    d_cos_phi[0] = calc_t();
    d_cos_phi[nlatm1] = calc_t();

    // finish off the calc by multiplying the factors
    for (unsigned long j = 1; j < nlatm1; ++j)
    {
        calc_t d_cos_phi_j = d_cos_phi[j];
        unsigned long jj = j*nlon;
        for (unsigned long i = 1; i < nlonm1; ++i)
        {
            area[labels[jj + i]] += rho_sq_d_theta[i]*d_cos_phi_j;
        }
    }

    free(rad_lat);
    free(d_cos_phi);
    free(rho_sq_d_theta);
}

}

// --------------------------------------------------------------------------
teca_2d_component_area::teca_2d_component_area() :
    component_variable(""), contiguous_component_ids(0), background_id(-1)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_2d_component_area::~teca_2d_component_area()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_2d_component_area::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_2d_component_area":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_variable,
            "name of the varibale containing region labels")
        TECA_POPTS_GET(int, prefix, contiguous_component_ids,
            "when the region label ids start at 0 and are consecutive "
            "this flag enables use of an optimization")
        TECA_POPTS_GET(long, prefix, background_id,
            "the label id that corresponds to the background")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_2d_component_area::set_properties(const std::string &prefix,
    variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, component_variable)
    TECA_POPTS_SET(opts, int, prefix, contiguous_component_ids)
    TECA_POPTS_SET(opts, long, prefix, background_id)
}
#endif

// --------------------------------------------------------------------------
int teca_2d_component_area::get_component_variable(std::string &component_var)
{
    if (this->component_variable.empty())
        return -1;

    component_var = this->component_variable;
    return 0;
}

// --------------------------------------------------------------------------
teca_metadata teca_2d_component_area::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::get_output_metadata" << endl;
#endif
    (void) port;

    teca_metadata md = input_md[0];
    return md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_2d_component_area::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::get_upstream_request" << endl;
#endif
    (void) port;
    (void) input_md;

    std::vector<teca_metadata> up_reqs;

    // get the name of the array to request
    std::string component_var;
    if (this->get_component_variable(component_var))
    {
        TECA_FATAL_ERROR("component_variable was not specified")
        return up_reqs;
    }

    // pass the incoming request upstream, and
    // add in what we need
    teca_metadata req(request);
    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);
    arrays.insert(component_var);

    req.set("arrays", arrays);

    // send up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_2d_component_area::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_2d_component_area::execute" << endl;
#endif
    (void)port;
    (void)request;

    // get the input
    const_p_teca_cartesian_mesh in_mesh =
        std::dynamic_pointer_cast<const teca_cartesian_mesh>(
            input_data[0]);
    if (!in_mesh)
    {
        TECA_FATAL_ERROR("empty input, or not a cartesian_mesh")
        return nullptr;
    }

    // create output and copy metadata, coordinates, etc
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    // get the input array
    std::string component_var;
    if (this->get_component_variable(component_var))
    {
        TECA_FATAL_ERROR("component_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array component_array
        = out_mesh->get_point_arrays()->get(component_var);
    if (!component_array)
    {
        TECA_FATAL_ERROR("label variable \"" << component_var
            << "\" is not in the input")
        return nullptr;
    }

    // get mesh dimension
    unsigned long extent[6];
    out_mesh->get_extent(extent);

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nxy = nx*ny;

    unsigned long nz = extent[5] - extent[4] + 1;
    if (nz != 1)
    {
        TECA_FATAL_ERROR("This calculation requires 2D data. The current dataset "
            "extents are [" << extent[0] << ", " << extent[1] << ", "
            << extent[2] << ", " << extent[3] << ", " << extent[4] << ", "
            << extent[5] << "]")
        return nullptr;
    }

    // get the coordinate axes
    const_p_teca_variant_array xc = in_mesh->get_x_coordinates();
    const_p_teca_variant_array yc = in_mesh->get_y_coordinates();

    // get the input and output metadata
    teca_metadata &in_metadata =
        const_cast<teca_metadata&>(in_mesh->get_metadata());

    teca_metadata &out_metadata = out_mesh->get_metadata();

    // get the background_id, and pass it through
    long bg_id = this->background_id;
    if (this->background_id == -1)
    {
        if (in_metadata.get("background_id", bg_id))
        {
            TECA_FATAL_ERROR("Metadata is missing the key \"background_id\". "
                "One should specify it via the \"background_id\" algorithm "
                "property")
            return nullptr;
        }
    }
    out_metadata.set("background_id", bg_id);

    // calculate area of components
    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        xc.get(),
        _COORD,
        // the calculation is sensative to floating point precision
        // and should be made in double precision
        using calc_t = double;

        const NT_COORD *p_xc = static_cast<TT_COORD*>(xc.get())->get();
        const NT_COORD *p_yc = static_cast<TT_COORD*>(yc.get())->get();

        NESTED_TEMPLATE_DISPATCH_I(const teca_variant_array_impl,
            component_array.get(),
            _LABEL,

            const NT_LABEL *p_labels = static_cast<TT_LABEL*>(component_array.get())->get();

            unsigned int n_labels = 0;

            bool has_component_id = in_metadata.has("component_ids");
            if (this->contiguous_component_ids || has_component_id)
            {
                // use a contiguous buffer to hold the result, only for
                // contiguous lables that start at 0
                p_teca_variant_array component_id;
                if (has_component_id)
                {
                    in_metadata.get("number_of_components", int(0), n_labels);
                    component_id = in_metadata.get("component_ids");
                }
                else
                {
                    NT_LABEL max_component_id = ::get_max_component_id(nxy, p_labels);
                    n_labels = max_component_id + 1;
                    p_teca_variant_array_impl<NT_LABEL> tmp = teca_variant_array_impl<NT_LABEL>::New(n_labels);
                    for (unsigned int i = 0; i < n_labels; ++i)
                        tmp->set(i, NT_LABEL(i));
                    component_id = tmp;
                }
                std::vector<calc_t> component_area(n_labels);
                ::component_area(nx,ny, p_xc,p_yc, p_labels, component_area);

                // transfer the result to the output
                out_metadata.set("number_of_components", n_labels);
                out_metadata.set("component_ids", component_id);
                out_metadata.set("component_area", component_area);
            }
            else
            {
                // use an associative array to handle any labels
                //std::map<NT_LABEL, NT_COORD> result;
                decltype(std::map<NT_LABEL, calc_t>()) result;
                ::component_area(nx,ny, p_xc,p_yc, p_labels, result);

                // transfer the result to the output
                n_labels = result.size();

                p_teca_variant_array_impl<NT_LABEL> component_id =
                    teca_variant_array_impl<NT_LABEL>::New(n_labels);

                p_teca_variant_array_impl<calc_t> component_area =
                    teca_variant_array_impl<calc_t>::New(n_labels);

                //std::map<NT_LABEL,NT_COORD>::iterator it = result.begin();
                auto it = result.begin();
                for (unsigned int i = 0; i < n_labels; ++i,++it)
                {
                    component_id->set(i, it->first);
                    component_area->set(i, it->second);
                }

                out_metadata.set("number_of_components", n_labels);
                out_metadata.set("component_ids", component_id);
                out_metadata.set("component_area", component_area);
            }
            )
        )

    return out_mesh;
}


//  To use the simple FFT implementation
//  g++ -o demofft -I.. -Wall -O3 FFT.cpp

//  To use the FFTW implementation
//  g++ -o demofft -I.. -DUSE_FFTW -Wall -O3 FFT.cpp -lfftw3 -lfftw3f -lfftw3l

#ifdef USE_FFTW
#include <fftw3.h>
#endif

#include <vector>
#include <complex>
#include <algorithm>
#include <iterator>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/FFT>

using namespace std;
using namespace Eigen;

template <typename T>
T mag2(T a)
{
    return a*a;
}
template <typename T>
T mag2(std::complex<T> a)
{
    return norm(a);
}

template <typename T>
T mag2(const std::vector<T> & vec)
{
    T out=0;
    for (size_t k=0;k<vec.size();++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
T mag2(const std::vector<std::complex<T> > & vec)
{
    T out=0;
    for (size_t k=0;k<vec.size();++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
vector<T> operator-(const vector<T> & a,const vector<T> & b )
{
    vector<T> c(a);
    for (size_t k=0;k<b.size();++k)
        c[k] -= b[k];
    return c;
}

template <typename T>
void RandomFill(std::vector<T> & vec)
{
    for (size_t k=0;k<vec.size();++k)
        vec[k] = T( rand() )/T(RAND_MAX) - .5;
}

template <typename T>
void RandomFill(std::vector<std::complex<T> > & vec)
{
    for (size_t k=0;k<vec.size();++k)
        vec[k] = std::complex<T> ( T( rand() )/T(RAND_MAX) - .5, T( rand() )/T(RAND_MAX) - .5);
}

template <typename T_time,typename T_freq>
void fwd_inv(size_t nfft)
{
    typedef typename NumTraits<T_freq>::Real Scalar;
    vector<T_time> timebuf(nfft);
    RandomFill(timebuf);

    vector<T_freq> freqbuf;
    static FFT<Scalar> fft;
    fft.fwd(freqbuf,timebuf);

    vector<T_time> timebuf2;
    fft.inv(timebuf2,freqbuf);

    long double rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
    cout << "roundtrip rmse: " << rmse << endl;
}

template <typename T_scalar>
void two_demos(int nfft)
{
    cout << "     scalar ";
    fwd_inv<T_scalar,std::complex<T_scalar> >(nfft);
    cout << "    complex ";
    fwd_inv<std::complex<T_scalar>,std::complex<T_scalar> >(nfft);
}

void demo_all_types(int nfft)
{
    cout << "nfft=" << nfft << endl;
    cout << "   float" << endl;
    two_demos<float>(nfft);
    cout << "   double" << endl;
    two_demos<double>(nfft);
    cout << "   long double" << endl;
    two_demos<long double>(nfft);
}

int main()
{
    demo_all_types( 2*3*4*5*7 );
    demo_all_types( 2*9*16*25 );
    demo_all_types( 1024 );
    return 0;
}


/**
 * @brief 視錐台クラス
 */

#include "View/Frustum.h"

#include <boost/assert.hpp>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <iostream>
#include <vector>

void Frustum::SetupPerspective(float fovy, float aspectRatio, float near,
                               float far) {
  fovy_ = fovy;
  ar_ = aspectRatio;
  near_ = near;
  far_ = far;

  type_ = ProjectionType::Perspective;
}

void Frustum::SetupOrtho(float left, float right, float bottom, float top,
                         float near, float far) {
  left_ = left;
  right_ = right;
  bottom_ = bottom;
  top_ = top;
  near_ = near;
  far_ = far;

  type_ = ProjectionType::Ortho;
}

void Frustum::SetupCorners(const glm::vec3 &eyePt, const glm::vec3 &lookatPt,
                           const glm::vec3 &upVec) {
  corners_[0] = glm::vec3(-1.0, 1.0, 1.0);
  corners_[1] = glm::vec3(1.0, 1.0, 1.0);
  corners_[2] = glm::vec3(1.0, -1.0, 1.0);
  corners_[3] = glm::vec3(-1.0, -1.0, 1.0);

  corners_[4] = glm::vec3(-1.0, 1.0, -1.0);
  corners_[5] = glm::vec3(1.0, 1.0, -1.0);
  corners_[6] = glm::vec3(1.0, -1.0, -1.0);
  corners_[7] = glm::vec3(-1.0, -1.0, -1.0);

  const auto kView = glm::lookAt(eyePt, lookatPt, upVec);
  const auto kProj = GetProjectionMatrix();
  const auto kInvVP = glm::inverse(kProj * kView);
  for (int i = 0; i < 8; i++) {
    const auto corner = kInvVP * glm::vec4(corners_[i], 1.0f);
    corners_[i] = glm::vec3(corner) / corner.w;
  }
}

BSphere Frustum::ComputeBSphere() const {
  glm::vec3 center = glm::vec3(0.0f);
  for (int i = 0; i < 8; i++) {
    center += corners_[i];
  }
  center /= 8.0f;

  float radius = 0.0f;
  for (int i = 0; i < 8; i++) {
    float len = glm::length(corners_[i] - center);
    radius = glm::max(radius, len);
  }
  radius = std::ceil(radius * 16.0f) / 16.0f;

  return {center, radius};
}

glm::mat4 Frustum::GetProjectionMatrix() const {
  if (type_ == ProjectionType::Perspective) {
    return glm::perspective(fovy_, ar_, near_, far_);
  } else {
    return glm::ortho(left_, right_, bottom_, top_, near_, far_);
  }
}


//---------------------------------------------------------------------------
//    $Id: theta_timestepping.cc 28377 2013-02-13 15:22:32Z heister $
//
//    Copyright (C) 2005-2006, 2010, 2012 by the deal.II authors
//
//    This file is subject to QPL and may not be  distributed
//    without copyright and license information. Please refer
//    to the file deal.II/doc/license.html for the  text  and
//    further information on this license.
//
//---------------------------------------------------------------------------

// See documentation of ThetaTimestepping for documentation of this example

#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>

#include <deal.II/algorithms/operator.h>
#include <deal.II/algorithms/theta_timestepping.h>

#include <iostream>

using namespace dealii;
using namespace Algorithms;

class Explicit
  : public Operator<Vector<double> >
{
public:
  Explicit(const FullMatrix<double> &matrix);
  void operator() (NamedData<Vector<double>*> &out,
                   const NamedData<Vector<double>*> &in);

  void initialize_timestep_data(const TimestepData &);
private:
  const TimestepData *timestep_data;
  SmartPointer<const FullMatrix<double>, Explicit> matrix;
  FullMatrix<double> m;
};

class Implicit
  : public Operator<Vector<double> >
{
public:
  Implicit(const FullMatrix<double> &matrix);
  void operator() (NamedData<Vector<double>*> &out,
                   const NamedData<Vector<double>*> &in);

  void initialize_timestep_data(const TimestepData &);
private:
  const TimestepData *timestep_data;
  SmartPointer<const FullMatrix<double>, Implicit> matrix;
  FullMatrix<double> m;
};

// End of declarations

int main()
{
  FullMatrix<double> matrix(2);
  matrix(0,0) = 1.;
  matrix(1,1) = 1.;
  matrix(0,1) = 31.4;
  matrix(1,0) = -31.4;

  OutputOperator<Vector<double> > out;
  out.initialize_stream(std::cout);

  Explicit op_explicit(matrix);
  Implicit op_implicit(matrix);
  ThetaTimestepping<Vector<double> > solver(op_explicit, op_implicit);
  op_explicit.initialize_timestep_data(solver.explicit_data());
  op_implicit.initialize_timestep_data(solver.implicit_data());
  solver.set_output(out);

  Vector<double> value(2);
  value(0) = 1.;
  NamedData<Vector<double>*> indata;
  NamedData<Vector<double>*> outdata;
  Vector<double> *p = &value;
  outdata.add(p, "value");

  solver.notify(Events::initial);
  solver(outdata, indata);
}

Explicit::Explicit(const FullMatrix<double> &M)
  :
  matrix(&M)
{
  m.reinit(M.m(), M.n());
}

void
Explicit::initialize_timestep_data(const TimestepData &t)
{
  timestep_data = &t;
}

void
Explicit::operator() (NamedData<Vector<double>*> &out, const NamedData<Vector<double>*> &in)
{
  if (this->notifications.test(Events::initial) || this->notifications.test(Events::new_timestep_size))
    {
      m.equ(-timestep_data->step, *matrix);
      for (unsigned int i=0; i<m.m(); ++i)
        m(i,i) += 1.;
    }
  this->notifications.clear();
  unsigned int i = in.find("Previous iterate");
  m.vmult(*out(0), *in(i));
}

Implicit::Implicit(const FullMatrix<double> &M)
  :
  matrix(&M)
{
  m.reinit(M.m(), M.n());
}

void
Implicit::initialize_timestep_data(const TimestepData &t)
{
  timestep_data = &t;
}

void
Implicit::operator() (NamedData<Vector<double>*> &out, const NamedData<Vector<double>*> &in)
{
  if (this->notifications.test(Events::initial) || this->notifications.test(Events::new_timestep_size))
    {
      m.equ(timestep_data->step, *matrix);
      for (unsigned int i=0; i<m.m(); ++i)
        m(i,i) += 1.;
      m.gauss_jordan();
    }
  this->notifications.clear();

  unsigned int i = in.find("Previous time");
  m.vmult(*out(0), *in(i));
}


#pragma once

// #include <boost/container/pmr/monotonic_buffer_resource.hpp>
// #include <boost/container/pmr/set.hpp>
// #include <boost/container/pmr/unsynchronized_pool_resource.hpp>
// #include <experimental/memory_resource>
#include <set>

namespace perceive
{
//
// Shortest path on a DAG, using sparse representations
//
template<typename V>
inline vector<V> shortest_path_sparse(
    const V source,
    const V sink,
    std::function<real(const V& u, const V& v)> edge_weight,
    std::function<void(const V& u, std::function<void(const V&)>)>
        for_each_neighbour,
    const real k_max_cost = std::numeric_limits<real>::max())
{
   if(source == sink) return {};

   Expects(edge_weight);
   Expects(for_each_neighbour);

   using Key     = std::pair<real, V>;
   using Compare = std::less<Key>;

   // boost::container::pmr::monotonic_buffer_resource alloc(1024 * 10);
   std::set<Key, Compare> Q;

   std::unordered_map<V, V> parent_v;
   std::unordered_map<V, real> dist_;

   real max_cost = std::isfinite(k_max_cost) ? k_max_cost
                                             : std::numeric_limits<real>::max();

   auto dist = [&](const V u) {
      auto ii = dist_.find(u);
      return (ii == cend(dist_)) ? max_cost : ii->second;
   };

   Q.insert({0.0, source});
   dist_[source]   = 0.0;
   real path_score = dNAN;

   while(Q.size() > 0) {
      const V u = Q.begin()->second;
      Q.erase(Q.begin()); // Remove element
      if(u == sink) {
         Expects(parent_v.find(u) != cend(parent_v));
         break; // We're done
      }
      Expects(dist(u) < std::numeric_limits<real>::max());
      const auto dist_u = dist(u);

      for_each_neighbour(u, [&](const V v) {
         const real w = edge_weight(u, v);
         Expects(std::isfinite(w));
         const auto dist_v = dist(v);
         if(dist_v > dist_u + w) {
            Q.erase({dist_v, v});
            const auto new_dist_v = dist_u + w;
            Q.insert({new_dist_v, v});
            parent_v[v] = u;
            dist_[v]    = new_dist_v;
         }
      });
   }

   if(parent_v.find(sink) == cend(parent_v)) return {};

   // Reconstruct the path
   vector<V> out;
   {
      out.reserve(Q.size() + 2);
      out.push_back(sink);
      while(out.back() != source) {
         const auto ii = parent_v.find(out.back());
         Expects(ii != cend(parent_v));
         out.push_back(ii->second);
      }
      std::reverse(begin(out), end(out));
   }

   return out;
}

} // namespace perceive


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_LOG1P_INCLUDED
#define BOOST_MATH_LOG1P_INCLUDED

#include <cmath>
#include <math.h> // platform's ::log1p
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::fabs; using ::log; }
#endif

namespace boost{ namespace math{

namespace detail{

//
// Functor log1p_series returns the next term in the Taylor series
// pow(-1, k-1)*pow(x, k) / k
// each time that operator() is invoked.
//
template <class T>
struct log1p_series
{
   typedef T result_type;

   log1p_series(T x)
      : k(0), m_mult(-x), m_prod(-1){}

   T operator()()
   {
      m_prod *= m_mult;
      return m_prod / ++k;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_mult;
   T m_prod;
   log1p_series(const log1p_series&);
   log1p_series& operator=(const log1p_series&);
};

} // namespace

//
// Algorithm log1p is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > x > epsilon, which may
// require up to std::numeric_limits<T>::digits+1 terms to be calculated.  It would
// be much more efficient to use the equivalence:
// log(1+x) == (log(1+x) * x) / ((1-x) - 1)
// Unfortunately optimizing compilers make such a mess of this, that it performs
// no better than log(1+x): which is to say not very well at all.
//
template <class T>
T log1p(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif
   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::log(T(1.0) + x);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::log1p_series<T> s(x);
   return detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
// these overloads work around a type deduction bug:
inline float log1p(float z)
{
   return log1p<float>(z);
}
inline double log1p(double z)
{
   return log1p<double>(z);
}
inline long double log1p(long double z)
{
   return log1p<long double>(z);
}
#endif

#ifdef log1p
#  ifndef BOOST_HAS_LOG1P
#     define BOOST_HAS_LOG1P
#  endif
#  undef log1p
#endif

#ifdef BOOST_HAS_LOG1P
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float log1p(float x){ return ::log1pf(x); }
inline long double log1p(long double x){ return ::log1pl(x); }
#else
inline float log1p(float x){ return ::log1p(x); }
#endif
inline double log1p(double x){ return ::log1p(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 *
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <tuple>

using namespace dealii;

class MyTuple
{
  std::tuple<int, int, int> Tuple;

public:
  MyTuple(Triangulation<2> &T)
    : Tuple(std::make_tuple(T.n_levels(), T.n_cells(), T.n_active_cells())) {}

  void print()
  {
    std::cout << "Number of level:\t" << std::get<0>(this->Tuple) << "\n"
              << "Number of cells:\t" << std::get<1>(this->Tuple) << "\n"
              << "Number of active cells:\t" << std::get<2>(this->Tuple) << std::endl;
  }
};

MyTuple helper(Triangulation<2> &T) {return MyTuple(T);}

void
circle_grid()
{
  const Point<2> center(0, 0);
  const double   radius = 1.0;

  Triangulation<2> triangulation;
  GridGenerator::hyper_ball(triangulation, center, radius, false);
  triangulation.refine_global(2);

  std::ofstream out("circle_grid.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to circle_grid.svg" << std::endl;

  Triangulation<2> triangulation2;
  GridGenerator::hyper_ball(triangulation2, center, radius, true);
  triangulation2.refine_global(2);

  std::ofstream out2("circle_grid2.svg");
  GridOut       grid_out2;
  grid_out2.write_svg(triangulation2, out2);
  std::cout << "Grid written to circle_grid2.svg" << std::endl;
}

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);

  std::cout << "Number of original vertices: " << triangulation.n_vertices()
            << std::endl;

  triangulation.refine_global(4);

  std::cout << "Numbe  r of vertices after 4 refinmentss: "
            << triangulation.n_vertices() << std::endl;

  std::ofstream out("grid-1.vtk");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  // triangulation.reset_manifold(0);
  /*
  questo comando fa si che la triangolazione passi dal descrivere
  il Manifold che aveva in precedenza (in questo caso una hyper shell)
  al descrivere un FlatManifold
  */

  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

void
third_grid()
{
  Triangulation<2> triangulation;
  GridGenerator::hyper_L(triangulation, 0., 0.9);
  /*triangulation.refine_global(1);*/
  const Point<2> corner(0.45, 0.45);

  for (unsigned int step = 0; step < 6; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          Point<2>     cell_center          = cell->center();
          const double distance_from_corner = corner.distance(cell_center);

          if (distance_from_corner < 1. / 3)
            {
              cell->set_refine_flag();
            }
        }
      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-3.vtk");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-3.vtk" << std::endl;

  MyTuple t = helper(triangulation);
  t.print();
}

int
main()
{
  first_grid();
  second_grid();
  third_grid();

  circle_grid();
}


/*******************************************************************************
 *
 * Standard domain of numerical congruences extended with bitwise
 * operations.
 *
 * Author: Alexandre C. D. Wimmers (alexandre.c.wimmers@nasa.gov)
 *
 * Contributors: Jorge A. Navas (jorge.a.navaslaserna@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <crab/common/types.hpp>
#include <crab/common/stats.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>

#include <boost/optional.hpp>

namespace ikos {

template <typename Number>
class congruence {
  typedef interval<Number> interval_t;

public:
  typedef congruence<Number> congruence_t;

private:
  bool _is_bottom;

  /// A congruence is denoted by aZ + b, where b \in Z and a \in N.
  /// The abstract state aZ + b represents all numbers that are
  /// congruent to b modulo a.
  Number _a; // modulo
  Number _b; // remainder

  // Notes about the % operator
  //
  // The semantics of r = n % d is to set r to "n mod d". The sign of
  // the d is ignored and r is always non-negative.
  //
  // We assume that n % d (also n /d) raises a runtime error if d==0.

  void normalize(void) {
    // Set to standard form: 0 <= b < a for a != 0
    if (_a != 0) {
      _b = _b % _a;
    }
  }

  // if true then top (1Z + 0) else bottom
  congruence(bool b): _is_bottom(!b), _a(1), _b(0) {}

  congruence(int n): _is_bottom(false), _a(0), _b(n) {}

  congruence(Number a, Number b)
    : _is_bottom(false),
      _a(a),
      _b(b) {
    normalize();
  }

  Number abs(Number x) const { return x < 0 ? -x : x; }

  Number max(Number x, Number y) const { return x.operator<=(y) ? y : x; }

  Number min(Number x, Number y) const { return x.operator<(y) ? x : y; }

  Number gcd(Number x, Number y, Number z) const  { return gcd(x, gcd(y, z)); }
  // Not to be called explicitly outside of gcd
  Number gcd_helper(Number x, Number y) const {
    return (y == 0) ? x : gcd_helper(y, x % y);
  }
  Number gcd(Number x, Number y) const { return gcd_helper(abs(x), abs(y)); }

  Number lcm(Number x, Number y) const {
    Number tmp = gcd(x, y);
    return abs(x * y) / tmp;
  }

  bool is_zero() const {
    return !is_bottom() && _a == 0 && _b == 0;
  }

  bool all_ones() const {
    return !is_bottom() && _a == 0 && _b == -1;
  }

  interval_t to_interval() const {
    assert(singleton());
    return interval_t(*(singleton()));
  }

public:

  static congruence_t top() { return congruence(true); }

  static congruence_t bottom() { return congruence(false); }

  congruence(): _is_bottom(false), _a(1), _b(0) {}

  congruence(Number n): _is_bottom(false), _a(0), _b(n) {}

  congruence(const congruence_t& o)
      : _is_bottom(o._is_bottom), _a(o._a), _b(o._b) {}

  congruence_t operator=(congruence_t o) {
    _is_bottom = o._is_bottom;
    _a = o._a;
    _b = o._b;
    return *this;
  }

  bool is_bottom() const { return _is_bottom; }

  bool is_top() const { return _a == 1; }

  boost::optional<Number> singleton() const {
    if (!this->is_bottom() && _a == 0) {
      return boost::optional<Number>(_b);
    } else {
      return boost::optional<Number>();
    }
  }

  Number get_modulo() const { return _a;}

  Number get_remainder() const { return _b;}

  bool operator==(congruence_t o) const {
    return (is_bottom() == o.is_bottom() && _a == o._a && _b == o._b) ;
  }

  bool operator!=(congruence_t x) const { return !this->operator==(x); }

  /** Lattice Operations **/

  bool operator<=(congruence_t o) {
    if (is_bottom()) {
      return true;
    } else if (o.is_bottom()) {
      return false;
    } else if (_a == 0 && o._a == 0) {
      return  (_b == o._b);
    } else if (_a == 0) {
      if ((_b % o._a) == (o._b % o._a)) {
        return true;
      }
    } else if (o._a == 0) {
      if (_b % _a == (o._b % _a)) {
        return false;
      }
    }
    return (_a % o._a == 0) && (_b % o._a == o._b % o._a);
  }

  congruence_t operator|(congruence_t o) {
    if (is_bottom()) {
      return o;
    } else if (o.is_bottom()) {
      return *this;
    } else if (is_top() || o.is_top()) {
      return top();
    } else {
      return congruence_t(gcd(_a, o._a, abs(_b - o._b)), min(_b, o._b));
    }
  }

  congruence_t operator&(congruence_t o) {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    }

    // lcm has meaning only if both a and o.a are not 0
    if (_a == 0 && o._a == 0) {
      if (_b == o._b) {
        return *this;
      } else {
        return bottom();
      }
    } else if (_a == 0) {
      // b & a'Z + b' iff \exists k such that a'*k + b' = b iff ((b - b') %a' == 0)
      if ((_b - o._b) % o._a == 0) {
        return *this;
      } else {
        return bottom();
      }
    } else if (o._a == 0) {
      // aZ+b & b' iff \exists k such that a*k+b  = b' iff ((b'-b %a) == 0)
      if ((o._b - _b) % _a == 0) {
        return o;
      } else {
        return bottom();
      }
    } else {
      // pre: a and o.a != 0
      Number x = gcd(_a, o._a);
      if (_b % x == (o._b % x)) {
        // the part max(b,o.b) needs to be verified. What we really
        // want is to find b'' such that
        // 1) b'' % lcm(a,a') == b  % lcm(a,a'), and
        // 2) b'' % lcm(a,a') == b' % lcm(a,a').
        // An algorithm for that is provided in Granger'89.
        return congruence_t(lcm(_a, o._a), max(_b, o._b));
      } else {
        return congruence_t::bottom();
      }
    }

  }

  congruence_t operator||(congruence_t o) {
    // Equivalent to join, domain is flat
    return *this | o;
  }

  congruence_t operator&&(congruence_t o) {
    // Simply refines top element
    return (is_top()) ? o : *this;
  }

  /** Arithmetic Operators **/

  congruence_t operator+(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b + o._b);
  }

  congruence_t operator-(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b - o._b);
  }

  congruence_t operator-() {
    if (this->is_bottom() || this->is_top())
      return *this;
    else
      return congruence_t(_a, -_b + _a);
  }

  congruence_t operator*(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if ((this->is_top() || o.is_top()) && _a != 0 && o._a != 0)
      return congruence_t::top();
    else
      return congruence_t(gcd(_a * o._a, _a * o._b, o._a * _b), _b * o._b);
  }

  // signed division
  congruence_t operator/(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      /*
         aZ+b / 0Z+b':
            if b'|a then  (a/b')Z + b/b'
            else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0)
          return congruence_t(_a / o._b, _b / o._b);
        else
          return congruence_t::top();
      }

      /*
         0Z+b / a'Z+b':
            if N>0   (b div N)Z + 0
            else     0Z + 0

           where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n > 0) {
          return congruence_t(_b / n, 0);
        } else {
          return congruence_t(0, 0);
        }
      }

      /*
        General case: no singleton
      */
      return congruence_t::top();
    }
  }

  // signed remainder operator
  congruence_t operator%(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      /*
         aZ+b mod 0Z+b':
             if b'|a then  (a/b')Z + b/b'
             else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0) {
          return congruence_t(0, _b % o._b);
        } else {
          return congruence_t(gcd(_a, o._b), _b);
        }
      }

      /*
          0Z+b mod a'Z+b':
           if N<=0           then 0Z+b
           if (b div N) == 1 then gcd(b',a')Z + b
           if (b div N) >= 2 then N(b div N)Z  + b

         where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n <= 0) {
          return congruence_t(_a, _b);
        } else if (_b == n) {
          return congruence_t(gcd(o._b, o._a), _b);
        } else if ((_b / n) >= 2) {
          return congruence_t(_b, _b);
        } else {
          CRAB_ERROR("unreachable");
        }
      }

      /*
          general case: no singleton
      */
      return congruence_t(gcd(_a, o._a, o._b), _b);
    }
  }

  /**
      Bitwise operators.
      They are very imprecise because we ignore bitwidth.

      Bitwise operation can be implemented more precisely based on
      Stefan Bygde's paper: Static WCET analysis based on abstract
      interpretation and counting of elements, Vasteras : School of
      Innovation, Design and Engineering, Malardalen University (2010).
   **/

  congruence_t And(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero() || o.is_zero()) {
	return congruence_t(0);
      } else if (all_ones()) {
	return o;
      } else if (o.all_ones()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b & o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Or(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (all_ones() || o.all_ones()) {
	return congruence_t(-1);
      } else if (is_zero()) {
	return o;
      } else if (o.is_zero()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b | o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Xor(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero()) {
	return o;
      } else if (o.is_zero()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b ^ o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Shl(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton

	if (o._b < 0) {
	  return bottom();
	}

        // aZ + b << 0Z + b'  = (a*2^b')Z + b*2^b'
        Number x = Number(1) << o._b;
        return congruence_t(_a * x, _b * x);
      } else {

        Number x = Number(1) << o._b;
        Number y = Number(1) << o._a;
        // aZ + b << a'Z + b' = (gcd(a, b * (2^a' - 1)))*(2^b')Z + b*(2^b')
        return congruence_t(gcd(_a, _b * (y - 1)) * x, _b * x);
      }
    }
  }

  congruence_t AShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton
	// aZ + b >> 0Z + b'
	if (o._b < 0) {
	  return congruence_t::bottom();
	}
      }

      if (singleton() && o.singleton()) {
	interval_t res = to_interval().AShr(o.to_interval());
	if (boost::optional<Number> n = res.singleton()) {
	  return congruence(*n);
	}
      }

      return congruence_t::top();
    }
  }

  congruence_t LShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) {
	// aZ + b >> 0Z + b'
	if (o._b < 0) {
	  return congruence_t::bottom();
	}
      }

      if (singleton() && o.singleton()) {
	interval_t res = to_interval().LShr(o.to_interval());
	if (boost::optional<Number> n = res.singleton()) {
	  return congruence(*n);
	}
      }

      return congruence_t::top();
    }
  }

  // division and remainder operations

  congruence_t SDiv(congruence_t x) { return this->operator/(x); }

  congruence_t UDiv(congruence_t x) { return congruence_t::top(); }

  congruence_t SRem(congruence_t x) { return this->operator%(x); }

  congruence_t URem(congruence_t x) { return congruence_t::top(); }

  void write(crab::crab_os& o) const {
    if (is_bottom()) {
      o << "_|_";
      return;
    }

    if (_a == 0) {
      o << _b;
      return;
    }

    o << _a << "Z+" << _b;

  }
}; // end class congruence

template<typename Number>
inline crab::crab_os& operator<<(crab::crab_os& o, const congruence<Number>& c) {
  c.write(o);
  return o;
}

template <typename Number>
inline congruence<Number> operator+(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) + x;
}

template <typename Number>
inline congruence<Number> operator+(
    congruence<Number> x, Number c) {
  return x + congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator*(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) * x;
}

template <typename Number>
inline congruence<Number> operator*(
    congruence<Number> x, Number c) {
  return x * congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator/(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) / x;
}

template <typename Number>
inline congruence<Number> operator/(
    congruence<Number> x, Number c) {
  return x / congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator-(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) - x;
}

template <typename Number>
inline congruence<Number> operator-(
    congruence<Number> x, Number c) {
  return x - congruence<Number>(c);
}

template <typename Number,
	  typename VariableName,
	  typename CongruenceCollection>
class equality_congruence_solver {
  // TODO: check correctness of the solver. Granger provides a sound
  // and more precise solver for equality linear congruences (see
  // Theorem 4.4).
private:
  typedef congruence<Number> congruence_t;
  typedef variable<Number, VariableName> variable_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef linear_constraint_system<Number, VariableName>
      linear_constraint_system_t;

  typedef std::vector<linear_constraint_t> cst_table_t;
  typedef typename linear_constraint_t::variable_set_t variable_set_t;

  std::size_t m_max_cycles;
  bool m_is_contradiction;
  cst_table_t m_cst_table;
  variable_set_t m_refined_variables;
  std::size_t m_op_count;

private:
  bool refine(variable_t v, congruence_t i,
              CongruenceCollection& env) {
    congruence_t old_i = env[v];
    congruence_t new_i = old_i & i;
    if (new_i.is_bottom()) {
      return true;
    }
    if (old_i != new_i) {
      env.set(v, new_i);
      m_refined_variables += v;
      ++(m_op_count);
    }
    return false;
  }

  congruence_t compute_residual(linear_constraint_t cst,
                                variable_t pivot,
                                CongruenceCollection& env) {
    congruence_t residual(cst.constant());
    for (typename linear_constraint_t::iterator it = cst.begin();
         it != cst.end();
         ++it) {
      variable_t v = it->second;
      if (!(v == pivot)) {
        residual = residual - (it->first * env[v]);
        ++(m_op_count);
      }
    }
    return residual;
  }

  bool propagate(linear_constraint_t cst, CongruenceCollection& env) {
    for (typename linear_constraint_t::iterator it = cst.begin();
         it != cst.end();
         ++it) {
      Number c = it->first;
      variable_t pivot = it->second;
      congruence_t rhs =
          compute_residual(cst, pivot, env) / congruence_t(c);

      if (cst.is_equality()) {
        if (refine(pivot, rhs, env)) {
	  return true;
	}
      } else if (cst.is_inequality() || cst.is_strict_inequality()) {
        // Inequations (>=, <=, >, and <) do not work well with
        // congruences because for any number n there is always x and y
        // \in gamma(aZ+b) such that n < x and n > y.
        //
        // The only cases we can catch is when all the expressions
        // are constants. We do not bother because any product
        // with intervals or constants should get those cases.
        continue;
      } else {
        // TODO: cst is a disequation
      }
    }
    return false;
  }

  bool solve_system(CongruenceCollection& env) {
    std::size_t cycle = 0;
    do {
      ++cycle;
      m_refined_variables.clear();
      for (typename cst_table_t::iterator it = m_cst_table.begin();
           it != m_cst_table.end();
           ++it) {
        if (propagate(*it, env)) {
	  return true;
	}
      }
    } while (m_refined_variables.size() > 0 &&
             cycle <= m_max_cycles);
    return false;
  }

public:
  equality_congruence_solver(linear_constraint_system_t csts,
                             std::size_t max_cycles)
      : m_max_cycles(max_cycles), m_is_contradiction(false) {
    for (typename linear_constraint_system_t::iterator it = csts.begin();
         it != csts.end();
         ++it) {
      linear_constraint_t cst = *it;
      if (cst.is_contradiction()) {
        m_is_contradiction = true;
        return;
      } else if (cst.is_tautology()) {
        continue;
      } else {
        m_cst_table.push_back(cst);
      }
    }
  }

  void run(CongruenceCollection& env) {
    if (m_is_contradiction) {
      env.set_to_bottom();
    } else {
      if (solve_system(env)) {
	env.set_to_bottom();
      }
    }
  }

}; // class equality_congruence_solver

template <typename Number, typename VariableName>
class congruence_domain final:
  public crab::domains::abstract_domain<congruence_domain<Number,VariableName>> {
public:
  typedef congruence<Number> congruence_t;

private:
  // note that this is assuming that all variables have the same bit
  // width which is unrealistic.
  typedef congruence_domain<Number, VariableName>
  congruence_domain_t;
  typedef crab::domains::abstract_domain<congruence_domain_t>
  abstract_domain_t;

public:
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  typedef Number number_t;
  typedef VariableName varname_t;
  using typename abstract_domain_t::pointer_constraint_t;

private:
  typedef separate_domain<variable_t, congruence_t> separate_domain_t;
  typedef equality_congruence_solver<number_t, varname_t,
				     separate_domain_t> solver_t;

public:
  typedef typename separate_domain_t::iterator iterator;

private:
  separate_domain_t _env;

private:
  congruence_domain(separate_domain_t env) : _env(env) {}

public:
  void set_to_top() {
    congruence_domain abs(separate_domain_t::top());
    std::swap(*this, abs);
  }

  void set_to_bottom() {
    congruence_domain abs(separate_domain_t::bottom());
    std::swap(*this, abs);
  }

  congruence_domain() : _env(separate_domain_t::top()) {}

  congruence_domain(const congruence_domain_t& e)
      : _env(e._env) {
    crab::CrabStats::count(getDomainName() + ".count.copy");
    crab::ScopedCrabStats __st__(getDomainName() + ".copy");
  }

  congruence_domain_t& operator=(const congruence_domain_t& o) {
    crab::CrabStats::count(getDomainName() + ".count.copy");
    crab::ScopedCrabStats __st__(getDomainName() + ".copy");
    if (this != &o)
      this->_env = o._env;
    return *this;
  }

  iterator begin() { return this->_env.begin(); }

  iterator end() { return this->_env.end(); }

  bool is_bottom() { return this->_env.is_bottom(); }

  bool is_top() { return this->_env.is_top(); }

  bool operator<=(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.leq");
    crab::ScopedCrabStats __st__(getDomainName() + ".leq");
    return this->_env <= e._env;
  }

  void operator|=(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.join");
    crab::ScopedCrabStats __st__(getDomainName() + ".join");
    this->_env = this->_env | e._env;
  }

  congruence_domain_t operator|(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.join");
    crab::ScopedCrabStats __st__(getDomainName() + ".join");
    return this->_env | e._env;
  }

  congruence_domain_t operator&(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.meet");
    crab::ScopedCrabStats __st__(getDomainName() + ".meet");
    return this->_env & e._env;
  }

  congruence_domain_t operator||(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.widening");
    crab::ScopedCrabStats __st__(getDomainName() + ".widening");
    return this->_env || e._env;
  }

  congruence_domain_t widening_thresholds(congruence_domain_t other,
					  const crab::iterators::thresholds<number_t>&) {
    return (*this || other);
  }

  congruence_domain_t operator&&(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.narrowing");
    crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");
    return this->_env && e._env;
  }

  void set(variable_t v, congruence_t i) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");
    this->_env.set(v, i);
  }

  void set(variable_t v, number_t n) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");
    this->_env.set(v, congruence_t(n));
  }

  void operator-=(variable_t v) {
    crab::CrabStats::count(getDomainName() + ".count.forget");
    crab::ScopedCrabStats __st__(getDomainName() + ".forget");
    this->_env -= v;
  }

  void operator-=(std::vector<variable_t> vs) {
    for (typename std::vector<variable_t>::iterator it = vs.begin(),
	   end = vs.end(); it != end; ++it) {
      this->operator-=* it;
    }
  }

  congruence_t operator[](variable_t v) { return this->_env[v]; }

  congruence_t operator[](linear_expression_t expr) {
    congruence_t r(expr.constant());
    for (typename linear_expression_t::iterator it = expr.begin();
         it != expr.end();
         ++it) {
      congruence_t c(it->first);
      r = r + (c * this->_env[it->second]);
    }
    return r;
  }

  void add(linear_constraint_system_t csts) {
    crab::CrabStats::count(getDomainName() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");
    const std::size_t threshold = 10;
    if (!this->is_bottom()) {
      solver_t solver(csts, threshold);
      solver.run(this->_env);
    }
  }

  void operator+=(linear_constraint_system_t csts) {
    this->add(csts);
  }

  congruence_domain_t operator+(linear_constraint_system_t csts) {
    congruence_domain_t e(this->_env);
    e += csts;
    return e;
  }

  void assign(variable_t x, linear_expression_t e) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");

    congruence_t r = e.constant();
    for (typename linear_expression_t::iterator it = e.begin(); it != e.end();
         ++it) {
      r = r + (it->first * this->_env[it->second]);
    }
    this->_env.set(x, r);
  }

  void apply(operation_t op, variable_t x, variable_t y, variable_t z) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_ADDITION:
        xi = yi + zi;
        break;
      case OP_SUBTRACTION:
        xi = yi - zi;
        break;
      case OP_MULTIPLICATION:
        xi = yi * zi;
        break;
      case OP_SDIV:
        xi = yi / zi;
        break;
      case OP_UDIV:
        xi = yi.UDiv(zi);
        break;
      case OP_SREM:
        xi = yi.SRem(zi);
        break;
      case OP_UREM:
        xi = yi.URem(zi);
        break;
    default:
      CRAB_ERROR("Operation ", op, " not supported");
    }
    this->_env.set(x, xi);
  }

  void apply(operation_t op, variable_t x, variable_t y, number_t k) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_ADDITION:
        xi = yi + zi;
        break;
      case OP_SUBTRACTION:
        xi = yi - zi;
        break;
      case OP_MULTIPLICATION:
        xi = yi * zi;
        break;
      case OP_SDIV:
        xi = yi / zi;
        break;
      case OP_UDIV:
        xi = yi.UDiv(zi);
        break;
      case OP_SREM:
        xi = yi.SRem(zi);
        break;
      case OP_UREM:
        xi = yi.URem(zi);
        break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
    }
    this->_env.set(x, xi);
  }

  // backward operations
  void backward_assign(variable_t x, linear_expression_t e,
		       congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_assign");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      assign(*this, x, e, inv);
  }

  void backward_apply(operation_t op,
		      variable_t x, variable_t y, number_t z,
		      congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_apply");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      apply(*this, op, x, y, z, inv);
  }

  void backward_apply(operation_t op,
		      variable_t x, variable_t y, variable_t z,
		      congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_apply");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      apply(*this, op, x, y, z, inv);
  }

  // cast operations

  void apply(crab::domains::int_conv_operation_t /*op*/,
	     variable_t dst, variable_t src) {
    // ignore widths
    assign(dst, src);
  }

  // bitwise operations

  void apply(bitwise_operation_t op, variable_t x, variable_t y, variable_t z) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_AND: {
        xi = yi.And(zi);
        break;
      }
      case OP_OR: {
        xi = yi.Or(zi);
        break;
      }
      case OP_XOR: {
        xi = yi.Xor(zi);
        break;
      }
      case OP_SHL: {
        xi = yi.Shl(zi);
        break;
      }
      case OP_LSHR: {
        xi = yi.LShr(zi);
        break;
      }
      case OP_ASHR: {
        xi = yi.AShr(zi);
        break;
      }
      default: { CRAB_ERROR("unreachable"); }
    }
    this->_env.set(x, xi);
  }

  void apply(bitwise_operation_t op, variable_t x, variable_t y, number_t k) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_AND: {
        xi = yi.And(zi);
        break;
      }
      case OP_OR: {
        xi = yi.Or(zi);
        break;
      }
      case OP_XOR: {
        xi = yi.Xor(zi);
        break;
      }
      case OP_SHL: {
        xi = yi.Shl(zi);
        break;
      }
      case OP_LSHR: {
        xi = yi.LShr(zi);
        break;
      }
      case OP_ASHR: {
        xi = yi.AShr(zi);
        break;
      }
      default: { CRAB_ERROR("unreachable"); }
    }
    this->_env.set(x, xi);
  }

  /*
     Begin unimplemented operations

     congruence_domain implements only standard abstract operations of
     a numerical domain.  The implementation of boolean, array, or
     pointer operations is empty because they should never be called.
  */

  // boolean operations
  void assign_bool_cst(variable_t lhs, linear_constraint_t rhs) {}
  void assign_bool_var(variable_t lhs, variable_t rhs, bool is_not_rhs) {}
  void apply_binary_bool(crab::domains::bool_operation_t op,
			 variable_t x,variable_t y,variable_t z) {}
  void assume_bool(variable_t v, bool is_negated) {}
  // backward boolean operations
  void backward_assign_bool_cst(variable_t lhs, linear_constraint_t rhs,
				congruence_domain_t invariant){}
  void backward_assign_bool_var(variable_t lhs, variable_t rhs, bool is_not_rhs,
				congruence_domain_t invariant) {}
  void backward_apply_binary_bool(crab::domains::bool_operation_t op,
				  variable_t x,variable_t y,variable_t z,
				  congruence_domain_t invariant) {}
  // array operations
  void array_init(variable_t a, linear_expression_t elem_size,
		  linear_expression_t lb_idx, linear_expression_t ub_idx,
		  linear_expression_t val) {}
  void array_load(variable_t lhs,
		  variable_t a, linear_expression_t elem_size,
		  linear_expression_t i) {}
  void array_store(variable_t a, linear_expression_t elem_size,
		   linear_expression_t i, linear_expression_t v,
		   bool is_strong_update) {}
  void array_store(variable_t a_new, variable_t a_old,
		   linear_expression_t elem_size,
		   linear_expression_t i, linear_expression_t v,
		   bool is_strong_update) {}
  void array_store_range(variable_t a, linear_expression_t elem_size,
			 linear_expression_t i, linear_expression_t j,
			 linear_expression_t v) {}
  void array_store_range(variable_t a_new, variable_t a_old,
			 linear_expression_t elem_size,
			 linear_expression_t i, linear_expression_t j,
			 linear_expression_t v) {}
  void array_assign(variable_t lhs, variable_t rhs) {}
  // backward array operations
  void backward_array_init(variable_t a, linear_expression_t elem_size,
			   linear_expression_t lb_idx, linear_expression_t ub_idx,
			   linear_expression_t val, congruence_domain_t invariant) {}
  void backward_array_load(variable_t lhs,
			   variable_t a, linear_expression_t elem_size,
			   linear_expression_t i, congruence_domain_t invariant) {}
  void backward_array_store(variable_t a, linear_expression_t elem_size,
			    linear_expression_t i, linear_expression_t v,
			    bool is_strong_update, congruence_domain_t invariant) {}
  void backward_array_store(variable_t a_new, variable_t a_old,
			    linear_expression_t elem_size,
			    linear_expression_t i, linear_expression_t v,
			    bool is_strong_update, congruence_domain_t invariant) {}
  void backward_array_store_range(variable_t a, linear_expression_t elem_size,
				  linear_expression_t i, linear_expression_t j,
				  linear_expression_t v, congruence_domain_t invariant) {}
  void backward_array_store_range(variable_t a_new, variable_t a_old,
				  linear_expression_t elem_size,
				  linear_expression_t i, linear_expression_t j,
				  linear_expression_t v, congruence_domain_t invariant) {}
  void backward_array_assign(variable_t lhs, variable_t rhs, congruence_domain_t invariant) {}
  // pointer operations
  void pointer_load(variable_t lhs, variable_t rhs)  {}
  void pointer_store(variable_t lhs, variable_t rhs) {}
  void pointer_assign(variable_t lhs, variable_t rhs, linear_expression_t offset) {}
  void pointer_mk_obj(variable_t lhs, ikos::index_t address) {}
  void pointer_function(variable_t lhs, varname_t func) {}
  void pointer_mk_null(variable_t lhs) {}
  void pointer_assume(pointer_constraint_t cst) {}
  void pointer_assert(pointer_constraint_t cst) {}
  /* End unimplemented operations */

  void forget(const variable_vector_t& variables) {
    if (is_bottom() || is_top()) {
      return;
    }
    for (variable_t var: variables){
      this->operator-=(var);
    }
  }

  void project(const variable_vector_t& variables){
    crab::CrabStats::count(getDomainName() + ".count.project");
    crab::ScopedCrabStats __st__(getDomainName() + ".project");

    if (is_bottom() || is_top()) {
      return;
    }

    separate_domain_t env;
    for (variable_t var : variables){
      env.set(var, this->_env[var]);
    }
    std::swap(_env, env);
  }

  void expand(variable_t x, variable_t new_x) {
    crab::CrabStats::count(getDomainName() + ".count.expand");
    crab::ScopedCrabStats __st__(getDomainName() + ".expand");

    if (is_bottom() || is_top()) {
      return;
    }

    set(new_x , this->_env[x]);
  }

  void normalize() {}

  void minimize() {}

  void write(crab::crab_os& o) {
    crab::CrabStats::count(getDomainName() + ".count.write");
    crab::ScopedCrabStats __st__(getDomainName() + ".write");

    this->_env.write(o);
  }

  linear_constraint_system_t to_linear_constraint_system() {
    crab::CrabStats::count(getDomainName() + ".count.to_linear_constraint_system");
    crab::ScopedCrabStats __st__(getDomainName() + ".to_linear_constraint_system");

    linear_constraint_system_t csts;
    if (is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (iterator it = this->_env.begin(); it != this->_env.end(); ++it) {
      variable_t v = it->first;
      congruence_t c = it->second;
      boost::optional<number_t> n = c.singleton();
      if (n) {
        csts += (v == *n);
      }
    }
    return csts;
  }

  disjunctive_linear_constraint_system_t to_disjunctive_linear_constraint_system() {
    auto lin_csts = to_linear_constraint_system();
    if (lin_csts.is_false()) {
      return disjunctive_linear_constraint_system_t(true /*is_false*/);
    } else if (lin_csts.is_true()) {
      return disjunctive_linear_constraint_system_t(false /*is_false*/);
    } else {
      return disjunctive_linear_constraint_system_t(lin_csts);
    }
  }

  static std::string getDomainName() {
    return "Congruences";
  }

}; // class congruence_domain

} // namespace ikos

namespace crab {
namespace domains {

  template <typename Number, typename VariableName>
  struct abstract_domain_traits<ikos::congruence_domain<Number, VariableName>> {
    typedef Number number_t;
    typedef VariableName varname_t;
  };

}
}


#ifndef MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP
#define MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP

#include <Eigen/Core>
#include <functional>
#include <mathtoolbox/bfgs.hpp>
#include <mathtoolbox/l-bfgs.hpp>
#include <stdexcept>

namespace mathtoolbox
{
    namespace optimization
    {
        enum class Algorithm
        {
            Bfgs,
            LBfgs
        };

        enum class Type
        {
            Min,
            Max
        };

        struct Setting
        {
            Algorithm                                              algorithm          = Algorithm::Bfgs;
            Eigen::VectorXd                                        x_init             = Eigen::VectorXd(0);
            std::function<double(const Eigen::VectorXd&)>          f                  = nullptr;
            std::function<Eigen::VectorXd(const Eigen::VectorXd&)> g                  = nullptr;
            double                                                 epsilon            = 1e-05;
            unsigned int                                           max_num_iterations = 1000;
            Type                                                   type               = Type::Min;
        };

        struct Result
        {
            Eigen::VectorXd x_star;
            unsigned        num_iterations;
        };

        /// \brief Run optimization with the specified setting
        ///
        /// \details This is a utility function to run various optimization types in a consitent interface.
        inline Result RunOptimization(const Setting& input)
        {
            const auto f = (input.type == Type::Min) ? input.f : [&input](const Eigen::VectorXd& x) -> double {
                return -input.f(x);
            };
            const auto g = (input.type == Type::Min) ? input.g : [&input](const Eigen::VectorXd& x) -> Eigen::VectorXd {
                return -input.g(x);
            };

            switch (input.algorithm)
            {
                case Algorithm::Bfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunBfgs(input.x_init,
                            f,
                            g,
                            input.epsilon,
                            input.max_num_iterations,
                            result.x_star,
                            result.num_iterations);
                    return result;
                }
                case Algorithm::LBfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunLBfgs(input.x_init,
                             f,
                             g,
                             input.epsilon,
                             input.max_num_iterations,
                             result.x_star,
                             result.num_iterations);
                    return result;
                }
            }
        }
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP