//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP
#define CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP

#include <type_traits>

#include <boost/type_traits.hpp>

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace random {
            /*!
             * @brief
             * @tparam AlgebraicType denote an some algebraic type (field, curve group types).
             * @tparam Engine denote an some base \RandomNumberEngine generating random numbers
             *
             * The class template algebraic_engine is a pseudo-random number engine adaptor that generate random values
             * of algebraic type using data produced by the base engine. It models (not fully) a
             * \RandomNumberEngine. https://en.cppreference.com/w/cpp/named_req/RandomNumberEngine
             *
             * @warning The class template algebraic_engine differs from \RandomNumberEngine as it doesn't have
             * constructor and seed function with parameter of result_type. This is due to the fact that
             * algebraic_engine is adapter wrapping some base \RandomNumberEngine (Engine), so instead it has
             * constructor and seed function with parameter of Engine::result_type.
             */
            template<typename AlgebraicType, typename Engine = boost::random::mt19937, typename = void>
            struct algebraic_engine;

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                        !algebra::is_extended_field<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType field_type;
                typedef typename field_type::value_type field_value_type;
                typedef typename field_type::integral_type integral_type;

                typedef Engine internal_generator_type;
                typedef boost::random::uniform_int_distribution<integral_type> internal_distribution_type;

                constexpr static integral_type _min = 0;
                constexpr static integral_type _max = field_type::modulus - 1;

            public:
                typedef field_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type min() {
                    constexpr result_type min_value(_min);
                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                constexpr static inline result_type max() {
                    constexpr result_type max_value(_max);
                    return max_value;
                }

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    return dist(gen);
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen && x_.dist == y_.dist;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
                internal_distribution_type dist = internal_distribution_type(_min, _max);
            };

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_field<AlgebraicType>::value &&
                                        algebra::is_extended_field<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType extended_field_type;
                typedef typename extended_field_type::value_type extended_field_value_type;
                typedef typename extended_field_type::underlying_field_type underlying_field_type;

                typedef algebraic_engine<underlying_field_type, Engine> internal_generator_type;

            public:
                typedef extended_field_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate min_value at compile-time
                constexpr static inline result_type min() {
                    result_type min_value;
                    for (auto& coord : min_value.data) {
                        coord = internal_generator_type::min();
                    }

                    return min_value;
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    result_type max_value;
                    for (auto& coord : max_value.data) {
                        coord = internal_generator_type::max();
                    }

                    return max_value;
                }

                /** Returns a random value in the range [min, max]. */
                result_type operator()() {
                    result_type result;
                    for (auto& coord : result.data) {
                        coord = gen();
                    }

                    return result;
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
            };

            template<typename AlgebraicType, typename Engine>
            struct algebraic_engine<
                AlgebraicType,
                Engine,
                typename std::enable_if<algebra::is_curve_group<AlgebraicType>::value &&
                                        boost::is_integral<typename Engine::result_type>::value>::type> {
            protected:
                typedef AlgebraicType group_type;
                typedef typename group_type::value_type group_value_type;
                typedef typename group_type::curve_type::scalar_field_type scalar_field_type;

                typedef algebraic_engine<scalar_field_type, Engine> internal_generator_type;

            public:
                typedef group_value_type result_type;

                /**
                 * Constructs a @c algebraic_engine and calls @c seed().
                 */
                algebraic_engine() {
                    seed();
                }
                /**
                 * Constructs a @c algebraic_engine and calls @c seed(value).
                 */
                BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(algebraic_engine, typename Engine::result_type, value) {
                    seed(value);
                }
                /**
                 * Constructs a algebraic_engine and calls @c seed(seq).
                 *
                 * @xmlnote
                 * The copy constructor will always be preferred over
                 * the templated constructor.
                 * @endxmlnote
                 */
                BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(algebraic_engine, SeedSeq, seq) {
                    seed(seq);
                }

                /** Calls @c seed(default_seed) of base Engine. */
                void seed() {
                    gen.seed();
                }
                /** Calls @c seed(value) of base Engine. */
                BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(algebraic_engine, typename Engine::result_type, value) {
                    gen.seed(value);
                }
                /** Calls @c seed(seq) of base Engine. */
                BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(algebraic_engine, SeeqSeq, seq) {
                    gen.seed(seq);
                }

                /** Returns the smallest value that the \algebraic_random_device can produce. */
                // TODO: evaluate returned value at compile-time
                constexpr static inline result_type min() {
                    return result_type::zero();
                }

                /** Returns the largest value that the \algebraic_random_device can produce. */
                // TODO: evaluate max_value at compile-time
                constexpr static inline result_type max() {
                    return result_type::one() * (scalar_field_type::modulus - 1);
                }

                /**
                 * Returns a random value in the range [min, max]. Elements of group are ordered in exponent growing
                 * order with respect to group base element.
                 */
                // TODO: check correctness of the generation method
                result_type operator()() {
                    return result_type::one() * gen();
                }

                /**
                 * Advances the state of the generator by @c z steps.  Equivalent to
                 */
                void discard(std::size_t z) {
                    while (z--) {
                        (*this)();
                    }
                }

                /** Writes a algebraic_engine to a @c std::ostream */
                template<class CharT, class Traits>
                friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os,
                                                                     const algebraic_engine& ae) {
                    os << ae.gen;
                    return os;
                }

                /** Reads a algebraic_engine from a @c std::istream */
                template<class CharT, class Traits>
                friend std::basic_istream<CharT, Traits>& operator>>(std::basic_istream<CharT, Traits>& is,
                                                                     algebraic_engine& ae) {
                    is >> ae.gen;
                    return is;
                }

                /**
                 * Returns true if the two generators are in the same state,
                 * and will thus produce identical sequences.
                 */
                friend bool operator==(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return x_.gen == y_.gen;
                }

                /**
                 * Returns true if the two generators are in different states.
                 */
                friend bool operator!=(const algebraic_engine& x_, const algebraic_engine& y_) {
                    return !(x_ == y_);
                }

            protected:
                internal_generator_type gen;
            };
        }    // namespace random
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_RANDOM_ALGEBRAIC_ENGINE_HPP


#include "vtkCarGeometricCalibration.h"

#include <cmath>
#include <numeric>
#include <random>
#include <iostream>
#include <fstream>

#include <vtkMath.h>

#include <Eigen/Geometry>

#include "vtkConversions.h"
#include "vtkEigenTools.h"

Eigen::Vector3d GetXYZ(const vtkSmartPointer<vtkVelodyneTransformInterpolator> trajectory,
                       double time)
{
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  trajectory->InterpolateTransform(time, transform);
  return PositionVectorFromTransform(transform);
}

Eigen::Matrix3d GetR(const vtkSmartPointer<vtkVelodyneTransformInterpolator> trajectory,
                     double time)
{
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  trajectory->InterpolateTransform(time, transform);
  return RotationMatrixFromTransform(transform);
}

// method to identify a directions in straight lines
enum class DIRECTION_METHOD
{
  TWO_POINTS,
};

// method to identify the normal of the plane containing the turn
enum class NORMAL_METHOD
{
  CROSS_PRODUCT,
};

// method to optimize the direction after the normal has been computed
enum class DIRECTION_OPTIMIZATION_METHOD
{
  NONE,
};

// method to compute an average orientation
// in a zone where the orientation should be constant
// (but could contain noise)
enum class AVERAGE_ORIENTATION_METHOD
{
  SINGLE_POINT
};

std::vector<std::vector<double>> ComputeTurns(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> poseTrajectory,
        double timeWindow,
        double curveTreshold,
        bool emptyIntersection,
        std::string debugCSV,
        bool verbose
        )
{
  int minPtsInDirectionSets = 3; // could be as low as 2
  int minPtsInTurn = 3; // could be as low as 1
  std::ofstream debugFile;
  bool debugFileEnabled = !debugCSV.empty();
  if (debugFileEnabled)
  {
    debugFile.open(debugCSV);
  }
  // Compute local curvature, in rad/second
  // for each sample: is it inside a turn ?
  // Using the curveTreshold treshold
  std::vector<int> sampleId =
    std::vector<int>();
  sampleId.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<double> sampleTime =
    std::vector<double>();
  sampleTime.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<bool> sampleStatus =
    std::vector<bool>();
  sampleStatus.reserve(poseTrajectory->GetNumberOfTransforms());

  if (verbose)
  {
    std::cout << "Computing per-sample status for "
              << poseTrajectory->GetNumberOfTransforms()
              << " samples" << std::endl;
  }
  for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
  {
    vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
    double t;
    poseTrajectory->GetSample(i, sample, t);
    if (t - 0.5 * timeWindow < poseTrajectory->GetMinimumT()
        || t + 0.5 * timeWindow > poseTrajectory->GetMaximumT())
    {
      continue;
    }
    sampleId.push_back(i);
    sampleTime.push_back(t);

    vtkSmartPointer<vtkTransform> prev = vtkSmartPointer<vtkTransform>::New();
    poseTrajectory->InterpolateTransform(t - 0.5 * timeWindow, prev);
    vtkSmartPointer<vtkTransform> next = vtkSmartPointer<vtkTransform>::New();
    poseTrajectory->InterpolateTransform(t + 0.5 * timeWindow, next);
    Eigen::AngleAxisd aa = Eigen::AngleAxisd(RotationMatrixFromTransform(next)
		    * RotationMatrixFromTransform(prev).transpose());
    double curvature = std::abs(aa.angle()) / timeWindow;
    sampleStatus.push_back(curvature >= curveTreshold);
  }

  if (verbose)
  {
    std::cout << "Aggregating samples" << std::endl;
  }
  // Compute turn limits (exprimed in samples indexes)
  unsigned int i = 0; // ! warning that is not the id of a sample
  // if the first samples are inside a turn, discard this turn
  // and start searching turns after its end.
  while (i < sampleStatus.size() && sampleStatus[i] == true)
  {
    i = i + 1;
  }
  // we are ready to search turns and know that sample i
  // is not inside a turn
  bool inside = false;

  // turn_limits[n][0] will contain the time of the first sample inside turn n
  // turn_limits[n][1] will contain the time of the last sample inside turn n
  // We have turn_limits[n][0] <= turn_limits[n][1]
  std::vector<std::vector<int>> turnLimits = std::vector<std::vector<int>>();
  for (unsigned int j = i; j < sampleStatus.size(); j++)
  {
    if (!inside && sampleStatus[j])
    {
      // entering turn
      inside = true;
      turnLimits.push_back(std::vector<int>());
      turnLimits[turnLimits.size() - 1].push_back(j);
    }
    else if (inside && !sampleStatus[j])
    {
      // leaving turn
      inside = false;
      turnLimits[turnLimits.size() - 1].push_back(j - 1);
    }
  }
  // if the end of last turn is not seen, discard this turn
  if (inside)
  {
    if (verbose)
    {
      std::cout << "pop_back" << std::endl;
    }
    turnLimits.pop_back();
  }

  // convert indexes to times:
  if (verbose)
  {
    std::cout << "Converting indexes to time" << std::endl;
  }
  std::vector<std::vector<double>> turnTimes = std::vector<std::vector<double>>();

  for (unsigned int i = 0; i < turnLimits.size(); i++) {
    if (turnLimits[i][1] - turnLimits[i][0] + 1 < minPtsInTurn)
    {
      if (verbose)
      {
        std::cout << "warning: skipped one turn that did not contain enough points" << std::endl;
      }
      continue;
    }
    std::vector<double> times = std::vector<double>(2);
    times[0] = sampleTime[turnLimits[i][0]];
    times[1] = sampleTime[turnLimits[i][1]];
    turnTimes.push_back(times);
  }

  // Now we want to get a direction vector before and after the curve,
  // if that is possible.
  // ! we cannot use the time to look N seconds before/after,
  // because turns are ofen preceded by a traffic light
  // which can be red, in this case there is a stop in the trajectory,
  // right before the turn. Sometimes there is a stop
  // just at the end of a turn if there is a traffic jam.
  // In both cases the risk is that the set of points selected with time
  // before/after the turn will be just 1 point,
  // or will be a set of points very centered locally.
  // (So this set of points will not permit a reliable estimation of
  // a direction vector)

  // precompute a way to tell the length of the curve at any time
  // (will be used by dichotomy search)
  if (verbose)
  {
    std::cout << "Precomputing curve length" << std::endl;
  }
  double currentLength = 0.0;
  std::vector<double> length = std::vector<double>();
  length.reserve(poseTrajectory->GetNumberOfTransforms());
  std::vector<double> lengthTimes = std::vector<double>();
  lengthTimes.reserve(poseTrajectory->GetNumberOfTransforms());
  vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
  double time;
  poseTrajectory->GetSample(0, sample, time);
  Eigen::Vector3d previousPos = PositionVectorFromTransform(sample);
  for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
  {
    poseTrajectory->GetSample(i, sample, time);
    Eigen::Vector3d pos = PositionVectorFromTransform(sample);
    currentLength += (pos - previousPos).norm();
    lengthTimes.push_back(time);
    length.push_back(currentLength);
    previousPos = pos;
  }

  if (verbose)
  {
    std::cout << "Placing bounds" << std::endl;
  }
  std::vector<std::vector<double>> bounds = std::vector<std::vector<double>>();
  for (unsigned int i = 0; i < turnTimes.size(); i++)
  {
    int iStart = std::distance(lengthTimes.begin(),
		    std::lower_bound(lengthTimes.begin(), lengthTimes.end(), turnTimes[i][0]));
    int iEnd = std::distance(lengthTimes.begin(),
		    std::lower_bound(lengthTimes.begin(), lengthTimes.end(), turnTimes[i][1]));
    double turnLength = length[iEnd] - length[iStart];
    if (verbose)
    {
      std::cout << "turn length is: " << turnLength << " (scale unknown)" << std::endl;
    }
    int iBefore = std::distance(length.begin(),
		    std::lower_bound(length.begin(), length.end(), length[iStart] - turnLength));
    int iAfter = std::distance(length.begin(),
		    std::lower_bound(length.begin(), length.end(), length[iEnd] + turnLength));
    // if it was not possible to find iBefore/iAfter
    // it means that the turn is either at the very start
    // or very end of the trajectory, so it is hard to tell which
    // was the direction before the turn
    if (iBefore == 0 || iAfter == static_cast<int>(length.size()))
    {
      continue;
    }
    // if there is not enough points in sets "before" or "after",
    // discard this turn:
    if (iStart - iBefore + 1 < minPtsInDirectionSets
        || iAfter - iEnd + 1 < minPtsInDirectionSets)
    {
      continue;
    }
    std::vector<double> bound = std::vector<double>(4);
    bound[0] = lengthTimes[iBefore];
    bound[1] = turnTimes[i][0];
    bound[2] = turnTimes[i][1];
    bound[3] = lengthTimes[iAfter];
    bounds.push_back(bound);
  }

  // look for bug in my algo/my code/the data:
  std::vector<int> misbounded = std::vector<int>();
  for (unsigned int i = 0; i < bounds.size(); i++)
  {
    if (!(bounds[i][0] < bounds[i][1]
          && bounds[i][1] < bounds[i][2]
          && bounds[i][2] < bounds[i][3]))
    {
      if (verbose)
      {
        std::cout << "warning: turn " << i
          << "(" << bounds[i][0] << ", "
          << bounds[i][1] << ", "
          << bounds[i][2] << ", "
          << bounds[i][3] << ") is not bounded correctly, discarding it."
          << std::endl;
      }
      misbounded.push_back(i);
    }
  }
  // delete misbounded turns:
  if (verbose)
  {
    std::cout << "Discarding mis-bounded turns" << std::endl;
  }
  for (int i = misbounded.size() - 1; i >= 0; i--)
  {
    bounds.erase(bounds.begin() + misbounded[i]);
  }

  // handle the cases where two turns are close
  // if possible, make the bounds intersection empty
  // this is not mandatory but it help visualization a lot
  if (emptyIntersection)
  {
    if (verbose)
    {
      std::cout << "Changing bounds to empty intersections" << std::endl;
    }
    std::vector<int> mixedUpTurns = std::vector<int>();
    for (unsigned int i = 1; i < bounds.size(); i++)
    {
      if (bounds[i - 1][3] > bounds[i][0])
      {
        double newLimit = 0.5 * (bounds[i - 1][3] + bounds[i][0]);
        // check if this new_limit solves the problem:
        if (newLimit <= bounds[i - 1][2])
        {
          // cannot solve easily, discard the first turn
          if (verbose)
          {
          std::cout << "warning: could not fix (first) turn " << i - 1
            << " discarding it" << std::endl;
          }
          mixedUpTurns.push_back(i - 1);
        }
        else if (newLimit >= bounds[i][1])
        {
          // cannot solve easily, discard the second turn
          if (verbose)
          {
            std::cout << "warning: could not fix (second) turn " << i
              << " discarding it" << std::endl;
          }
          mixedUpTurns.push_back(i);
        }
        else
        {
          // one could argue that we should not do this fix
          // because the lengths of the curve in the new after/before
          // parts will not be equal to the lengths inside the turns
          if (verbose)
          {
            std::cout << "warning: fixing bounds of turns " << i - 1
              << " and " << i << " that are touching each other" << std::endl;
          }
          bounds[i - 1][3] = newLimit;
          bounds[i][0] = newLimit;
        }
      }
    }

    if (verbose)
    {
      std::cout << "Discarding turns with no empty intersection" << std::endl;
    }
    // delete turns whose bounds intersection could not be made empty:
    for (int i = mixedUpTurns.size() - 1; i >= 0; i--)
    {
      bounds.erase(bounds.begin() + mixedUpTurns[i]);
    }
  }

  // write debug file:
  // may not work correctly if emptyIntersection != true
  if (debugFileEnabled)
  {
    if (verbose)
    {
      std::cout << "Writing debug file" << std::endl;
    }
    int currentTurnToConsider = 0;
    for (int i = 0; i < poseTrajectory->GetNumberOfTransforms(); i++)
    {
      vtkSmartPointer<vtkTransform> sample = vtkSmartPointer<vtkTransform>::New();
      double time;
      poseTrajectory->GetSample(i, sample, time);
      double status;
      if (currentTurnToConsider == static_cast<int>(bounds.size()))
      {
        status = 0.0; // will stay outside from now on
      }
      else if (time < bounds[currentTurnToConsider][0])
      {
        status = 0.0; // before
      }
      else if (time < bounds[currentTurnToConsider][1])
      {
        status = 0.4; // inside "before" set of samples
      }
      else if (time < bounds[currentTurnToConsider][2])
      {
        status = 1.0; // inside turn samples
      }
      else if (time < bounds[currentTurnToConsider][3])
      {
        status = 0.2; // inside "after" set of samples
      }
      else
      {
        status = 0.0; // after
        currentTurnToConsider = currentTurnToConsider + 1;
      }
      if (debugFileEnabled)
      {
        debugFile << PositionVectorFromTransform(sample)[0]
                  << "," << PositionVectorFromTransform(sample)[1]
                  << "," << PositionVectorFromTransform(sample)[2]
                  << "," << status << std::endl;
      }
    }
  }

  if (debugFileEnabled)
  {
    if (verbose)
    {
      std::cout << "Closing debug file" << std::endl;
    }
    debugFile.close();
  }

  return bounds;
}

bool ProcessTurn(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> reference,
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> aligned,
        double t0, double t1, double t2, double t3,
        DIRECTION_METHOD directionMethod,
        NORMAL_METHOD normalMethod,
        DIRECTION_OPTIMIZATION_METHOD normalBasedDirectionOptimisation,
        AVERAGE_ORIENTATION_METHOD orientationMethod,
        Eigen::Matrix3d& rotationBefore, // 1st result
        Eigen::Matrix3d& rotationAfter, // 2nd result
        double& scaleBefore,
        double& scaleAfter
        )
{
  if (!(t0 < t1 && t1 < t2 && t2 < t3))
  {
    std::cerr << "WARNING: times not valid in ProcessTurn(). Bug in implementation" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  Eigen::Vector3d vBeforeReference = GetXYZ(reference, t1) - GetXYZ(reference, t0);
  Eigen::Vector3d vAfterReference = GetXYZ(reference, t3) - GetXYZ(reference, t2);
  Eigen::Vector3d vBeforeAligned = GetXYZ(aligned, t1) - GetXYZ(aligned, t0);
  Eigen::Vector3d vAfterAligned = GetXYZ(aligned, t3) - GetXYZ(aligned, t2);

  scaleBefore = vBeforeAligned.norm() / vBeforeReference.norm();
  scaleAfter = vAfterAligned.norm() / vAfterReference.norm();

  // depending on options, these values will be recomputed later
  // using a more robust method
  Eigen::Vector3d directionBeforeReference = vBeforeReference.normalized();
  Eigen::Vector3d directionAfterReference = vAfterReference.normalized();
  Eigen::Vector3d directionBeforeAligned = vBeforeAligned.normalized();
  Eigen::Vector3d directionAfterAligned = vAfterAligned.normalized();

  if (directionMethod == DIRECTION_METHOD::TWO_POINTS)
  {
    // no need to recompute direction{Before,After}{Reference,Aligned}
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute normal to plane containing the trajectory
  Eigen::Vector3d normalOrientationReference;
  Eigen::Vector3d normalOrientationAligned;
  if (normalMethod == NORMAL_METHOD::CROSS_PRODUCT)
  {
    normalOrientationReference = directionBeforeReference.cross(directionAfterReference);
    normalOrientationAligned = directionBeforeAligned.cross(directionAfterAligned);
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  if (normalBasedDirectionOptimisation == DIRECTION_OPTIMIZATION_METHOD::NONE)
  {
    // nothing to do
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute orientation of trajectories
  Eigen::Matrix3d trajectoryOrientationBeforeReference;
  // operator "<<" stacks the vectors horizontally
  // (Eigen vectors are vertical i.e. single-column matrices)
  trajectoryOrientationBeforeReference <<
      directionBeforeReference,
      normalOrientationReference,
      directionBeforeReference.cross(normalOrientationReference);

  Eigen::Matrix3d trajectoryOrientationAfterReference;
  trajectoryOrientationAfterReference <<
      directionAfterReference,
      normalOrientationReference,
      directionAfterReference.cross(normalOrientationReference);

  Eigen::Matrix3d trajectoryOrientationBeforeAligned;
  trajectoryOrientationBeforeAligned <<
      directionBeforeAligned,
      normalOrientationAligned,
      directionBeforeAligned.cross(normalOrientationAligned);

  Eigen::Matrix3d trajectoryOrientationAfterAligned;
  trajectoryOrientationAfterAligned <<
      directionAfterAligned,
      normalOrientationAligned,
      directionAfterAligned.cross(normalOrientationAligned);

  Eigen::Matrix3d RRemapBefore = trajectoryOrientationBeforeReference
      * trajectoryOrientationBeforeAligned.transpose();

  Eigen::Matrix3d RRemapAfter = trajectoryOrientationAfterReference
      * trajectoryOrientationAfterAligned.transpose();

  Eigen::Matrix3d orientationBeforeReference;
  Eigen::Matrix3d orientationAfterReference;
  Eigen::Matrix3d orientationBeforeAligned;
  Eigen::Matrix3d orientationAfterAligned;
  // compute remapped orientation of sensors
  if (orientationMethod == AVERAGE_ORIENTATION_METHOD::SINGLE_POINT)
  {
        orientationBeforeReference = GetR(reference, t0);
        orientationAfterReference = GetR(reference, t3);
        orientationBeforeAligned = GetR(aligned, t0);
        orientationAfterAligned = GetR(aligned, t3);
  }
  else
  {
    std::cerr << "WARNING: unimplemented option passed to ProcessTurn()" << std::endl;
    return false; // rotationBefore/After are not valid
  }

  // compute rotation between sensors
  // so that R_rotation * orientation_aligned = orientation_reference
  rotationBefore = orientationBeforeReference.transpose()
      * RRemapBefore * orientationBeforeAligned;
  rotationAfter = orientationAfterReference.transpose()
      * RRemapAfter * orientationAfterAligned;
  return true; // rotations are valid
}

bool ProcessTurn(
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> reference,
        const vtkSmartPointer<vtkVelodyneTransformInterpolator> aligned,
        double t0, double t1, double t2, double t3,
        Eigen::Matrix3d& rotationBefore, // 1st result
        Eigen::Matrix3d& rotationAfter, // 2nd result
        double scaleBefore,
        double scaleAfter
        )
{
  return ProcessTurn(
        reference,
        aligned,
        t0, t1, t2, t3,
        DIRECTION_METHOD::TWO_POINTS,
        NORMAL_METHOD::CROSS_PRODUCT,
        DIRECTION_OPTIMIZATION_METHOD::NONE,
        AVERAGE_ORIENTATION_METHOD::SINGLE_POINT,
        rotationBefore,
        rotationAfter,
        scaleBefore,
        scaleAfter
        );
}

enum class ROTATION_ESTIMATOR
{
  ESTIMATOR_UNDEFINED = 0,
  ESTIMATOR_L2_CHORDAL_SVD = 0
};

void estimateL2ChordalSVD(const std::vector<Eigen::Matrix3d>& rotations,
                    Eigen::Matrix3d& result,
                    bool& valid)
{
  if (rotations.size() == 0)
  {
    result = Eigen::Matrix3d::Identity();
    valid = false;
  }

  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  for (unsigned int i = 0; i < rotations.size(); i++)
  {
    S += rotations[i];
  }

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  // we should have S == U * D * V.transpose();

  Eigen::Matrix3d R = U *  V.transpose(); // not using singular values
  if (R.determinant() < 0.0) // i.e. close to -1
  {
    Eigen::Vector3d D;
    D << 1.0, 1.0, -1.0;
    R = U * D.asDiagonal() * V.transpose();
  }

  result = R;
  valid = true;
}

void RansacRotation(const std::vector<Eigen::Matrix3d>& rotations,
                    ROTATION_ESTIMATOR estimator,
                    int maxIterations,
                    int sampleToEstimate,
                    int sampleToValidate,
                    float maxAngleToFit,
                    Eigen::Matrix3d& result,
                    bool& valid,
                    int& samplesUsed)
{
  std::default_random_engine rng = std::default_random_engine();
  std::vector<int> shuffled(rotations.size());
  std::iota(std::begin(shuffled), std::end(shuffled), 0); //0 is the starting number

  if (estimator != ROTATION_ESTIMATOR::ESTIMATOR_L2_CHORDAL_SVD)
  {
    std::cerr << "Unknown estimator passed to RansacRotation" << std::endl;
    result = Eigen::Matrix3d::Identity();
    valid = false;
    return;
  }
  for (int i = 0; i < maxIterations; i++)
  {
    // re-shuffle
    std::shuffle(std::begin(shuffled), std::end(shuffled), rng);
    std::vector<Eigen::Matrix3d> samples = std::vector<Eigen::Matrix3d>(sampleToEstimate);
    for (int j = 0; j < sampleToEstimate; j++)
    {
      samples[j] = rotations[shuffled[j]];
    }

    Eigen::Matrix3d estimation;
    bool estimationValid = false;
    estimateL2ChordalSVD(samples, estimation, estimationValid);
    if (!estimationValid)
    {
      continue;
    }

    // count how many rotations are close enough:
    std::vector<Eigen::Matrix3d> samplesFitting = std::vector<Eigen::Matrix3d>();
    samplesFitting.reserve(rotations.size());
    for (unsigned int j = 0; j < rotations.size(); j++)
    {
      double angle = Eigen::AngleAxisd(estimation.transpose() * rotations[j]).angle();
      if (angle <= maxAngleToFit)
      {
        samplesFitting.push_back(rotations[j]);
      }
    }

    if (static_cast<int>(samplesFitting.size()) >= sampleToValidate)
    {
      // do a final estimation, then return
      samplesUsed = samplesFitting.size();
      Eigen::Matrix3d estimation;
      bool estimationValid = false;
      estimateL2ChordalSVD(samplesFitting, estimation, estimationValid);
      result = estimation;
      valid = estimationValid;
      return;
    }
  }

  result = Eigen::Matrix3d::Identity();
  valid = false;
}

void ComputeCarCalibrationRotationScale(
        const vtkSmartPointer<vtkTemporalTransforms> reference,
        const vtkSmartPointer<vtkTemporalTransforms> aligned,
        double curveTreshold,
        int ransacMaxIter,
        double ransacMaxAngleToFit,
        double ransacFittingRatio,
        double ransacValidationRatio,
        DIRECTION_METHOD directionMethod,
        NORMAL_METHOD normalMethod,
        DIRECTION_OPTIMIZATION_METHOD normalBasedDirectionOptimisation,
        AVERAGE_ORIENTATION_METHOD orientationMethod,
        Eigen::Matrix3d& result,
        double& scale,
        bool& validResult,
        bool verbose
        )
{
  vtkSmartPointer<vtkVelodyneTransformInterpolator> referenceI
      = reference->CreateInterpolator();
  referenceI->SetInterpolationTypeToLinear();
  vtkSmartPointer<vtkVelodyneTransformInterpolator> alignedI
      = aligned->CreateInterpolator();
  alignedI->SetInterpolationTypeToLinear();
  if (verbose)
  {
    std::cout << "Detecting turns" << std::endl;
  }
  std::vector<std::vector<double>> turns =
      ComputeTurns(referenceI, 0.8, curveTreshold, false, "", verbose);
  if (verbose)
  {
    std::cout << "Processing " << turns.size() << " turns" << std::endl;
  }

  std::vector<Eigen::Matrix3d> rotations = std::vector<Eigen::Matrix3d>();
  std::vector<double> scales;
  for (unsigned int i = 0; i < turns.size(); i++)
  {
    if (turns[i][0] < alignedI->GetMinimumT()
                    || turns[i][3] > alignedI->GetMaximumT())
    {
      // if this turn (that was seen in "reference")
      // is not contained in aligned, skip it
      continue;
    }
    Eigen::Matrix3d RBefore;
    Eigen::Matrix3d RAfter;
    double scaleBefore, scaleAfter;
    ProcessTurn(referenceI, alignedI,
                turns[i][0], turns[i][1], turns[i][2], turns[i][3],
                    directionMethod,
                    normalMethod,
                    normalBasedDirectionOptimisation,
                    orientationMethod,
                    RBefore,
                    RAfter,
                    scaleBefore,
                    scaleAfter);

    scales.push_back(scaleBefore);
    scales.push_back(scaleAfter);

    Eigen::Vector3d yprBefore = (180.0 / vtkMath::Pi()) * RBefore.eulerAngles(2,1,0);
    Eigen::Vector3d yprAfter = (180.0 / vtkMath::Pi()) * RAfter.eulerAngles(2,1,0);
    if (verbose)
    {
      std::cout << "rotation before: " << yprBefore[2]
                << ", " << yprBefore[1]
                << ", " << yprBefore[0] << std::endl;
      std::cout << "rotation after: " << yprAfter[2]
                << ", " << yprAfter[1]
                << ", " << yprAfter[0] << std::endl;
    }

    if (RBefore.allFinite())
    {
      rotations.push_back((RBefore));
    }
    if (RAfter.allFinite())
    {
      rotations.push_back((RAfter));
    }
  }

  if (verbose)
  {
    // must be displayed before any call of median on the std::vector, else the
    // order is changed
    for (unsigned int i = 0; i < scales.size() / 2; i++)
    {
      std::cout << "scale before: " << scales[2*i]
                << ", scale after: " << scales[2*i+1] << std::endl;
    }
  }

  scale = ComputeMedian(scales);

  if (verbose)
  {
    std::cout << "median of scales is: " << ComputeMedian(scales) << std::endl;
  }

  Eigen::Matrix3d R;
  bool valid;
  int sampleUsed = 0;
  RansacRotation(rotations,
                 ROTATION_ESTIMATOR::ESTIMATOR_L2_CHORDAL_SVD,
                 ransacMaxIter,
                 std::max(1, static_cast<int>(vtkMath::Round(ransacFittingRatio * static_cast<double>(rotations.size())))),
                 std::max(1, static_cast<int>(vtkMath::Round(ransacValidationRatio * static_cast<double>(rotations.size())))),
                 (vtkMath::Pi() / 180.0) * ransacMaxAngleToFit,
                 R,
                 valid,
                 sampleUsed);

  if (verbose)
  {
    if (valid)
    {
      std::cout << "Ransac result is valid" << std::endl;
    }
    else
    {
      std::cout << "Ransac result not valid" << std::endl;
    }
  }
  Eigen::Vector3d rotationYPR = (180.0 / vtkMath::Pi()) * R.eulerAngles(2,1,0);
  if (verbose)
  {
    std::cout << "Rotation found using " << sampleUsed << " samples among " << rotations.size() << ": "
              << rotationYPR[2] << ", " << rotationYPR[1] << ", " << rotationYPR[0] << std::endl;
  }

  if (valid)
  {
    result = R;
  }
  else
  {
    result = Eigen::Matrix3d::Identity();
  }

  validResult = valid;
}

void ComputeCarCalibrationRotationScale(
        const vtkSmartPointer<vtkTemporalTransforms> reference,
        const vtkSmartPointer<vtkTemporalTransforms> aligned,
        double curveTreshold,
        int ransacMaxIter,
        double ransacMaxAngleToFit,
        double ransacFittingRatio,
        double ransacValidationRatio,
        Eigen::Matrix3d& result,
        double& scale,
        bool& validResult,
        bool verbose
        )
{
  ComputeCarCalibrationRotationScale(reference, aligned, curveTreshold,
        ransacMaxIter, ransacMaxAngleToFit,
        ransacFittingRatio, ransacValidationRatio,
        DIRECTION_METHOD::TWO_POINTS,
        NORMAL_METHOD::CROSS_PRODUCT,
        DIRECTION_OPTIMIZATION_METHOD::NONE,
        AVERAGE_ORIENTATION_METHOD::SINGLE_POINT,
        result, scale, validResult, verbose);
}


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 * \file
 *
 * Matrix factorization with the Alternative Least Squares (ALS)  - parallel coordinate descent algorithm.
 * See the papers:
 * H.-F. Yu, C.-J. Hsieh, S. Si, I. S. Dhillon, Scalable Coordinate Descent Approaches to Parallel Matrix Factorization for Recommender Systems. IEEE International Conference on Data Mining(ICDM), December 2012.
 * Steffen Rendle, Zeno Gantner, Christoph Freudenthaler, and Lars Schmidt-Thieme. 2011. Fast context-aware recommendations with factorization machines. In Proceedings of the 34th international ACM SIGIR conference on Research and development in Information Retrieval (SIGIR '11). ACM, New York, NY, USA, 635-644.
 * Written by Danny Bickson, CMU
 */

#include <graphlab/util/stl_util.hpp>
#include <graphlab/util/timer.hpp>
#include <graphlab.hpp>
#include <graphlab/engine/gl3engine.hpp>
#include <Eigen/Dense>
#include "eigen_serialization.hpp"
#include <graphlab/macros_def.hpp>

typedef Eigen::VectorXd vec_type;

#define ALS_COORD_MAP_REDUCE 0
#define ALS_COORD_TRANSFORM 1
//when using negative node id range, we are not allowed to use
//0 and 1 so we add 2.
const static int SAFE_NEG_OFFSET=2;
const static int regnormal = 0;
static bool debug;
int max_iter = 10;
double maxval = 1e100;
double minval = -1e100;
std::string predictions;
bool isuser(uint node){
  return ((int)node) >= 0;
}

/**
 * \ingroup toolkit_matrix_factorization
 *
 * \brief the vertex data type which contains the latent pvec.
 *
 * Each row and each column in the matrix corresponds to a different
 * vertex in the SGD graph.  Associated with each vertex is a pvec
 * (vector) of latent parameters that represent that vertex.  The goal
 * of the SGD algorithm is to find the values for these latent
 * parameters such that the non-zero entries in the matrix can be
 * predicted by taking the dot product of the row and column pvecs.
 */
struct vertex_data {
  /**
   * \brief A shared "constant" that specifies the number of latent
   * values to use.
   */
  static size_t NLATENT;
  /** \brief The latent pvec for this vertex */
  vec_type pvec;
  vec_type prev;
  float z;
  int t; //index inside the latent feature vector

  /**
   * \brief Simple default constructor which randomizes the vertex
   *  data
   */
  vertex_data() : t(0),z(0) { if (debug) pvec = vec_type::Ones(NLATENT); else randomize(); prev = vec_type::Zero(NLATENT); }
  /** \brief Randomizes the latent pvec */
  void randomize() { pvec.resize(NLATENT); pvec.setRandom(); }
  /** \brief Save the vertex data to a binary archive */
  void save(graphlab::oarchive& arc) const {
    arc << pvec << t << prev << z;
  }
  /** \brief Load the vertex data from a binary archive */
  void load(graphlab::iarchive& arc) {
    arc >> pvec >> t >> prev >> z;
  }
}; // end of vertex data

std::size_t hash_value(vertex_data const& b) {
  return (size_t)b.pvec[0]*1000;
}

/**
 * \brief The edge data stores the entry in the matrix.
 *
 * In addition the edge data sgdo stores the most recent error estimate.
 */
struct edge_data : public graphlab::IS_POD_TYPE {
  /**
   * \brief The type of data on the edge;
   *
   * \li *Train:* the observed value is correct and used in training
   * \li *Validate:* the observed value is correct but not used in training
   * \li *Predict:* The observed value is not correct and should not be
   *        used in training.
   */
  enum data_role_type { TRAIN, VALIDATE, PREDICT  };

  /** \brief the observed value for the edge */
  float obs;

  /** \brief cached value for A_ij - prediction */
  float R_ij;

  /** \brief The train/validation/test designation of the edge */
  data_role_type role;

  /** \brief basic initialization */
  edge_data(float obs = 0, data_role_type role = PREDICT) :
    obs(obs), role(role), R_ij(0) { }

}; // end of edge data

std::size_t hash_value(edge_data const& b) {
  return boost::hash_value(b.obs);
}

/**
 * \brief The graph type is defined in terms of the vertex and edge
 * data.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;
typedef graphlab::gl3engine<graph_type> engine_type;

bool isuser_node(const graph_type::vertex_type& vertex){
  return isuser(vertex.id());
}

/**
 * \brief The graph loader function is a line parser used for
 * distributed graph construction.
 */
inline bool graph_loader(graph_type& graph,
                         const std::string& filename,
                         const std::string& line) {
  ASSERT_FALSE(line.empty());
  // Determine the role of the data
  edge_data::data_role_type role = edge_data::TRAIN;
  if(boost::ends_with(filename,".validate")) role = edge_data::VALIDATE;
  else if(boost::ends_with(filename, ".predict")) role = edge_data::PREDICT;
  // Parse the line
  std::stringstream strm(line);
  graph_type::vertex_id_type source_id(-1), target_id(-1);
  float obs(0);
  strm >> source_id >> target_id;

  // for test files (.predict) no need to read the actual rating value.
  if(role == edge_data::TRAIN || role == edge_data::VALIDATE) {
    strm >> obs;
  }
  target_id = -(graphlab::vertex_id_type(target_id + SAFE_NEG_OFFSET));

  // Create an edge and add it to the graph
  graph.add_edge(source_id, target_id, edge_data(obs, role));
  return true; // successful load
} // end of graph_loader

double LAMBDA = 0.001;

class gather_type {
public:
  double numerator;
  double denominator;

  gather_type() {
    numerator = 0;
    denominator = 0;
  }

  gather_type(double numerator, double denominator) : numerator(numerator),
     denominator(denominator){
  }

  /** \brief Save the values to a binary archive */
  void save(graphlab::oarchive& arc) const { arc << numerator << denominator; }

  /** \brief Read the values from a binary archive */
  void load(graphlab::iarchive& arc) { arc >> numerator >> denominator; }

  /**
   * sums up values
   */
  gather_type& operator+=(const gather_type& other) {
    numerator += other.numerator;
    denominator += other.denominator;
    return *this;
  } // end of operator+=

}; // end of gather type

gather_type als_coord_map(const graph_type::vertex_type& center,
                         graph_type::edge_type& edge,
                         const graph_type::vertex_type& other) {

   if (center.data().t == 0){
     double prediction = center.data().pvec.dot(other.data().pvec);
     prediction = std::min(prediction, maxval);
     prediction = std::max(prediction, minval);
     edge.data().R_ij = edge.data().obs - prediction;
   }
   //compute numerator of equation (6) in ICDM paper above
   //             (A_ij        - w_i^T*h_j  + wit          * h_jt        )*h_jt
   gather_type ret((edge.data().R_ij
                               + center.data().pvec[center.data().t] * other.data().pvec[center.data().t])*other.data().pvec[center.data().t],
   //compute denominator of equation (6) in ICDM paper above
   //h_jt^2
     pow(other.data().pvec[center.data().t], 2));
   return ret;

}

void als_coord_transform(const graph_type::vertex_type& center,
                         graph_type::edge_type& edge,
                         const graph_type::vertex_type& other) {
   //update using equation (7) in ICDM paper
   //R_ij     -= (z             - w_it         )*h_jt
   edge.data().R_ij -= (center.data().z - center.data().prev[center.data().t])*other.data().pvec[center.data().t];
}

//sum up two numerators and denomenators
void als_coord_combine(gather_type& v1, const gather_type& v2) {
    v1 += v2;
}

//the main update function
void als_coord_function(engine_type::context_type& context,
                  graph_type::vertex_type& vertex) {

   double regularization = LAMBDA;
   for (vertex.data().t=0; vertex.data().t< (int)vertex_data::NLATENT; vertex.data().t++){
     gather_type frac =  context.map_reduce<gather_type>(ALS_COORD_MAP_REDUCE, graphlab::ALL_EDGES);
     assert(frac.denominator > 0);
     vertex.data().z = (frac.numerator/(frac.denominator+regularization));
     vertex.data().prev = vertex.data().pvec;
     //update using equation (8) in ICDM paper
     //w_it                              = z;
     vertex.data().pvec[vertex.data().t] = vertex.data().z;

     //update the cached R_ij using equation (7) in ICDM paper
     context.edge_transform(ALS_COORD_TRANSFORM, graphlab::ALL_EDGES);
   }

}

/**
 * \brief Given an edge compute the error associated with that edge
 */
double extract_l2_error(const graph_type::edge_type & edge) {
  double pred =
      edge.source().data().pvec.dot(edge.target().data().pvec);
  double rmse = (edge.data().obs - pred) * (edge.data().obs - pred);
  return rmse;
} // end of extract_l2_error

size_t vertex_data::NLATENT = 20;
/**
 * \brief The prediction saver is used by the graph.save routine to
 * output the final predictions back to the filesystem.
 */
struct prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    return ""; //nop
  }
  std::string save_edge(const edge_type& edge) const {
    if(edge.data().role == edge_data::PREDICT) {
      std::stringstream strm;
      double prediction =
        edge.source().data().pvec.dot(edge.target().data().pvec);
      prediction = std::min(prediction, maxval);
      prediction = std::max(prediction, minval);
      strm << edge.source().id() << '\t';
      strm << (-edge.target().id() - SAFE_NEG_OFFSET) << '\t';
      strm << prediction << '\n';
      return strm.str();
    } else return "";
  }
}; // end of prediction_saver

struct linear_model_saver_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
  */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
        ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
  */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + ") ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
        ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

int main(int argc, char** argv) {
  global_logger().set_log_level(LOG_INFO);
  global_logger().set_log_to_console(true);

  // Parse command line options -----------------------------------------------
  const std::string description =
      "Compute the ALS factorization of a matrix.";
  graphlab::command_line_options clopts(description);
  std::string input_dir;
  std::string exec_type = "synchronous";
  clopts.attach_option("matrix", input_dir,
                       "The directory containing the matrix file");
  clopts.add_positional("matrix");
  clopts.attach_option("D", vertex_data::NLATENT,
                       "Number of latent parameters to use.");
  clopts.attach_option("maxval", maxval, "max allowed value");
  clopts.attach_option("minval", minval, "min allowed value");
  clopts.attach_option("predictions", predictions,
                       "The prefix (folder and filename) to save predictions.");
  clopts.attach_option("lambda", LAMBDA,
                       "regularization weight");
  clopts.attach_option("max_iter", max_iter,
                       "number of iterations");
  if(!clopts.parse(argc, argv) || input_dir == "") {
    std::cout << "Error in parsing command line arguments." << std::endl;
    clopts.print_description();
    return EXIT_FAILURE;
  }
  ///! Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  dc.cout() << "Loading graph." << std::endl;
  graphlab::timer timer;
  graph_type graph(dc, clopts);
  graph.load(input_dir, graph_loader);
  dc.cout() << "Loading graph. Finished in "
            << timer.current_time() << std::endl;

  dc.cout() << "Finalizing graph." << std::endl;
  timer.start();
  graph.finalize();
  dc.cout() << "Finalizing graph. Finished in "
            << timer.current_time() << std::endl;

  if (!graph.num_edges() || !graph.num_vertices())
     logstream(LOG_FATAL)<< "Failed to load graph. Check your input path: " << input_dir << std::endl;

  dc.cout()
      << "========== Graph statistics on proc " << dc.procid()
      << " ==============="
      << "\n Num vertices: " << graph.num_vertices()
      << "\n Num edges: " << graph.num_edges()
      << "\n Num replica: " << graph.num_replicas()
      << "\n Replica to vertex ratio: "
      << float(graph.num_replicas())/graph.num_vertices()
      << "\n --------------------------------------------"
      << "\n Num local own vertices: " << graph.num_local_own_vertices()
      << "\n Num local vertices: " << graph.num_local_vertices()
      << "\n Replica to own ratio: "
      << (float)graph.num_local_vertices()/graph.num_local_own_vertices()
      << "\n Num local edges: " << graph.num_local_edges()
      //<< "\n Begin edge id: " << graph.global_eid(0)
        << "\n Edge balance ratio: "
        << float(graph.num_local_edges())/graph.num_edges()
        << std::endl;

  dc.cout() << "Creating engine" << std::endl;

  engine_type engine(dc, graph, clopts);
  engine.register_map_reduce(ALS_COORD_MAP_REDUCE, als_coord_map, als_coord_combine);
  engine.register_edge_transform(ALS_COORD_TRANSFORM, als_coord_transform);
  for (int i=0; i< max_iter; i++){
     engine.parfor_all_local_vertices(als_coord_function);
     engine.wait();
     double rmse = graph.map_reduce_edges<double>(extract_l2_error);
     dc.cout() << "RMSE = " << sqrt(rmse / graph.num_edges()) << std::endl;
  }

  const double runtime = timer.current_time();
  dc.cout() << "----------------------------------------------------------"
            << std::endl
            << "Final Runtime (seconds):   " << runtime;

  // Compute the final training error -----------------------------------------
  dc.cout() << "Final error: " << std::endl;
  // Make predictions ---------------------------------------------------------
  if(!predictions.empty()) {
    std::cout << "Saving predictions" << std::endl;
    const bool gzip_output = false;
    const bool save_vertices = false;
    const bool save_edges = true;
    const size_t threads_per_machine = 2;

    //save the predictions
    graph.save(predictions, prediction_saver(),
               gzip_output, save_vertices,
               save_edges, threads_per_machine);
    //save the linear model
    graph.save(predictions + ".U", linear_model_saver_U(),
		gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".V", linear_model_saver_V(),
		gzip_output, save_edges, save_vertices, threads_per_machine);

  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // end of main


/**
 * \file fea/tetrahedral_mesh.cpp
 * This file is part of SANM, a symbolic asymptotic numerical solver.
 */

#include "fea/tetrahedral_mesh.h"
#include "fea/mesh_template.h"

#include <Eigen/Dense>

#include <cerrno>
#include <cstdio>
#include <cstring>
#include <fstream>

using namespace fea;

INST_MESH_IO_TRANS(3, TetrahedralMesh);

namespace {
void copy_mat3_cols(fp_t* dst, const Vec3& c0, const Vec3& c1, const Vec3& c2) {
    const Vec3* cs[] = {&c0, &c1, &c2};
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            dst[i * 3 + j] = (*cs[j])[i];
        }
    }
}
}  // anonymous namespace

const CoordMat3D& TetrahedralMesh::vertex_norms() const {
    if (m_vertex_norms.valid()) {
        return m_vertex_norms.val();
    }
    size_t nr_tet = this->nr_tet();
    m_vertex_norms.init();
    CoordMat3D& vertex_norms = m_vertex_norms.val();
    vertex_norms.resize(3, nr_tet * 4);
    m_tet_volumes.resize(nr_tet);
    auto ds_ptr = m_shape_matrix.set_shape({nr_tet, 3, 3}).woptr();

    for (size_t i = 0; i < nr_tet; ++i) {
        Vec3 x0 = m_vertices.col(m_tet(0, i)), x1 = m_vertices.col(m_tet(1, i)),
             x2 = m_vertices.col(m_tet(2, i)), x3 = m_vertices.col(m_tet(3, i)),
             v1 = x1 - x0, v2 = x2 - x0, v3 = x3 - x0;
        copy_mat3_cols(ds_ptr + i * 9, v1, v2, v3);

        fp_t det = v1.dot(v2.cross(v3));

        m_tet_volumes[i] = std::fabs(det) / 6;

        // - volume * D^{-T}, equal to -cofactor*sign(det)
        // normal for a vertex is also the area-weighted outward normal of the
        // opposite face

        Vec3 t1 = v2.cross(v3), t2 = v3.cross(v1), t3 = v1.cross(v2);
        if (det > 0) {
            t1 = -t1;
            t2 = -t2;
            t3 = -t3;
        }
        vertex_norms.col(4 * i + 0) = -(t1 + t2 + t3);
        vertex_norms.col(4 * i + 1) = t1;
        vertex_norms.col(4 * i + 2) = t2;
        vertex_norms.col(4 * i + 3) = t3;
    }
    vertex_norms *= 1.0 / 6;
    return vertex_norms;
}

const std::vector<fp_t>& TetrahedralMesh::tet_volumes() const {
    if (m_tet_volumes.empty()) {
        vertex_norms();
    }
    return m_tet_volumes;
}

const sanm::TensorND& TetrahedralMesh::shape_matrix() const {
    if (m_shape_matrix.empty()) {
        vertex_norms();
    }
    return m_shape_matrix;
}

const MeshVertexReverseList& TetrahedralMesh::vertex_reverse_list() const {
    if (!m_vertex_reverse_list.valid()) {
        m_vertex_reverse_list.init(
                MeshVertexReverseList::from_mesh<4>(nr_vertices(), m_tet));
    }
    return m_vertex_reverse_list.val();
}

TetrahedralMeshPtr TetrahedralMesh::make_cuboid(size_t nr_vtx_x,
                                                size_t nr_vtx_y,
                                                size_t nr_vtx_z, fp_t size) {
    sanm_assert(nr_vtx_x >= 2 && nr_vtx_y >= 2 && nr_vtx_z >= 2 && size > 0);
    // code adopted from the CompFabAssignment
    const int vertex_num = nr_vtx_x * nr_vtx_y * nr_vtx_z;
    const int element_num =
            5 * (nr_vtx_x - 1) * (nr_vtx_y - 1) * (nr_vtx_z - 1);
    auto ret = std::make_shared<TetrahedralMesh>();
    CoordMat3D& vertex = ret->m_vertices;
    TetIndexMat& element = ret->m_tet;
    vertex.resize(3, vertex_num);
    element.resize(4, element_num);
    int id = 0;
    for (size_t i = 0; i < nr_vtx_x; i++) {
        for (size_t j = 0; j < nr_vtx_y; j++) {
            for (size_t k = 0; k < nr_vtx_z; k++) {
                vertex(0, id) = i * size;
                vertex(1, id) = j * size;
                vertex(2, id) = k * size;
                if (i == 0 || i == nr_vtx_x - 1 || j == 0 ||
                    j == nr_vtx_y - 1 || k == 0 || k == nr_vtx_z - 1) {
                    ret->m_surface_vtx.insert(id);
                }
                id++;
            }
        }
    }
    auto get_cuboid_id = [nr_vtx_y, nr_vtx_z](size_t x, size_t y,
                                              size_t z) -> int {
        return (x * nr_vtx_y + y) * nr_vtx_z + z;
    };
    id = 0;
    for (size_t i = 0; i < nr_vtx_x - 1; i++) {
        for (size_t j = 0; j < nr_vtx_y - 1; j++) {
            for (size_t k = 0; k < nr_vtx_z - 1; k++) {
                int hex_id[] = {get_cuboid_id(i, j, k),
                                get_cuboid_id(i + 1, j, k),
                                get_cuboid_id(i + 1, j + 1, k),
                                get_cuboid_id(i, j + 1, k),
                                get_cuboid_id(i, j, k + 1),
                                get_cuboid_id(i + 1, j, k + 1),
                                get_cuboid_id(i + 1, j + 1, k + 1),
                                get_cuboid_id(i, j + 1, k + 1)};
                auto add_face = [&](int a, int b, int c) {
                    auto& t = ret->m_surfaces.emplace_back();
                    t[0] = hex_id[a];
                    t[1] = hex_id[b];
                    t[2] = hex_id[c];
                };

                if (i == 0) {
                    add_face(3, 0, 7);
                    add_face(7, 0, 4);
                }
                if (i == nr_vtx_x - 2) {
                    add_face(1, 2, 6);
                    add_face(6, 5, 1);
                }
                if (j == 0) {
                    add_face(0, 1, 5);
                    add_face(0, 5, 4);
                }
                if (j == nr_vtx_y - 2) {
                    add_face(7, 6, 3);
                    add_face(6, 2, 3);
                }
                if (k == 0) {
                    add_face(1, 3, 2);
                    add_face(0, 3, 1);
                }
                if (k == nr_vtx_z - 2) {
                    add_face(4, 5, 7);
                    add_face(7, 5, 6);
                }

                // 0, 2, 1, 5
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[2];
                element(2, id) = hex_id[1];
                element(3, id) = hex_id[5];
                id++;
                // 0, 4, 7, 5
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[4];
                element(2, id) = hex_id[7];
                element(3, id) = hex_id[5];
                id++;
                // 0, 2, 5, 7
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[2];
                element(2, id) = hex_id[5];
                element(3, id) = hex_id[7];
                id++;
                // 2, 6, 5, 7
                element(0, id) = hex_id[2];
                element(1, id) = hex_id[6];
                element(2, id) = hex_id[5];
                element(3, id) = hex_id[7];
                id++;
                // 0, 7, 3, 2
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[7];
                element(2, id) = hex_id[3];
                element(3, id) = hex_id[2];
                id++;
            }
        }
    }
    sanm_assert(id == element_num);
    return ret;
}

TetrahedralMeshPtr TetrahedralMesh::from_tetgen_files(
        const std::string& filebase) {
    std::ifstream fin_ele{filebase + ".ele"}, fin_node{filebase + ".node"},
            fin_face{filebase + ".face"};
    sanm_assert(fin_ele.good() && fin_node.good() && fin_face.good(),
                "failed to open input files: %s.{ele,node,face}",
                filebase.c_str());
    auto ret = std::make_shared<TetrahedralMesh>();

    // see https://wias-berlin.de/software/tetgen/fformats.node.html
    size_t nr_vtx, dim, nr_attr, bound_mark;
    fin_node >> nr_vtx >> dim >> nr_attr >> bound_mark;
    sanm_assert(dim == 3 && !nr_attr && !bound_mark);

    CoordMat3D& vtx = ret->m_vertices;
    vtx.resize(3, nr_vtx);
    for (size_t i = 0; i < nr_vtx; ++i) {
        size_t idx;
        fin_node >> idx >> vtx(0, i) >> vtx(1, i) >> vtx(2, i);
        sanm_assert(idx == i, "failed to read vertex %zu: got idx %zu", i, idx);
    }
    sanm_assert(fin_node.good());

    // see https://wias-berlin.de/software/tetgen/fformats.ele.html
    size_t nr_tet, node_per_tet;
    fin_ele >> nr_tet >> node_per_tet >> nr_attr;
    sanm_assert(node_per_tet == 4 && !nr_attr);

    TetIndexMat& tet = ret->m_tet;
    tet.resize(4, nr_tet);
    for (size_t i = 0; i < nr_tet; ++i) {
        size_t idx;
        fin_ele >> idx >> tet(0, i) >> tet(1, i) >> tet(2, i) >> tet(3, i);
        sanm_assert(idx == i, "failed to read tetrahedron %zu: got idx %zu", i,
                    idx);
    }

    size_t nr_face, boundary_marker;
    fin_face >> nr_face >> boundary_marker;
    for (size_t i = 0; i < nr_face; ++i) {
        size_t idx;
        int a, b, c;
        fin_face >> idx >> a >> b >> c;
        sanm_assert(idx == i);
        ret->m_surface_vtx.insert(a);
        ret->m_surface_vtx.insert(b);
        ret->m_surface_vtx.insert(c);
        if (boundary_marker) {
            fin_face >> b;
        }
        // do not read into m_surfaces since tetgen may invert the surface
    }

    return ret;
}

void TetrahedralMesh::write_to_file(FILE* fout,
                                    const VertexSet* filter_set) const {
    if (!filter_set) {
        if (!m_surfaces.empty()) {
            write_to_file(fout, m_vertices, m_surfaces);
            return;
        }

        if (!m_surface_vtx.empty()) {
            filter_set = &m_surface_vtx;
        }
    }
    write_to_file(fout, m_vertices, m_tet, filter_set);
}

void TetrahedralMesh::write_to_surface_vtx_file(FILE* fout) const {
    sanm_assert(!m_surface_vtx.empty());
    int vmin = std::numeric_limits<int>::max(), vmax = 0;
    for (int i : m_surface_vtx) {
        vmin = std::min(vmin, i);
        vmax = std::max(vmax, i);
    }
    sanm_assert(vmin == 0, "min surface vtx num is not zero: %d", vmin);
    sanm_assert(vmax == static_cast<int>(m_surface_vtx.size()) - 1,
                "max surface vtx num is %d, size is %zu", vmax,
                m_surface_vtx.size());

    for (int i = vmin; i <= vmax; ++i) {
        const Vec3& v = m_vertices.col(i);
        fprintf(fout, "%g %g %g\n", v[0], v[1], v[2]);
    }
}

void TetrahedralMesh::write_to_file(FILE* fout, const CoordMat3D& V,
                                    const TetIndexMat& F,
                                    const VertexSet* filter_set) {
    sanm_assert(fout);
    std::unordered_map<int, int> vtx_id_map;
    if (filter_set) {
        sanm_assert(!filter_set->empty());
    }
    auto write_facet = [fout, filter_set, &vtx_id_map](int v0, int v1, int v2) {
        if (filter_set) {
            if (!filter_set->count(v0) || !filter_set->count(v1) ||
                !filter_set->count(v2)) {
                return;
            }
            v0 = vtx_id_map.at(v0);
            v1 = vtx_id_map.at(v1);
            v2 = vtx_id_map.at(v2);
        }
        fprintf(fout, "f %d %d %d\n", v0 + 1, v1 + 1, v2 + 1);
    };
    for (int i = 0; i < V.cols(); ++i) {
        if (!filter_set || filter_set->count(i)) {
            if (filter_set) {
                int id = vtx_id_map.size();
                vtx_id_map[i] = id;
            }
            Vec3 vi = V.col(i);
            fprintf(fout, "v %g %g %g\n", vi.x(), vi.y(), vi.z());
        }
    }

    for (Eigen::Index i = 0; i < F.cols(); ++i) {
        int i0 = F(0, i), i1 = F(1, i), i2 = F(2, i), i3 = F(3, i);
        Vec3 v0 = V.col(i0), v1 = V.col(i1), v2 = V.col(i2), v3 = V.col(i3);

        if ((v1 - v0).dot((v2 - v0).cross(v3 - v0)) > 0) {
            std::swap(i1, i2);
        }

        write_facet(i0, i1, i2);
        write_facet(i1, i3, i2);
        write_facet(i1, i0, i3);
        write_facet(i0, i2, i3);
    }
}

void TetrahedralMesh::write_to_file(FILE* fout, const CoordMat3D& V,
                                    const FaceList& F) {
    sanm_assert(fout && !F.empty());
    std::unordered_map<int, int> vtx_id_map;
    std::vector<int> vtx_ids;
    vtx_ids.reserve(F.size() * 2);
    vtx_id_map.reserve(F.size() * 2);

    for (auto&& f : F) {
        for (int v : f) {
            int id = vtx_id_map.size();
            if (vtx_id_map.insert({v, id}).second) {
                vtx_ids.push_back(v);
            }
        }
    }

    for (int i : vtx_ids) {
        Vec3 vi = V.col(i);
        fprintf(fout, "v %g %g %g\n", vi.x(), vi.y(), vi.z());
    }

    for (auto&& f : F) {
        int v0 = vtx_id_map.at(f[0]), v1 = vtx_id_map.at(f[1]),
            v2 = vtx_id_map.at(f[2]);
        fprintf(fout, "f %d %d %d\n", v0 + 1, v1 + 1, v2 + 1);
    }
}

void TetrahedralMesh::replace_with_mask(const CoordMask3D& mask,
                                        const sanm::TensorND& value) {
    fea::replace_with_mask(m_vertices, mask, value.ptr(),
                           value.shape().total_nr_elems());
    clear_cache();
}

void TetrahedralMesh::apply_vtx_delta(const CoordMat3D& delta) {
    sanm_assert(delta.cols() == m_vertices.cols());
    m_vertices += delta;
    clear_cache();
}

void TetrahedralMesh::replace_vtx(const CoordMat3D& vtx) {
    sanm_assert(vtx.cols() == m_vertices.cols());
    m_vertices = vtx;
    clear_cache();
}

void TetrahedralMesh::clear_cache() {
    m_vertex_norms.reset();
    m_tet_volumes.clear();
    m_shape_matrix.clear();
}

void TetrahedralMesh::resize_inplace(fp_t scale) {
    m_vertices *= scale;
    clear_cache();
}


#define CGAL_CHECK_EXPENSIVE

#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/boost/graph/Seam_mesh.h>

#include <CGAL/Surface_mesh_parameterization/Error_code.h>
#include <CGAL/surface_mesh_parameterization.h>

#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>

#include <boost/functional/hash.hpp>

#include <iostream>
#include <fstream>

namespace SMP = CGAL::Surface_mesh_parameterization;
namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::Simple_cartesian<double>            Kernel;
typedef Kernel::Point_2                           Point_2;
typedef Kernel::Point_3                           Point_3;

#define MVC_POLYHEDRON_MESH
#define ARAP_POLYHEDRON_MESH
#define BARY_SURF_MESH
#define ARAP_SURF_MESH
#define DCM_PM_SEAM_MESH
#define DAC_SM_SEAM_MESH
#define ORBIFOLD_SM_MESH
#define ITERATIVE_SURF_MESH

// POLYHEDRON_MESH
typedef CGAL::Polyhedron_3<Kernel>                                PMesh;

typedef boost::graph_traits<PMesh>::vertex_descriptor             PM_vertex_descriptor;
typedef boost::graph_traits<PMesh>::halfedge_descriptor           PM_halfedge_descriptor;

typedef CGAL::Unique_hash_map<PM_halfedge_descriptor, Point_2>    PM_UV_hmap;
typedef boost::associative_property_map<PM_UV_hmap>               PM_UV_pmap;

// SURF_MESH
typedef CGAL::Surface_mesh<Point_3>                               SMesh;

typedef boost::graph_traits<SMesh>::vertex_descriptor             SM_vertex_descriptor;
typedef boost::graph_traits<SMesh>::halfedge_descriptor           SM_halfedge_descriptor;

typedef SMesh::Property_map<SM_halfedge_descriptor, Point_2>      SM_UV_pmap;

// PM_SEAM_MESH
typedef boost::graph_traits<PMesh>::edge_descriptor               PM_edge_descriptor;

typedef CGAL::Unique_hash_map<PM_edge_descriptor, bool>           PM_seam_edge_hmap;
typedef boost::associative_property_map<PM_seam_edge_hmap>        PM_seam_edge_pmap;
typedef CGAL::Unique_hash_map<PM_vertex_descriptor, bool>         PM_seam_vertex_hmap;
typedef boost::associative_property_map<PM_seam_vertex_hmap>      PM_seam_vertex_pmap;

typedef CGAL::Seam_mesh<PMesh, PM_seam_edge_pmap, PM_seam_vertex_pmap>
                                                                  PM_Seam_mesh;

typedef boost::graph_traits<PM_Seam_mesh>::vertex_descriptor      PM_SE_vertex_descriptor;
typedef boost::graph_traits<PM_Seam_mesh>::halfedge_descriptor    PM_SE_halfedge_descriptor;

// SM_SEAM_MESH
typedef boost::graph_traits<SMesh>::edge_descriptor               SM_edge_descriptor;

typedef SMesh::Property_map<SM_edge_descriptor, bool>             SM_seam_edge_pmap;
typedef SMesh::Property_map<SM_vertex_descriptor, bool>           SM_seam_vertex_pmap;

typedef CGAL::Seam_mesh<SMesh, SM_seam_edge_pmap, SM_seam_vertex_pmap>
                                                                  SM_Seam_mesh;

typedef boost::graph_traits<SM_Seam_mesh>::vertex_descriptor      SM_SE_vertex_descriptor;
typedef boost::graph_traits<SM_Seam_mesh>::halfedge_descriptor    SM_SE_halfedge_descriptor;

int main(int, char**)
{
  std::cout.precision(17);
  CGAL::IO::set_pretty_mode(std::cout);

  // ***************************************************************************
  // Default case
  // ***************************************************************************

#ifdef MVC_POLYHEDRON_MESH
  {
    std::cout << " ----------- MVC POLYHEDRON -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/mushroom.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    PM_halfedge_descriptor hd = PMP::longest_border(pm).first;

    CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                          boost::hash<PM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                            boost::hash<PM_vertex_descriptor> > > uvpm(uvhm);

    // Go to default (MVC)
    SMP::Error_code status = SMP::parameterize(pm, hd, uvpm);

    if(status != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with MVC (POLY)!" << std::endl;
    }
  }
#endif // MVC_POLYHEDRON_MESH

  // ***************************************************************************
  // ARAP WITH POLYHEDRON_MESH
  // ***************************************************************************

#ifdef ARAP_POLYHEDRON_MESH
  {
    std::cout << " ----------- ARAP POLYHEDRON -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/three_peaks.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    PM_halfedge_descriptor hd = PMP::longest_border(pm).first;

    // UV map
    CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                          boost::hash<PM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                            boost::hash<PM_vertex_descriptor> > > uvpm(uvhm);

    // Indices map
    typedef CGAL::dynamic_vertex_property_t<int>                                 Vertex_int_tag;
    typedef typename boost::property_map<PMesh, Vertex_int_tag>::type            Vertex_int_map;
    Vertex_int_map vipm = get(Vertex_int_tag(), pm);
    CGAL::Surface_mesh_parameterization::internal::fill_index_map_of_cc(hd, pm, vipm);

    // Vertex parameterized map
    typedef CGAL::dynamic_vertex_property_t<bool>                                Vertex_bool_tag;
    typedef typename boost::property_map<PMesh, Vertex_bool_tag>::type           Vertex_bool_map;
    Vertex_bool_map vpm = get(Vertex_bool_tag(), pm);

    // Parameterizer
    SMP::ARAP_parameterizer_3<PMesh> parameterizer;
    SMP::Error_code status = parameterizer.parameterize(pm, hd, uvpm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(pm, parameterizer, hd, uvpm);
    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with ARAP (POLY)!" << std::endl;
    }
  }
#endif // ARAP_POLYHEDRON_MESH

  // ***************************************************************************
  // Barycentric mapping
  // ***************************************************************************

#ifdef BARY_SURF_MESH
  {
    std::cout << " ----------- BARY SURFACE MESH ----------- " << std::endl;

    std::ifstream in("data/oni.off");
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;
    assert(hd != SM_halfedge_descriptor());

    // UV map
    typedef SMesh::Property_map<SM_vertex_descriptor, Point_2>  UV_pmap;
    UV_pmap uvpm = sm.add_property_map<SM_vertex_descriptor, Point_2>("h:uv").first;

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Vertex parameterized map
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::Barycentric_mapping_parameterizer_3<SMesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(sm, hd, uvpm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(sm, parameterizer, hd, uvpm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Barycentric (SM)!" << std::endl;
    }
  }
#endif // BARY_SURF_MESH

  // ***************************************************************************
  // ARAP WITH SURF_MESH
  // ***************************************************************************

#ifdef ARAP_SURF_MESH
  {
    std::cout << " ----------- ARAP SURFACE MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/nefertiti.off"));
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    // halfedge on the longest border
    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;

    CGAL::Unique_hash_map<SM_vertex_descriptor, Point_2,
                          boost::hash<SM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<SM_vertex_descriptor,
                            Point_2,
                            boost::hash<SM_vertex_descriptor> > > uv_pm(uvhm);

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized bool pmap
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map< boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::ARAP_parameterizer_3<SMesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(sm, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(sm, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with ARAP (SM)!" << std::endl;
    }
  }
#endif // ARAP_SURF_MESH

#ifdef DCM_PM_SEAM_MESH
  {
    std::cout << " ----------- DCM POLYHEDRON SEAM MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/fandisk.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }
    const char* selection = "data/fandisk.dcm.selection.txt";

    PM_seam_edge_hmap seam_edge_hm(false);
    PM_seam_edge_pmap seam_edge_pm(seam_edge_hm);
    PM_seam_vertex_hmap seam_vertex_hm(false);
    PM_seam_vertex_pmap seam_vertex_pm(seam_vertex_hm);

    PM_Seam_mesh mesh(pm, seam_edge_pm, seam_vertex_pm);
    PM_halfedge_descriptor pmhd = mesh.add_seams(selection);
    if(pmhd == PM_halfedge_descriptor() ) {
      std::cerr << "Warning: No seams in input" << std::endl;
    }

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that the uv
    // is only stored for the canonical halfedges representing a vertex
    PM_UV_hmap uv_hm;
    PM_UV_pmap uv_pm(uv_hm);

    // a halfedge on the (possibly virtual) border
    PM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    // Indices
    typedef boost::unordered_map<PM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, mesh), mesh), mesh,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<PM_Seam_mesh, Indices>(mesh, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized
    boost::unordered_set<PM_SE_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<PM_SE_vertex_descriptor> > vpm(vs);

    SMP::Discrete_conformal_map_parameterizer_3<PM_Seam_mesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(mesh, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with DCM (SEAM POLY)!" << std::endl;
    }
  }
#endif // DCM_PM_SEAM_MESH

  // ***************************************************************************
  // DAC WITH SEAM_MESH (SM)
  // ***************************************************************************

#ifdef DAC_SM_SEAM_MESH
  {
    std::cout << " ----------- DAC SURFACE MESH SEAM MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/bear.off"));
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    const char* selection = "data/bear.dac.selection.txt";

    SM_seam_edge_pmap seam_edge_pm =
        sm.add_property_map<SM_edge_descriptor,bool>("e:on_seam", false).first;
    SM_seam_vertex_pmap seam_vertex_pm =
        sm.add_property_map<SM_vertex_descriptor,bool>("v:on_seam", false).first;

    SM_Seam_mesh mesh(sm, seam_edge_pm, seam_vertex_pm);
    SM_halfedge_descriptor smhd = mesh.add_seams(selection);
    if(smhd == SM_halfedge_descriptor() ) {
      std::cerr << "Warning: No seams in input" << std::endl;
    }

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that the uv
    // is only stored for the canonical halfedges representing a vertex
    SM_UV_pmap uv_pm = sm.add_property_map<SM_halfedge_descriptor,
                                           Point_2>("h:uv").first;

    // a halfedge on the (possibly virtual) border
    SM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    // Indices
    typedef boost::unordered_map<SM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, mesh), mesh), mesh,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SM_Seam_mesh, Indices>(mesh, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized
    boost::unordered_set<SM_SE_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_SE_vertex_descriptor> > vpm(vs);

    SMP::Discrete_authalic_parameterizer_3<SM_Seam_mesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(mesh, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with DAC (SEAM SM)!" << std::endl;
    }
  }
#endif // DAC_SM_SEAM_MESH

#ifdef ORBIFOLD_SM_MESH
  {
    std::cout << " ----------- ORBIFOLD SURFACE MESH -----------" << std::endl;

    SMesh sm; // underlying mesh of the seam mesh

    std::ifstream in(CGAL::data_file_path("meshes/fandisk.off"));
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    const char* cone_filename = "data/fandisk.orbifold.selection.txt";

    // Read the cones and find the corresponding vertex_descriptor in the underlying mesh 'sm'
    std::vector<SM_vertex_descriptor> cone_sm_vds;
    SMP::read_cones<SMesh>(sm, cone_filename, std::back_inserter(cone_sm_vds));

    // Two property maps to store the seam edges and vertices
    SM_seam_edge_pmap seam_edge_pm = sm.add_property_map<SM_edge_descriptor, bool>("e:on_seam", false).first;
    SM_seam_vertex_pmap seam_vertex_pm = sm.add_property_map<SM_vertex_descriptor, bool>("v:on_seam",false).first;

    // The seam mesh
    SM_Seam_mesh mesh(sm, seam_edge_pm, seam_vertex_pm);

    // Use the path provided between cones to create a seam mesh
    SM_halfedge_descriptor smhd = mesh.add_seams(cone_filename);
    if(smhd == SM_halfedge_descriptor() ) {
      std::list<SM_edge_descriptor> seam_edges;
      SMP::compute_shortest_paths_between_cones(sm, cone_sm_vds.begin(), cone_sm_vds.end(), seam_edges);

      // Add the seams to the seam mesh
      for(SM_edge_descriptor e : seam_edges) {
        mesh.add_seam(source(e, sm), target(e, sm));
      }
    }

    // Index map of the seam mesh (assuming a single connected component so far)
    typedef boost::unordered_map<SM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    boost::associative_property_map<Indices> vimap(indices);
    int counter = 0;
    for(SM_SE_vertex_descriptor vd : vertices(mesh)) {
      put(vimap, vd, counter++);
    }

    // Mark the cones in the seam mesh
    boost::unordered_map<SM_SE_vertex_descriptor, SMP::Cone_type> cmap;
    SMP::locate_cones(mesh, cone_sm_vds.begin(), cone_sm_vds.end(), cmap);

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that uv values
    // are only stored for the canonical halfedges representing a vertex
    SM_UV_pmap uvmap = sm.add_property_map<SM_halfedge_descriptor, Point_2>("h:uv").first;

    // Parameterizer
    typedef SMP::Orbifold_Tutte_parameterizer_3<SM_Seam_mesh>         Parameterizer;
    Parameterizer parameterizer(SMP::Parallelogram, SMP::Cotangent);

    // a halfedge on the (possibly virtual) border
    // only used in output (will also be used to handle multiple connected components in the future)
    SM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, cmap, uvmap, vimap);

    if(status != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Orbifold (SEAM SM)!" << std::endl;
    }
  }
#endif // ORBIFOLD_SM_MESH

  // ***************************************************************************
  // ITERATIVE AUTHALIC WITH SURFACE_MESH
  // ***************************************************************************

#ifdef ITERATIVE_SURF_MESH
  {
    std::cout << " ----------- ITERATIVE AUTHALIC SURFACE MESH ----------- " << std::endl;

    std::ifstream in("data/oni.off");
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;
    assert(hd != SM_halfedge_descriptor());

    // UV map
    typedef SMesh::Property_map<SM_vertex_descriptor, Point_2>  UV_pmap;
    UV_pmap uvpm = sm.add_property_map<SM_vertex_descriptor, Point_2>("h:uv").first;

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Vertex parameterized map
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::Iterative_authalic_parameterizer_3<SMesh> parameterizer;

    double error = 0;
    unsigned int iterations = 15;
    SMP::Error_code status = parameterizer.parameterize(sm, hd, uvpm, vipm, vpm, iterations, error);
    SMP::Error_code status_bis = parameterizer.parameterize(sm, uvpm, 10);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Barycentric (SM)!" << std::endl;
    }
  }
#endif // DAC_SM_SEAM_MESH

  std::cout << "Done!" << std::endl;

  return EXIT_SUCCESS;
}


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gdav2.3.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class vincenty
    : public strategy::distance::geographic
        <
            strategy::vincenty, Spheroid, CalculationType
        >
{
    typedef strategy::distance::geographic
        <
            strategy::vincenty, Spheroid, CalculationType
        > base_type;

public:
    inline vincenty()
        : base_type()
    {}

    explicit inline vincenty(Spheroid const& spheroid)
        : base_type(spheroid)
    {}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<vincenty<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<Spheroid, CalculationType>, P1, P2>
    : vincenty<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<vincenty<Spheroid, CalculationType> >
{
    typedef vincenty<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<vincenty<Spheroid, CalculationType> >
{
    static inline vincenty<Spheroid, CalculationType> apply(vincenty<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<Spheroid, CalculationType>, P1, P2>::type
        apply(vincenty<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP