// Compile with:
// clang++ -o demoPriorDrawing3D demoPriorDrawing3D.cpp -L../build/ -I ../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
//

#include <ctime>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <unordered_set>
#include <Eigen/Core>
#include "File.h"
#include "EuclideanMetric.h"
#include "KmeansClusterer.h"
#include "Ellipsoid.h"
#include "UniformPrior.h"
#include "NormalPrior.h"
#include "SuperGaussianPrior.h"
#include "GridUniformPrior.h"
#include "PrincipalComponentProjector.h"

using namespace std;
using namespace Eigen;

int main()
{
    // ------ IDENTIFY CLUSTERS FROM INPUT SAMPLE ------
    // Open the input file and read the data (synthetic sampling of a 2D parameter space)

    ifstream inputFile;
    File::openInputFile(inputFile, "onecluster3D.txt");
    unsigned long Nrows;
    int Ncols;

    File::sniffFile(inputFile, Nrows, Ncols);
    ArrayXXd data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    ArrayXXd sample = data.transpose();
    inputFile.close();

    // Set up the K-means clusterer using a Euclidean metric

    EuclideanMetric myMetric;
    int minNclusters = 1;
    int maxNclusters = 1;
    int Ntrials = 10;
    double relTolerance = 0.01;

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = true;

    KmeansClusterer kmeans(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // Do the clustering, and get for each point the index of the cluster it belongs to

    int optimalNclusters;
    vector<int> clusterIndices(Nrows);
    vector<int> clusterSizes;

    optimalNclusters = kmeans.cluster(sample, clusterIndices, clusterSizes);
    int Nclusters = optimalNclusters;

    // Output the results

    cerr << "Input number of clusters: 1" << endl;
    cerr << "Optimal number of clusters: " << optimalNclusters << endl;

    // ------ Compute Ellipsoids ------

    int Ndimensions = Ncols;
    assert(sample.cols() == clusterIndices.size());
    assert(sample.cols() >= Ndimensions + 1);            // At least Ndimensions + 1 points are required.

    // The enlargement fraction (it is the fraction by which each axis of an ellipsoid is enlarged)

    double enlargementFraction = 3.00;

    // Compute "sorted indices" such that clusterIndices[sortedindices[k]] <= clusterIndices[sortedIndices[k+1]]

    vector<int> sortedIndices = Functions::argsort(clusterIndices);

    // beginIndex will take values such that the indices for one particular cluster (# n) will be in
    // sortedIndex[beginIndex, ..., beginIndex + clusterSize[n] - 1]

    int beginIndex = 0;

    // Clear whatever was in the ellipsoids collection

    vector<Ellipsoid> ellipsoids;
    ellipsoids.clear();

    // Create an Ellipsoid for each cluster (provided it's large enough)

    for (int i = 0; i < Nclusters; i++)
    {
        // Skip cluster if number of points is not large enough

        if (clusterSizes[i] < Ndimensions + 1)
        {
            // Move the beginIndex up to the next cluster

            beginIndex += clusterSizes[i];

            // Continue with the next cluster

            continue;
        }
        else
        {
            // The cluster is indeed large enough to compute an Ellipsoid.

            // Copy those points that belong to the current cluster in a separate Array
            // This is because Ellipsoid needs a contiguous array of points.

            ArrayXXd sampleOfOneCluster(Ndimensions, clusterSizes[i]);

            for (int n = 0; n < clusterSizes[i]; ++n)
            {
                sampleOfOneCluster.col(n) = sample.col(sortedIndices[beginIndex+n]);
            }

            // Move the beginIndex up to the next cluster

            beginIndex += clusterSizes[i];

            // Add ellipsoid at the end of our vector

            ellipsoids.push_back(Ellipsoid(sampleOfOneCluster, enlargementFraction));
        }
    }

    int Nellipsoids = ellipsoids.size();
    cerr << "Nellispids: " << Nellipsoids << endl;

    // Find which ellipsoids are overlapping and which are not

    vector<unordered_set<int>> overlappingEllipsoidsIndices;

    // Remove whatever was in the container before

    overlappingEllipsoidsIndices.clear();

    // Make sure that the indices container has the right size

    overlappingEllipsoidsIndices.resize(ellipsoids.size());

    // If Ellipsoid i overlaps with ellipsoid j, than of course ellipsoid j also overlaps with i.
    // The indices are kept in an unordered_set<> which automatically takes care
    // that there are no duplicates.

    bool ellipsoidMatrixDecompositionIsSuccessful;

    for (int i = 0; i < Nellipsoids-1; ++i)
    {
        for (int j = i+1; j < Nellipsoids; ++j)
        {
            if (ellipsoids[i].overlapsWith(ellipsoids[j], ellipsoidMatrixDecompositionIsSuccessful))
            {
                overlappingEllipsoidsIndices[i].insert(j);
                overlappingEllipsoidsIndices[j].insert(i);
            }
        }
    }

    mt19937 engine;
    clock_t clockticks = clock();
    engine.seed(clockticks);
    uniform_real_distribution<> uniform(0.0, 1.0);

    // Get the hyper-volume for each of the ellipsoids and normalize it
    // to the sum of the hyper-volumes over all the ellipsoids

    vector<double> normalizedHyperVolumes(Nellipsoids);

    for (int n=0; n < Nellipsoids; ++n)
    {
        normalizedHyperVolumes[n] = ellipsoids[n].getHyperVolume();
    }

    double sumOfHyperVolumes = accumulate(normalizedHyperVolumes.begin(), normalizedHyperVolumes.end(), 0.0, plus<double>());

    cerr << "Normalized Hyper-Volumes" << endl;
    ArrayXd centerCoordinate(2);
    ArrayXXd covarianceMatrix(2,2);

    for (int n = 0; n < Nellipsoids; ++n)
    {
        normalizedHyperVolumes[n] /= sumOfHyperVolumes;
        centerCoordinate = ellipsoids[n].getCenterCoordinates();
        covarianceMatrix = ellipsoids[n].getCovarianceMatrix();
        cerr << "Ellipsoid #" << n << "   " << normalizedHyperVolumes[n] << endl;
        cerr << "Center Coordinates: " << centerCoordinate.transpose() << endl;
        cerr << "Covariance Matrix: " << endl;
        cerr << covarianceMatrix << endl;

        MatrixXd T1 = MatrixXd::Identity(Ndimensions+1,Ndimensions+1);
        T1.bottomLeftCorner(1,Ndimensions) = (-1.0) * centerCoordinate.transpose();
        MatrixXd A = MatrixXd::Zero(Ndimensions+1,Ndimensions+1);
        A(Ndimensions,Ndimensions) = -1;
        A.topLeftCorner(Ndimensions,Ndimensions) = covarianceMatrix.matrix().inverse();
        MatrixXd AT = T1*A*T1.transpose();        // Translating to ellipsoid center

        //cerr << "Ellipsoidal Matrix: " << endl;
        //cerr << AT << endl;
        //cerr << endl;
    }

    // Pick an ellipsoid with a probability according to its normalized hyper-volume
    // First generate a uniform random number between 0 and 1

    double uniformNumber = uniform(engine);

    // Select the ellipsoid that makes the cumulative hyper-volume greater than this random
    // number. Those ellipsoids with a larger hyper-volume will have a greater probability to
    // be chosen.

    double cumulativeHyperVolume = normalizedHyperVolumes[0];
    int indexOfSelectedEllipsoid = 0;

    while (cumulativeHyperVolume < uniformNumber)
    {
        indexOfSelectedEllipsoid++;
        cumulativeHyperVolume += normalizedHyperVolumes[indexOfSelectedEllipsoid];
    }

    cerr << "Selected ellipsoid #: " << indexOfSelectedEllipsoid << endl;
    cerr << endl;

    // ------ Set up prior distributions on each coordinate ------

    int Npoints = 10000;
    ArrayXXd sampleOfDrawnPoints(Npoints,Ndimensions);
    ArrayXd drawnPoint(Ndimensions);

    /*      MIX PRIOR       UNIFORM-GRID UNIFORM-UNIFORM
    vector<Prior*> ptrPriors(3);
    ArrayXd parametersMinima(1);
    ArrayXd parametersMaxima(1);
    parametersMinima <<  0.0;
    parametersMaxima << 4.0;
    UniformPrior uniformPrior1(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior1;

    ArrayXd parametersStartingCoordinate(1);
    ArrayXd parametersNgridPoints(1);
    ArrayXd parametersSeparation(1);
    ArrayXd parametersTolerance(1);
    parametersStartingCoordinate << 0.0;
    parametersNgridPoints << 6;
    parametersSeparation << 0.5;
    parametersTolerance << 0.1;
    GridUniformPrior gridUniformPrior(parametersStartingCoordinate, parametersNgridPoints, parametersSeparation, parametersTolerance);
    ptrPriors[0] = &gridUniformPrior;

    parametersMinima <<  0.0;
    parametersMaxima << 4.0;
    UniformPrior uniformPrior2(parametersMinima, parametersMaxima);
    ptrPriors[2] = &uniformPrior2;
    */

    /*      MIX PRIOR       NORMAL-UNIFORM-NORMAL
    vector<Prior*> ptrPriors(3);
    ArrayXd parametersMean(1);
    ArrayXd parametersSDV(1);
    parametersMean <<  2.0;
    parametersSDV << 0.4;
    NormalPrior normalPrior1(parametersMean, parametersSDV);
    ptrPriors[0] = &normalPrior1;

    ArrayXd parametersMinima(1);
    ArrayXd parametersMaxima(1);
    parametersMinima <<  0.0;
    parametersMaxima << 4.0;
    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[1] = &uniformPrior;

    parametersMean <<  2.0;
    parametersSDV << 0.4;
    NormalPrior normalPrior2(parametersMean, parametersSDV);
    ptrPriors[2] = &normalPrior2;
    */

    /*      UNIFORM PRIOR       */
    vector<Prior*> ptrPriors(1);
    ArrayXd parametersMinima(Ndimensions);
    ArrayXd parametersMaxima(Ndimensions);
    parametersMinima <<  0.0, 0.0, 0.0;
    parametersMaxima << 4.0, 4.0, 4.0;
    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    /*      GAUSSIAN PRIOR
    vector<Prior*> ptrPriors(1);
    ArrayXd parametersMean(Ndimensions);
    ArrayXd parametersSDV(Ndimensions);
    parametersMean <<  2.0, 2.0, 2.0;
    parametersSDV << 0.2, 0.4, 0.2;
    NormalPrior normalPrior(parametersMean, parametersSDV);
    ptrPriors[0] = &normalPrior;
    */

    /*      SUPER GAUSSIAN PRIOR
    vector<Prior*> ptrPriors(1);
    ArrayXd parametersMean(Ndimensions);
    ArrayXd parametersSDV(Ndimensions);
    ArrayXd parametersWOP(Ndimensions);
    parametersMean <<  2.0, 2.0, 2.0;
    parametersSDV << 0.1, 0.2, 0.3;
    parametersWOP << 0.4, 0.4, 0.4;
    SuperGaussianPrior superGaussianPrior(parametersMean, parametersSDV, parametersWOP);
    ptrPriors[0] = &superGaussianPrior;
    */

    // ------ Draw points from the Ellipsoid ------

    for (int i=0; i < Npoints; ++i)
    {
        bool newPointIsFound = false;

        while (newPointIsFound == false)
        {
            // Draw a new point inside the ellipsoid

            ellipsoids[indexOfSelectedEllipsoid].drawPoint(drawnPoint);

            // Check if the new point is also in other ellipsoids. If the point happens to be
            // in N overlapping ellipsoids, then accept it only with a probability 1/N. If we
            // wouldn't do this, the overlapping regions in the ellipsoids would be oversampled.

            if (!overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].empty())
            {
                // There are overlaps, so count the number of ellipsoids to which the new
                // point belongs

                int NenclosingEllipsoids = 1;

                for (auto index = overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].begin();
                          index != overlappingEllipsoidsIndices[indexOfSelectedEllipsoid].end();
                        ++index)
                {
                    if (ellipsoids[*index].containsPoint(drawnPoint))
                    {
                        //NenclosingEllipsoids = static_cast<int>(DBL_MAX);       // No drawing from overlapping regions!
                        //NenclosingEllipsoids++;
                    }
                }

                // Only accept the new point with a probability = 1/NenclosingEllipsoids.
                // If it's not accepted, go immediately back to the beginning of the while loop,
                // and draw a new point inside the ellipsoid.

                uniformNumber = uniform(engine);
                newPointIsFound = (uniformNumber < 1./NenclosingEllipsoids);
            }
            else
            {
                // There are no ellipsoids overlapping with the selected one, so the point
                // is automatically accepted

                newPointIsFound = true;
            }

            // The point should not only be drawn inside the ellipsoid, it should also be drawn
            // from the prior. Therefore, accept the point only with the probability given by the
            // prior, so that the regions inside the ellipsoid with a higher prior density will
            // be sampled more than the regions with a lower prior density.

            // Since different coordinates of our new point may have different priors,
            // we need to check this for all the priors.

            int beginIndex = 0;

            for (int priorIndex = 0; priorIndex < ptrPriors.size(); ++priorIndex)
            {
                // Figure out the number of parameters (=coordinates) that the current prior covers.

                const int NdimensionsOfPrior = ptrPriors[priorIndex]->getNdimensions();

                // Define a subset of the new point, consisting of those coordinates covered by the
                // same (current) prior distribution.

                ArrayXd subsetOfNewPoint = drawnPoint.segment(beginIndex, NdimensionsOfPrior);

                // Check if the new point is accepted according to the corresponding prior distribution.

                newPointIsFound = ptrPriors[priorIndex]->drawnPointIsAccepted(subsetOfNewPoint);

                if (!newPointIsFound)
                break;

                // Move the beginIndex on to the next set of coordinates covered by the prior.

                beginIndex += NdimensionsOfPrior;
            }

        }

        sampleOfDrawnPoints.row(i) = drawnPoint.transpose();
    }

    ofstream outputFile;
    File::openOutputFile(outputFile,"priorDrawing3D.txt");
    File::arrayXXdToFile(outputFile, sampleOfDrawnPoints);

    return EXIT_SUCCESS;
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#define BOOST_TEST_MODULE sp_util_lookback
#include <boost/test/unit_test.hpp>
#include <string>

#include <iostream>

#include "sp/util/lookback.hpp"

using namespace sp::util;

BOOST_AUTO_TEST_CASE( test_sp_util_lookback_simple) {

    fixed_lookback<float> hist(20);

    for(int i = 1; i < 20; ++i) {
        BOOST_REQUIRE_EQUAL(hist.mean(i), 0.0f);
    }

    hist.tick(5);
    hist.tick(5);
    hist.tick(5);
    hist.tick(5);
    hist.tick(5);

    BOOST_REQUIRE_EQUAL(hist.min(5), 5.0f);
    BOOST_REQUIRE_EQUAL(hist.max(5), 5.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(5), 0);
    BOOST_REQUIRE_EQUAL(hist.mean(5), 5.0f);

    BOOST_REQUIRE_EQUAL(hist.min(10), 0.0f);
    BOOST_REQUIRE_EQUAL(hist.max(10), 5.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(10), 5.0f / 9.0f);
    BOOST_REQUIRE_EQUAL(hist.mean(10), 2.5f);

    hist.tick(6);
    hist.tick(7);
    hist.tick(8);
    hist.tick(9);
    hist.tick(10);

    BOOST_REQUIRE_EQUAL(hist.min(10), 5.0f);
    BOOST_REQUIRE_EQUAL(hist.max(10), 10.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(10), 5.0f / 9.0f);
    BOOST_REQUIRE_EQUAL(hist.mean(10), 6.5f);

    BOOST_REQUIRE_EQUAL(hist.min(20), 0.0f);
    BOOST_REQUIRE_EQUAL(hist.max(20), 10.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(20), 10.0f / 19.0f);
    BOOST_REQUIRE_EQUAL(hist.mean(20), 3.25f);

    hist.tick(11);
    hist.tick(12);
    hist.tick(13);
    hist.tick(14);
    hist.tick(15);
    hist.tick(16);
    hist.tick(17);
    hist.tick(18);
    hist.tick(19);
    hist.tick(20);

    BOOST_REQUIRE_EQUAL(hist.min(5), 16.0f);
    BOOST_REQUIRE_EQUAL(hist.max(5), 20.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(5), 1.0f);
    BOOST_REQUIRE_EQUAL(hist.mean(10), 15.5f);

    BOOST_REQUIRE_EQUAL(hist.min(20), 5.0f);
    BOOST_REQUIRE_EQUAL(hist.max(20), 20.0f);
    BOOST_REQUIRE_EQUAL(hist.deriv(20), 0.789473712f);
    BOOST_REQUIRE_EQUAL(hist.mean(20), 11.0f);
}

BOOST_AUTO_TEST_CASE( test_sp_util_lookback_large) {

    fixed_lookback<float> hist(100);

    for(int i = 0; i < 100000; ++i) {
        hist.tick(i);
    }

    BOOST_REQUIRE_EQUAL(hist.mean(99), 99950);
}
BOOST_AUTO_TEST_CASE( test_sp_util_lookback_running ) {

    rolling_lookback<float> hist(0.1);

    for(int i = 1; i <= 10; ++i) {
        hist.tick(i);
    }

    BOOST_REQUIRE_EQUAL(hist.mean(), 4.13810539f);

}


/**
 * @file sophus_operators.hpp
 * @brief File with operators copied from sophus for SO(3).
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_KINEMATICS_SOPHUS_OPERATORS_HPP_
#define INCLUDE_OKVIS_KINEMATICS_SOPHUS_OPERATORS_HPP_

#include <stdint.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <okvis/kinematics/Transformation.hpp>
#include <glog/logging.h>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief kinematics Namespace for kinematics functionality, i.e. transformations and stuff.
namespace kinematics {

template <typename Scalar>
struct SophusConstants {
  EIGEN_ALWAYS_INLINE static Scalar epsilon() {
    return static_cast<Scalar>(1e-10);
  }

  EIGEN_ALWAYS_INLINE static Scalar pi() { return static_cast<Scalar>(M_PI); }
};

// from sophus/so3.hpp
template <typename Scalar>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static Eigen::Matrix<Scalar, 3, 1> vee(
    const Eigen::Matrix<Scalar, 3, 3>& Omega) {
  return static_cast<Scalar>(0.5) *
         Eigen::Matrix<Scalar, 3, 1>(Omega(2, 1) - Omega(1, 2),
                                     Omega(0, 2) - Omega(2, 0),
                                     Omega(1, 0) - Omega(0, 1));
}

/// Warn: Do not use sinc or its templated version for autodiff involving quaternions
///  as its real part may be calculated without considering the infinisimal input.
/// Use expAndTheta borrowed from Sophus instead for this purpose
template <typename Scalar>
Eigen::Quaternion<Scalar> expAndTheta(const Eigen::Matrix<Scalar, 3, 1> & omega) {
    Scalar theta_sq = omega.squaredNorm();
    Scalar theta = sqrt(theta_sq);
    Scalar half_theta = static_cast<Scalar>(0.5)*(theta);

    Scalar imag_factor;
    Scalar real_factor;
    if(theta<SophusConstants<Scalar>::epsilon()) {
      Scalar theta_po4 = theta_sq*theta_sq;
      imag_factor = static_cast<Scalar>(0.5)
                    - static_cast<Scalar>(1.0/48.0)*theta_sq
                    + static_cast<Scalar>(1.0/3840.0)*theta_po4;
      real_factor = static_cast<Scalar>(1)
                    - static_cast<Scalar>(0.5)*theta_sq +
                    static_cast<Scalar>(1.0/384.0)*theta_po4;
    } else {
      Scalar sin_half_theta = sin(half_theta);
      imag_factor = sin_half_theta/theta;
      real_factor = cos(half_theta);
    }

    return Eigen::Quaternion<Scalar>(real_factor,
                                               imag_factor*omega.x(),
                                               imag_factor*omega.y(),
                                               imag_factor*omega.z());
}

// From sophus so3.hpp
template <typename Scalar>
Eigen::Matrix<Scalar, 3, 1> logAndTheta(const Eigen::Quaternion<Scalar> & other,
                          Scalar * theta) {
  Scalar squared_n
      = other.vec().squaredNorm();
  Scalar n = sqrt(squared_n);
  Scalar w = other.w();

  Scalar two_atan_nbyw_by_n;

  // Atan-based log thanks to
  //
  // C. Hertzberg et al.:
  // "Integrating Generic Sensor Fusion Algorithms with Sound State
  // Representation through Encapsulation of Manifolds"
  // Information Fusion, 2011

  if (n < SophusConstants<Scalar>::epsilon()) {
    // If quaternion is normalized and n=0, then w should be 1;
    // w=0 should never happen here!
    CHECK_GT(abs(w), SophusConstants<Scalar>::epsilon()) <<
                  "Quaternion should be normalized!";
    Scalar squared_w = w*w;
    two_atan_nbyw_by_n = static_cast<Scalar>(2) / w
                         - static_cast<Scalar>(2)*(squared_n)/(w*squared_w);
  } else {
    if (abs(w)<SophusConstants<Scalar>::epsilon()) {
      if (w > static_cast<Scalar>(0)) {
        two_atan_nbyw_by_n = M_PI/n;
      } else {
        two_atan_nbyw_by_n = -M_PI/n;
      }
    }else{
      two_atan_nbyw_by_n = static_cast<Scalar>(2) * atan(n/w) / n;
    }
  }

  *theta = two_atan_nbyw_by_n*n;

  return two_atan_nbyw_by_n * other.vec();
}
/**
 * @brief ominus The inverse of Tbar = T.oplus(delta).
 * @param Tbar
 * @param T
 * @return delta
 */
inline Eigen::Matrix<double, 6, 1> ominus(
    const okvis::kinematics::Transformation& Tbar,
    const okvis::kinematics::Transformation& T) {
  Eigen::Matrix<double, 3, 3> dR = Tbar.C() * T.C().transpose();
  Eigen::Matrix<double, 6, 1> delta;
  delta.head<3>() = Tbar.r() - T.r();
  delta.tail<3>() = vee(dR);
  return delta;
}

inline bool motionLessThan(const okvis::kinematics::Transformation& Tab,
                           double distanceThreshold, double angleThreshold) {
  return Tab.r().norm() < distanceThreshold &&
      std::fabs(Eigen::AngleAxisd(Tab.q()).angle()) < angleThreshold;
}

} // namespace kinematics
} // namespace okvis

#endif /* INCLUDE_OKVIS_KINEMATICS_SOPHUS_OPERATORS_HPP_ */


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_2PI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_2PI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the compute the remainder modulo \f$2\pi\f$.

    @par Header <boost/simd/function/rem_2pi.hpp>

    @par Note

     - The result is in \f$[-\pi, \pi]\f$.

     - If the input is near \f$\pi\f$ the output can be \f$\pi\f$ or \f$-\pi\f$
      depending  on register disponibility if extended arithmetic is used.

    @par Example:

      @snippet rem_2pi.cpp rem_2pi

    @par Possible output:

      @snippet rem_2pi.txt rem_2pi

  **/
  IEEEValue rem_2pi(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/rem_2pi.hpp>
#include <boost/simd/function/simd/rem_2pi.hpp>

#endif


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
// Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program

// 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)

#include "test_formula.hpp"
#include "direct_cases.hpp"
#include "direct_cases_antipodal.hpp"

#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/karney_direct.hpp>
//#include <boost/geometry/formulas/series_expansion_direct.hpp>
#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/srs/srs.hpp>

template <typename Result>
void check_direct(Result const& result, expected_result const& expected, expected_result const& reference,
                  double reference_error, bool check_reference_only = false)
{
    check_direct_sph(result, expected, reference, reference_error, check_reference_only);
    check_one(result.reduced_length, expected.reduced_length, reference.reduced_length, reference_error);
    check_one(result.geodesic_scale, expected.geodesic_scale, reference.geodesic_scale, reference_error);
}

template <typename Result>
void check_direct_sph(Result const& result, expected_result const& expected, expected_result const& reference,
                      double reference_error, bool check_reference_only = false)
{
    check_one(result.lon2, expected.lon2, reference.lon2, reference_error, true, check_reference_only);
    check_one(result.lat2, expected.lat2, reference.lat2, reference_error, true, check_reference_only);
    check_one(result.reverse_azimuth, expected.reverse_azimuth, reference.reverse_azimuth, reference_error, true, check_reference_only);
}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lon1r = results.p1.lon * d2r;
    double lat1r = results.p1.lat * d2r;
    double distance = results.distance;
    double azi12r = results.azimuth12 * d2r;

    double lon1d = results.p1.lon;
    double lat1d = results.p1.lat;
    double azi12d = results.azimuth12;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::srs::sphere<double> const sphere;

    bg::formula::result_direct<double> result;

    typedef bg::formula::vincenty_direct<double, true, true, true, true> vi_t;
    result = vi_t::apply(lon1r, lat1r, distance, azi12r, spheroid);
    result.lon2 *= r2d;
    result.lat2 *= r2d;
    result.reverse_azimuth *= r2d;
    check_direct(result, results.vincenty, results.karney, 0.00000001);

    typedef bg::formula::thomas_direct<double, true, true, true, true, true> th_t;
    result = th_t::apply(lon1r, lat1r, distance, azi12r, spheroid);
    result.lon2 *= r2d;
    result.lat2 *= r2d;
    result.reverse_azimuth *= r2d;
    check_direct(result, results.thomas, results.karney, 0.0000001);

    typedef bg::formula::thomas_direct<double, false, true, true, true, true> th_t1st;
    result = th_t1st::apply(lon1r, lat1r, distance, azi12r, spheroid);
    result.lon2 *= r2d;
    result.lat2 *= r2d;
    result.reverse_azimuth *= r2d;
    check_direct(result, results.thomas1st, results.karney, 0.0000001);
/*
    typedef bg::formula::series_expansion_direct<double, true, true, true, true, 4> series;
    result = series::apply(lon1r, lat1r, distance, azi12r, spheroid);
    result.lon2 *= r2d;
    result.lat2 *= r2d;
    result.reverse_azimuth *= r2d;
    check_direct(result, results.series, results.karney, 0.0000001);
*/
    result = bg::formula::spherical_direct<true, true>(lon1r, lat1r, distance,
                                                       azi12r, sphere);
    result.lon2 *= r2d;
    result.lat2 *= r2d;
    result.reverse_azimuth *= r2d;
    check_direct_sph(result, results.spherical, results.karney, 0.1);

    typedef bg::formula::karney_direct<double, true, true, true, true, 2> ka_t;
    result = ka_t::apply(lon1d, lat1d, distance, azi12d, spheroid);
    check_direct(result, results.thomas, results.karney, 0.0000001);
}

void test_karney_antipodal(expected_results_antipodal const& results)
{
    double lon1d = results.p1.lon;
    double lat1d = results.p1.lat;
    double distance = results.distance;
    double azi12d = results.azimuth12;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_direct<double> result;

    typedef bg::formula::karney_direct<double, true, true, true, true, 8> ka_t;
    result = ka_t::apply(lon1d, lat1d, distance, azi12d, spheroid);
    check_direct(result, results.karney, results.karney, 0.0000001, true);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    for (size_t i = 0; i < expected_size_antipodal; ++i)
    {
        test_karney_antipodal(expected_antipodal[i]);
    }

    return 0;
}


#include <algorithm>
#include <cassert>
#include <limits>
#include <memory>
#include <ostream>
#include <random>
#include <stdexcept>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "bsgs.hpp"
#include "orbit.hpp"
#include "perm.hpp"
#include "perm_group.hpp"
#include "perm_set.hpp"
#include "util.hpp"

namespace mpsym
{

namespace internal
{

PermGroup::PermGroup(unsigned degree, PermSet const &generators)
{
  _bsgs = BSGS(degree, generators);
  _order = _bsgs.order();
}

bool PermGroup::operator==(PermGroup const &rhs) const
{
  assert(rhs.degree() == degree()
         && "comparing permutation groups of equal degree");

  if (_order != rhs.order())
    return false;

  for (Perm const &gen : rhs.generators()) {
    if (!contains_element(gen))
      return false;
  }

  return true;
}

bool PermGroup::operator!=(PermGroup const &rhs) const
{
  return !(*this == rhs);
}

PermGroup PermGroup::symmetric(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u);

  if (degree == 1u)
    return PermGroup(1u, {Perm(1u)});

  std::vector<unsigned> gen;
  for (unsigned i = 0u; i < degree; ++i)
    gen.push_back(i);

  return PermGroup(degree, {Perm(degree, {{0, 1}}), Perm(degree, {gen})});
}

PermGroup PermGroup::cyclic(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u);

  std::vector<unsigned> gen;
  for (unsigned i = 0u; i < degree; ++i)
    gen.push_back(i);

  return PermGroup(degree, {Perm(degree, {gen})});
}

PermGroup PermGroup::dihedral(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u && degree % 2 == 0);

  if (degree == 2u)
    return PermGroup(2, {Perm({1, 0})});

  if (degree == 4u)
    return PermGroup(4, {Perm({1, 0, 2, 3}), Perm({0, 1, 3, 2})});

  std::vector<unsigned> rotation(degree / 2u);

  // rotation
  for (unsigned i = 0u; i < degree / 2u - 1u; ++i)
    rotation[i] = i + 1u;

  rotation[degree / 2u - 1u] = 0u;

  // reflection
  std::vector<unsigned> reflection(degree / 2u);

  reflection[0] = 0u;

  for (unsigned i = 1u; i < (degree / 2u + 1u) / 2u; ++i) {
    reflection[i] = degree / 2u - i;
    reflection[degree / 2u - i] = i;
  }

  if ((degree / 2u) % 2 == 0)
    reflection[degree / 4u] = degree / 4u;

  return PermGroup(degree / 2u, {Perm(rotation), Perm(reflection)});
}

PermSet PermGroup::wreath_product_generators(PermGroup const &lhs,
                                             PermGroup const &rhs)
{
  unsigned wp_degree = lhs.degree() * rhs.degree();

  auto lhs_gens(lhs.generators());
  auto rhs_gens(rhs.generators());

  PermSet wp_generators;

  if (lhs.is_trivial() && rhs.is_trivial()) {
    return {};

  } else if (rhs.is_trivial()) {
    wp_generators.resize(lhs_gens.size(), Perm(wp_degree));

    for (unsigned i = 0u; i < rhs.degree(); ++i) {
      for (auto j = 0u; j < lhs_gens.size(); ++j)
        wp_generators[j] *= lhs_gens[j].shifted(lhs.degree() * i).extended(wp_degree);
    }

  } else {
    for (unsigned i = 0u; i < rhs.degree(); ++i) {
      for (Perm const &perm : lhs_gens)
        wp_generators.insert(perm.shifted(lhs.degree() * i).extended(wp_degree));
    }

    for (Perm const &gen : rhs_gens) {
      std::vector<std::vector<unsigned>> cycles {gen.cycles()};
      for (auto &cycle : cycles) {
        for (unsigned &x : cycle)
          x = x * lhs.degree();
      }

      std::vector<std::vector<unsigned>> shifted_cycles {cycles};

      for (unsigned i = 1u; i < lhs.degree(); ++i) {
        for (auto const &cycle : cycles) {
          std::vector<unsigned> shifted_cycle(cycle);

          for (unsigned &x : shifted_cycle)
            x += i;

          shifted_cycles.push_back(shifted_cycle);
        }
      }

      wp_generators.emplace(wp_degree, shifted_cycles);
    }
  }

  return wp_generators;
}

PermGroup PermGroup::wreath_product(PermGroup const &lhs,
                                    PermGroup const &rhs,
                                    BSGSOptions const *bsgs_options_,
                                    timeout::flag aborted)
{
  // degree of wreath product
  unsigned wp_degree = lhs.degree() * rhs.degree();

  // generators of wreath product
  auto wp_generators(wreath_product_generators(lhs, rhs));

  if (wp_generators.empty())
    return PermGroup(wp_degree);

  // order of wreath product
  auto wp_order(wreath_product_order(lhs, rhs));

  // construct wreath product
  auto bsgs_options(BSGSOptions::fill_defaults(bsgs_options_));
  bsgs_options.schreier_sims_random_known_order = wp_order;

  return PermGroup(BSGS(wp_degree, wp_generators, &bsgs_options, aborted));
}

BSGS::order_type PermGroup::wreath_product_order(PermGroup const &lhs,
                                                 PermGroup const &rhs)
{
  using boost::multiprecision::pow;

  auto lhs_order(lhs.order());
  auto rhs_order(rhs.order());

  if (lhs.is_trivial())
    return rhs_order;

  if (rhs.is_trivial())
    return lhs_order;

  return pow(lhs_order, rhs.degree()) * rhs_order;
}

bool PermGroup::is_symmetric() const
{
  if (_bsgs.is_symmetric() || degree() == 1u)
    return true;

  return _order == symmetric_order(degree());
}

bool PermGroup::is_shifted_symmetric() const
{
  unsigned degree_ = largest_moved_point() - smallest_moved_point() + 1u;

  return _order == symmetric_order(degree_);
}

bool PermGroup::is_transitive() const
{
  auto orbit(Orbit::generate(0u, generators().with_inverses()));

  return orbit.size() == degree();
}

bool PermGroup::contains_element(Perm const &perm) const
{
  assert(perm.degree() == degree() && "element has same degree as group");

  return _bsgs.strips_completely(perm);
}

Perm PermGroup::random_element() const
{
  static auto re(util::random_engine());

  Perm result(degree());
  for (unsigned i = 0u; i < _bsgs.base_size(); ++i) {
    auto orbit(_bsgs.orbit(i));

    std::uniform_int_distribution<> d(0u, orbit.size() - 1u);

    result *= _bsgs.transversal(i, *(orbit.begin() + d(re)));
  }

  return result;
}

PermGroup::const_iterator::const_iterator(PermGroup const &pg)
  : _trivial(pg.bsgs().base_empty()),
    _end(false)
{
  if (_trivial) {
    _current = Perm(pg.degree());

    _current_valid = true;

  } else {
    for (unsigned i = 0u; i < pg.bsgs().base_size(); ++i) {
      _state.push_back(0u);

      auto transv = pg.bsgs().transversals(i);

      _transversals.push_back(transv);
      _current_factors.insert(transv[0]);
    }

    _current_valid = false;
  }
}

bool PermGroup::const_iterator::operator==(PermGroup::const_iterator const &rhs) const
{
  if (_end != rhs._end)
    return false;

  if (_end && rhs._end)
    return true;

  for (unsigned i = 0u; i < _state.size(); ++i) {
    if (_state[i] != rhs._state[i])
      return false;
  }

  return true;
}

PermGroup::const_iterator::reference PermGroup::const_iterator::current()
{
  if (_current_valid)
    return _current;

  _current = _current_factors[0];
  for (unsigned j = 1u; j < _current_factors.size(); ++j)
    _current = _current_factors[j] * _current;

  _current_valid = true;

  return _current;
}

void PermGroup::const_iterator::next()
{
  if (_trivial) {
    _end = true;
    return;
  }

  for (unsigned i = 0u; i < _state.size(); ++i) {
    _state[i]++;
    if (_state[i] == _transversals[i].size())
      _state[i] = 0u;

    _current_factors[i] = _transversals[i][_state[i]];

    if (i == _state.size() - 1u && _state[i] == 0u) {
      _end = true;
      break;
    }

    if (_state[i] != 0u)
      break;
  }

  _current_valid = false;
}

std::ostream &operator<<(std::ostream &os, PermGroup const &pg)
{
  os << pg.bsgs() << "\n"
     << "ORDER: " << pg._order;

  return os;
}

} // namespace internal

} // namespace mpsym


/**
 * @file crossprod.cc
 * @brief NPDE homework CrossProd code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "crossprod.h"

#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <vector>

namespace CrossProd {

/* SAM_LISTING_BEGIN_0 */
void tab_crossprod() {
  // TO DO (13-1.e): solve the cross-product ODE with the implicit RK method
  // defined in solve_imp_mid. Tabulate the norms of the results at all steps.
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_0 */

  /* SAM_LISTING_BEGIN_1 */
  // TO DO (13-1.g): solve the cross-product ODE with the implicit RK method
  // defined in solve_lin_mid. Tabulate the norms of the results at all steps.
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_1 */
}

}  // namespace CrossProd


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef KRAW_APPROX_HPP
#define KRAW_APPROX_HPP

// Krawczyk method using approximate solution
// Newton iteration can be applied in advance.

#include <limits>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/autodif.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/make-candidate.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
bool
krawczyk_approx(F f, const ub::vector<T>& c, ub::vector< interval<T> >& result, int newton_max = 2, int verbose = 1)
{
	int s = c.size();

	ub::vector< interval<T> > I, fc, fi, Rfc, C, K;
	ub::matrix< interval<T> > fdc, fdi, M;
	ub::vector<T> c2, minus;
	ub::matrix<T> R;
	int i, j;
	bool r;
	ub::vector<T> newton_step;
	T tmp, tmp2;

	c2 = c;

	// Newton iteration
	// use interval<T> for argument of f
	// preparing for the case that f can not accept T.

	for (i=0; i<newton_max; i++) {
		C = c2;
		try {
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(C)), fc, fdc);
		}
		catch (std::domain_error& e) {
			return false;
		}
		r = invert(mid(fdc), R);
		if (!r) return false;

		minus = prod(R, mid(fc));

		tmp = 1.;
		tmp2 = 0.;
		for (j=0; j<s; j++) {
			using std::abs;
			tmp = std::max(tmp, abs(c2(j)));
			tmp2 = std::max(tmp2, abs(minus(j)));
		}

		c2 = c2 - minus;
		if (verbose >= 1) {
			std::cout << "newton" << i << ": " << c2 << "\n";
		}
		if (tmp2 <= tmp * std::numeric_limits<T>::epsilon()) break;
	}

	C = c2;
	try {
		autodif< interval<T> >::split(f(autodif< interval<T> >::init(C)), fc, fdc);
	}
	catch (std::domain_error& e) {
		return false;
	}
	r = invert(mid(fdc), R);
	if (!r) return false;
	Rfc = prod(R, fc);

	newton_step.resize(s);
	for (i=0; i<s; i++) {
		newton_step(i) = norm(Rfc(i));
	}

	make_candidate(newton_step);

	I = C;
	for (i=0; i<s; i++) {
		tmp = std::numeric_limits<T>::epsilon() * norm(I(i)) * (s+1) * 2;
		tmp2 = std::numeric_limits<T>::min() * (s+1) * 2;
		if (newton_step(i) < tmp) newton_step(i) = tmp;
		if (newton_step(i) < tmp2) newton_step(i) = tmp2;
		I(i) += newton_step(i) * interval<T>(-1., 1.);
	}

	if (verbose >= 1) {
		std::cout << "I: " << I << "\n";
	}

	try {
		autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
	}
	catch (std::domain_error& e) {
		return false;
	}

	// M = ub::identity_matrix< interval<T> >(s) - prod(R, fdi);
	M = ub::identity_matrix< interval<T> >(s);
	M -= prod(R, fdi);

	K = C - Rfc +  prod(M, I - C);

	if (verbose >= 1) {
		std::cout << "K: " << K << "\n";
	}

	if (proper_subset(K, I)) {
		result = K;
		return true;
	} else {
		return false;
	}
}

namespace krawczyk_approx_sub {

// generate 1-d vector function from scalar function
template <class F>
struct MakeVec {
	F f;
	MakeVec(F f): f(f) {}

	template <class T> ub::vector<T> operator()(const ub::vector<T>& x) {
		ub::vector<T> r(1);
		r(0) = f(x(0));
		return r;
	}
};

} // namespace krawczyk_approx_sub;

// 1 dimensional version

template <class T, class F>
bool
krawczyk_approx(F f, const T& c, interval<T>& result, int newton_max = 2, int verbose = 1)
{
	ub::vector<T> in(1);
	ub::vector< interval<T> > out;
	krawczyk_approx_sub::MakeVec<F> g(f);
	bool r;

	in(0) = c;
	r = krawczyk_approx(g, in, out, newton_max, verbose);
	if (!r) return r;
	result = out(0);
	return r;
}

} // namespace kv

#endif // KRAW_APPROX_HPP


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Polyhedron_3.h>

#include <CGAL/Surface_mesh_parameterization/IO/File_off.h>
#include <CGAL/Surface_mesh_parameterization/Square_border_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>

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

#include <boost/array.hpp>

#include <unordered_set>
#include <cstdlib>
#include <iostream>
#include <fstream>

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

typedef boost::graph_traits<PolyMesh>::halfedge_descriptor       halfedge_descriptor;
typedef boost::graph_traits<PolyMesh>::vertex_descriptor         vertex_descriptor;
typedef boost::graph_traits<PolyMesh>::face_descriptor           face_descriptor;

typedef boost::graph_traits<PolyMesh>::vertex_iterator           vertex_iterator;

typedef boost::array<vertex_descriptor, 4>                       Vd_array;

typedef CGAL::Unique_hash_map<vertex_descriptor, Point_2>        UV_uhm;
typedef boost::associative_property_map<UV_uhm>                  UV_pmap;

namespace SMP = CGAL::Surface_mesh_parameterization;

bool read_vertices(const PolyMesh& mesh,
                   const char* filename,
                   Vd_array& fixed_vertices)
{
  std::string str = filename;
  if( (str.length()) < 14 || (str.substr(str.length() - 14) != ".selection.txt") ) {
    std::cerr << "Error: vertices must be given by a *.selection.txt file" << std::endl;
    return false;
  }

  std::ifstream in(filename);
  std::string line;
  if(!std::getline(in, line)) {
    std::cerr << "Error: could not read input file: " << filename << std::endl;
    return false;
  }

  // The selection file is a list of integers, so we must build a correspondence
  // between vertices and the integers.
  std::vector<vertex_descriptor> vds;
  vds.reserve(num_vertices(mesh));
  vertex_iterator vi = vertices(mesh).begin(), vi_end = vertices(mesh).end();
  CGAL_For_all(vi, vi_end) {
    vds.push_back(*vi);
  }

  // Get the first line and read the fixed vertex indices
  std::size_t counter = 0;
  std::istringstream point_line(line);
  std::size_t s;
  std::unordered_set<std::size_t> indices;
  while(point_line >> s) {
    if(s >= vds.size())
    {
      std::cerr << "Error: Vertex index too large" << std::endl;
      return false;
    }

    vertex_descriptor vd = vds[s];
    if(!is_border(vd, mesh)) { // must be on the border
      std::cerr << "Error: vertex is not on the border of the mesh" << std::endl;
      return false;
    }

    if(counter >= 4) { // too many border vertices
      std::cerr << "Error: Too many vertices are fixed" << std::endl;
      return false;
    }

    fixed_vertices[counter++] = vd;
    indices.insert(s);
  }

  if(indices.size() < 4) {
    std::cerr << "Error: at least four unique vertices must be provided" << std::endl;
    return false;
  }

  return true;
}

int main(int argc, char** argv)
{
  std::ifstream in((argc>1) ? argv[1] : CGAL::data_file_path("meshes/nefertiti.off"));
  if(!in){
    std::cerr << "Error: problem loading the input data" << std::endl;
    return 1;
  }

  PolyMesh sm;
  in >> sm;

  halfedge_descriptor bhd = CGAL::Polygon_mesh_processing::longest_border(sm).first;

  // The 2D points of the uv parametrisation will be written into this map
  UV_uhm uv_uhm;
  UV_pmap uv_map(uv_uhm);

  const char* filename = (argc > 2) ? argv[2] : "data/square_corners.selection.txt";
  Vd_array vda;
  if(!read_vertices(sm, filename, vda)) {
    std::cerr << "Error: problem loading the square corners" << std::endl;
    return EXIT_FAILURE;
  }

  typedef SMP::Square_border_uniform_parameterizer_3<PolyMesh> Border_parameterizer;
  typedef SMP::Discrete_conformal_map_parameterizer_3<PolyMesh, Border_parameterizer> Parameterizer;

  // Border parameterizers (pick one)
  Border_parameterizer border_param(vda[0], vda[1], vda[2], vda[3]);
//  Border_parameterizer border_param; // the border parameterizer will compute the corner vertices

  SMP::Error_code err = SMP::parameterize(sm, Parameterizer(border_param), bhd, uv_map);

  if(err != SMP::OK) {
    std::cerr << "Error: " << SMP::get_error_message(err) << std::endl;
    return 1;
  }

  std::ofstream out("result.off");
  SMP::IO::output_uvmap_to_off(sm, bhd, uv_map, out);

  return EXIT_SUCCESS;
}


#include <iostream>

#pragma GCC diagnostic ignored "-Wparentheses"
#pragma GCC optimize ("rtti")

#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/io.hpp>

const auto meter = 1.0 * boost::units::si::meter;
const auto s     = 1.0 * boost::units::si::second;

void print( decltype( meter / s ) v ){
    std::cout << "velocity = " << v << "\n";
}

int main(){

   auto gravity = 10.0 * meter / ( s * s );
   auto duration = 2.0 * s;

   print( gravity * duration );

}


#ifndef _DUAL_CC_ISOSURFACE_H_
#define _DUAL_CC_ISOSURFACE_H_

#include <sisl/sisl.hpp>
#include <sisl/sparse_array.hpp>
#include <sisl/utility/ply_writer.hpp>

#include <Eigen/Dense>

#include <tuple>
#include <unordered_map>
#include <map>

namespace sisl{
namespace utility{
using namespace std;

template<class T>
class dualcc_isosurface{
public:
	struct cell_vertex
	{
		cell_vertex(){}
		std::vector<vector3<T> > touching;
		vector3<T> vertex;
		int vertexId;
	};

	dualcc_isosurface() : face_hash_table(1,1,1, {}){
		this->faceList.clear();
	}

	template<class L, class I, class O>
	void contour(
			L *f,
			const O &isoValue,
			const I &scalingParameter,
			sisl::vector3<I> origin,
			sisl::vector3<I> boundary ){

		I dh = scalingParameter;
		int res = int(1./(dh));
		this->faceList = sisl::sparse_array3<cell_vertex>(res+2, res+2, res+2, {});

		// Go over every lattice point
		#pragma omp parallel for
		for(int i = 2; i < res-2; i++) {
			/*
			 * We keep this local to each worker, so we only have to delve into
			 * a critical section at the end of each loop
			 */
			std::vector<std::vector<vector3<int>>> localFaceList;

			for(int j = 2; j < res-2; j++)
				for(int k = 2; k < res-2; k++){
					int ii = i;
					int jj = j;
					int kk = k;

					O value = f->f(dh*ii, dh*jj, dh*kk) - isoValue;

					// For each face in the minimal amount of faces of
					// the polyhedron
					for(auto idx : minimal_face_set) {
						auto polyhedron_vertex = polyhedron_vertices[idx];
						int x = polyhedron_vertex.i + ii,
							y = polyhedron_vertex.j + jj,
							z = polyhedron_vertex.k + kk;
						O next_value = f->f(dh*x, dh*y, dh*z) - isoValue;
						I zero_solution = 0.5;

						vector3<T> pv, n;

						// No sign change?
						if((next_value > 0 && value > 0) || (next_value <0 && value < 0))
							continue; // Whatever

						// Find the sign change.
						zero_solution = ((value - 0)/(value - next_value));
						pv = n = vector3<T>(x,y,z) - vector3<T>(ii,jj,kk);
						pv = vector3<T>(ii,jj,kk) + pv * zero_solution;
						n = n * (value > next_value ? -1 : 1);

						// Lookup all the dual points that touch this vertex
						std::vector<int> adj = adj_index[idx - 1];
						std::vector<std::vector<int>> luf = triangle_lookup[idx - 1];

						// Push all the faces into our local face list.
						for(auto triangle : luf) {
							vector3<int>
									hash1 = center_hash_offsets[triangle[0]] + vector3<int>(ii*2, jj*2, kk*2),
									hash2 = center_hash_offsets[triangle[1]] + vector3<int>(ii*2, jj*2, kk*2),
									hash3 = center_hash_offsets[triangle[2]] + vector3<int>(ii*2, jj*2, kk*2);

							vector3<int> t = (hash2 - hash1)%(hash3 - hash1);
							vector3<T> dir(t.i, t.j, t.k);
							if(dir * n > 0) localFaceList.push_back((std::vector<vector3<int>>){hash1, hash2, hash3});
							else localFaceList.push_back((std::vector<vector3<int>>){hash3, hash2, hash1});
						}

						// Mark the hashed dual vertex as having seen this primal vertex
						for(auto jdx : adj) {
							vector3<int> hash = center_hash_offsets[jdx] + vector3<int>(ii*2, jj*2, kk*2);

							#pragma omp critical (hash_bash_bcc)
							{
								face_hash_table(hash.i, hash.j, hash.k).touching.push_back({pv.i, pv.j, pv.k});
							}
						}

					}
				}
				// Merge the faces back in to global face list
				#pragma omp critical (lizst_cyst)
				{
					faceList.reserve(faceList.size() + localFaceList.size());
					faceList.insert(faceList.end(), localFaceList.begin(), localFaceList.end());
				}
		}
		processVertices<L,I,O>(f, dh);
		processFaces();
	}

	bool writeSurface(const std::string &out) const {
		return output_mesh.writePly(out);
	}

private:
	template<class L, class I, class O>
	void processVertices(L *f, const I &dh){
		// Calculate the vertex for each cell
		for (auto it = face_hash_table.siteMap.begin(); it != face_hash_table.siteMap.end(); ++it) {
			auto hash = it->first;
			auto vcache = it->second;

			std::vector<vector3<T>> normals;
			for(auto v : vcache.touching)
				normals.push_back(f->grad_f(v*dh).normalize());

			auto pavg = optimize_for_feature(vcache.touching, normals) * dh;
			auto normal = f->grad_f(pavg).normalize();

			face_hash_table.siteMap[hash].vertexId = output_mesh.addVertex({pavg, normal});
		}
	}

	void processFaces(){
		/* Build the final face list */
		for(auto face : faceList) {
			std::vector<int> index_face;
			for(auto hash : face) {
				index_face.push_back(face_hash_table(hash.i, hash.j, hash.k).vertexId);
			}
			output_mesh.addPolygon(index_face);
		}
	}

	std::vector<std::vector<vector3<int>>> faceList;
	sisl::sparse_array3<cell_vertex> face_hash_table;
	utility::ply_writer<T> output_mesh;

	const std::vector<vector3<int>> polyhedron_vertices = {
		{0,0,0}, {0,0,1}, {0,1,0}, {1,0,0},
		{-1,0,0},  {0,-1,0}, {0,0,-1},
	};

	const std::vector<vector3<int>> center_hash_offsets = {
		{1,1,1}, {1,1,-1}, {1,-1,1}, {1,-1,-1},
		{-1,1,1}, {-1,1,-1}, {-1,-1,1}, {-1,-1,-1},
	};

	const std::vector<std::vector<int>> adj_index = {
		{0,2,4,6}, {0,4,1,5}, {0,2,3,1},
		{4,6,5,7}, {6,2,7,3}, {7,3,5,1}
	};

	const std::vector<std::vector<std::vector<int>>> triangle_lookup = {
		{{0,2,4}, {2,4,6}}, {{0,4,1}, {4,1,5}},
	    {{0,2,3}, {3,1,0}}, {{4,6,5}, {6,5,7}},
	    {{6,2,7}, {2,7,3}}, {{7,3,5}, {3,5,1}}
	};

	const std::vector<int> minimal_face_set = {2, 3, 6};

	vector3<T> optimize_for_feature(
			const std::vector<vector3<T>> &points,
			const std::vector<vector3<T>> &normals,
			const T &threshold = 0.1,
			const bool &optimize = true) {
		using namespace Eigen;
		typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> EMatrix;
		vector3<T> center(0,0,0);
		EMatrix A(points.size(), 3), b(points.size(), 1);
		unsigned int i = 0;

		// Calculate the center and setup the matix
		for(auto v : points) {
			center += v;

			A(i, 0) = v.i;
			A(i, 1) = v.j;
			A(i, 2) = v.k;

			b(i, 0) = points[i] * normals[i];
			i++;
		}

		center = center * (1./(T(points.size())));

		return center;
	}

};
};
};

#endif // _DUAL_CC_ISOSURFACE_H_


#ifndef OBJECTIVE_HPP
#define OBJECTIVE_HPP

#include <math.h>
#include <armadillo>
#include "wrap2Pi.hpp"
#include "../dynamicalSystems/koopman/basis_functions/basis_template.hpp"

class Objective {

public:
    arma::mat Q;
    arma::mat Qf;
    arma::mat R;
    arma::mat xd;
    Basis* basis;

    arma::mat Qfk;

    Objective(arma::mat _Q, arma::mat _R, arma::mat _Qf, arma::vec _xd, Basis* _basis) {
        Q = _Q;
        R = _R;
        Qf = _Qf;
        xd = _xd;
        basis = _basis;
        arma::vec Qfkdiag = arma::ones<arma::vec>(_basis->_nX);
        Qfk = arma::diagmat(Qfkdiag);

    }

    inline double l(const arma::vec& x, const arma::vec& u, const arma::vec& phi) {
        arma::vec xn = x;
        arma::vec fk = basis->fk(xn, u);
        return  arma::as_scalar(0.5 * (xn.t() - xd.t()) * Q * (xn - xd));
    }

    arma::vec ldx(const arma::vec& x, const arma::vec& u, const arma::vec& phi) {
        arma::vec xn = x;
        arma::vec fk = basis->fk(xn, u);
        return Q * (xn - xd);

    }

    double m(const arma::vec& x) {
        return arma::as_scalar((x.t() - xd.t()) * Qf * (x - xd) );
    }

    arma::vec mdx(const arma::vec& x) {
        return  Qf * (x - xd);
    }

    double get_cost(const arma::mat& x, const arma::mat& u, const arma::mat & phi) {
        double J = 0.0;
        for (int k = 0; k  < u.n_cols; k++ ) {
            J += l(x.col(k), u.col(k), phi.col(k));
        }
        return J + m(x.tail_cols(1));
    }

};

#endif


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef DESCRIPTORS__GIST_HELPER_HPP
#define DESCRIPTORS__GIST_HELPER_HPP

#include <complex>
#include <cstddef>
#include <cmath>
#include <algorithm>

#include <boost/static_assert.hpp>
#include <opencv2/core/core.hpp>

////////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
void fftshift_even(const cv::Mat_<T>& src, cv::Mat_<T>& dst)
{
    assert(src.isContinuous() && dst.isContinuous());

    //assert that size is even!!!

    dst.create(src.size());

    const int w = src.size().width;
    const int h = src.size().height;
    const int hw = w / 2;
    const int hh = h / 2;

    for (int y = 0; y < hh; y++)
    {
        // src[i] gives the ith row with the T datatype
        std::copy(src[y], src[y] + hw, dst[y + hh] + hw); // copy tl quadrant to br quadrant
        std::copy(src[y] + hw, src[y] + w, dst[y + hh]);      // copy tr quadrant to bl quadrant
    }

    for (int y = hh; y < h; y++)
    {
        std::copy(src[y], src[y] + hw, dst[y - hh] + hw); // copy bl quadrant to tr quadrant
        std::copy(src[y] + hw, src[y] + w, dst[y - hh]);      // copy br quadrant to tl quadrant
    }
}

template <class T>
void generate_gaussian_filter(cv::Mat_<T>& image, double sigma)
{
    const int w = image.size().width;
    const int h = image.size().height;
    const int wh = w / 2;
    const int hh = h / 2;

    const double s = 1.0 / (sigma*sigma);

    for (int y = -hh; y < hh; y++)
    {
        size_t yy = (y + h) % h;
        for (int x = -wh; x < wh; x++)
        {
            size_t xx = (x + w) % w;
            double fx = x;
            double fy = y;
            image(yy, xx) = std::exp(-(fx*fx + fy*fy) * s);
        }
    }
}

class torralba_prefilter
{
    typedef std::complex<float> complex_t;

    double      _sigma;
    cv::Size    _size;
    cv::Mat_<complex_t> _filter;

    public:

    torralba_prefilter(std::size_t width, std::size_t height, double cycles = 4.0)
     : _sigma(cycles / std::sqrt(std::log(2.0)))
     , _size(width, height)
     , _filter(_size)
    {
        generate_gaussian_filter(_filter, _sigma);
    }

    void operator() (cv::Mat& img)
    {
        assert(img.type() == CV_8UC1 && img.size().width == _size.width && img.size().height == _size.height);

        // "whitening"
        cv::Mat_<float> logimg;
        img.convertTo(logimg, CV_32FC1);
        cv::log(1.0 + logimg, logimg);

        cv::Mat_<complex_t> spbuf(_size);
        std::copy(logimg.begin(), logimg.end(), spbuf.begin());

        cv::Mat_<complex_t> frbuf;
        cv::dft(spbuf, frbuf);

        cv::mulSpectrums(frbuf, 1.0 - _filter, frbuf, 0);

        cv::Mat_<complex_t> white;
        cv::idft(frbuf, white, cv::DFT_SCALE);

        // "local contrast normalization"
        cv::MatIterator_<complex_t> dit = spbuf.begin();
        for (cv::MatConstIterator_<complex_t> it = white.begin(); it != white.end(); ++it, ++dit)
        {
            const complex_t& v = *it;
            *dit = v.real() * v.real();
        }

        cv::dft(spbuf, frbuf);
        cv::mulSpectrums(frbuf, _filter, frbuf, 0);
        cv::idft(frbuf, spbuf, cv::DFT_SCALE);

        cv::MatIterator_<unsigned char> dst = img.begin<unsigned char>();
        cv::MatConstIterator_<complex_t> wit = white.begin();
        for (cv::MatConstIterator_<complex_t> it = spbuf.begin(); it != spbuf.end(); ++it, ++wit, ++dst)
        {
            float d = std::sqrt(std::abs((*it).real())) + 0.2;
            float v = std::min(255 * std::max((*wit).real(), 0.0f) / d, 255.0f);
            *dst = v;
        }
    }
};

template <class T>
void generate_gabor_filter(cv::Mat_<T>& image, double peakFreq, double deltaFreq, double orientAngle, double deltaAngle)
{
    const double C = std::sqrt(log(2.0) / M_PI);

    const double Ka = (deltaFreq - 1.0) / (deltaFreq + 1.0);
    const double Kb = std::tan(0.5 * deltaAngle);
    //const double lambda = Ka / Kb;

    // scaling factors of the gaussian envelope
    const double a = peakFreq * (Ka / C);
    //const double b = a / lambda;
    const double b = Kb * peakFreq/C * std::sqrt(1.0 - Ka*Ka);

    // spatial frequency in cartesian coordinates
    const double u0 = peakFreq * std::cos(orientAngle);
    const double v0 = peakFreq * std::sin(orientAngle);

    // default: set orientation of gaussian envelope (theta) equal to orientation of filter
    const double theta = orientAngle;

    // generate filter
    const size_t w = image.size().width;
    const size_t h = image.size().height;
    const double stepx = 1.0 / static_cast<double>(w);
    const double stepy = 1.0 / static_cast<double>(h);
    const double cos_theta = std::cos(theta);
    const double sin_theta = std::sin(theta);
    double v = 0.5 - v0;

    for (size_t yy = 0; yy < h; yy++)
    {
        size_t y = (yy + (h / 2)) % h;
        double u = -0.5 - u0;
        for (size_t xx = 0; xx < w; xx++)
        {
            size_t x = (xx + (w / 2)) % w;

            double ur = u * cos_theta + v * sin_theta;
            double vr = -u * sin_theta + v * cos_theta;

            double U = ur / a;
            double V = vr / b;

            double value = std::exp(-M_PI * (U*U + V*V));

            image(y, x) = value;

            u += stepx;
        }

        v -= stepy;
    }
}

template <class T>
void generate_polargabor_filter(cv::Mat_<T>& image, double peakFreq, double deltaFreq, double orientAngle, double deltaAngle)
{
    // sigma_omega = 1 / (kappa * omega)
    double kappa = (deltaFreq - 1) / ((deltaFreq + 1) * std::sqrt(2*std::log(2.0)));

//    double sigma_theta = std::sqrt(2*PI)*4.0*numorients/32.0; // torralba
    double sigma_theta = std::sqrt(std::log(2.0)) * 2.0 / deltaAngle;

    // generate filter
    const size_t w = image.size().width;
    const size_t h = image.size().height;
    const double stepx = 1.0 / static_cast<double>(w);
    const double stepy = 1.0 / static_cast<double>(h);

    double v = -0.5;
    for (size_t yy = 0; yy < h; yy++)
    {
        size_t y = (yy + (h / 2)) % h;

        double u = -0.5;
        for (size_t xx = 0; xx < w; xx++)
        {
            size_t x = (xx + (w / 2)) % w;

            double omega = std::sqrt(u*u + v*v);
            double theta = std::atan2(v, u);

            double Omega = omega/peakFreq - 1;
            double Theta = theta + orientAngle;

            if (Theta < -M_PI) Theta += 2*M_PI;
            if (Theta >  M_PI) Theta -= 2*M_PI;

            double value = std::exp(-1/(2*kappa*kappa) * Omega*Omega - sigma_theta*sigma_theta * Theta*Theta);

            image(y, x) = value;

            u += stepx;
        }

        v += stepy;
    }
}

template <class T>
void symmetric_pad(const cv::Mat_<T>& src, cv::Mat_<T>& dst)
{
    cv::Mat_<T> tmp;

    if (src.cols < dst.cols)
    {
        int width = dst.cols;
        int height = std::min(src.rows, dst.rows);

        int pad = dst.cols - src.cols;
        int border = src.cols + pad/2;

        tmp.create(height, width);

        cv::Mat_<T> flipped;
        cv::flip(src, flipped, 1);

        for (int p = 0, k = 0; p < border; p += src.cols, k++)
        {
            int w = std::min(src.cols, border - p);
            int h = height;

            cv::Mat_<T> r = tmp(cv::Rect(p, 0, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, 0, w, h)).copyTo(r);
            }
            else
            {
                src(cv::Rect(0, 0, w, h)).copyTo(r);
            }
        }

        for (int p = width, k = 1; p >= border; p -= src.cols, k++)
        {
            int w = std::min(src.cols, p - border);
            int h = height;

            cv::Mat_<T> r = tmp(cv::Rect(p - w, 0, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(src.cols - w, 0, w, h)).copyTo(r);
            }
            else
            {
                src(cv::Rect(src.cols - w, 0, w, h)).copyTo(r);
            }
        }
    }
    else
    {
        tmp = src;
    }

    if (src.rows < dst.rows)
    {
        int width = dst.cols;
        int height = dst.rows;

        int pad = dst.rows - src.rows;
        int border = src.rows + pad/2;

        cv::Mat_<T> flipped;
        cv::flip(tmp, flipped, 0);

        for (int p = 0, k = 0; p < border; p += src.rows, k++)
        {
            int w = width;
            int h = std::min(src.rows, border - p);

            cv::Mat_<T> r = dst(cv::Rect(0, p, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, 0, w, h)).copyTo(r);
            }
            else
            {
                tmp(cv::Rect(0, 0, w, h)).copyTo(r);
            }
        }

        for (int p = height, k = 1; p >= border; p -= src.rows, k++)
        {
            int w = width;
            int h = std::min(src.rows, p - border);

            cv::Mat_<T> r = dst(cv::Rect(0, p - h, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, src.rows - h, w, h)).copyTo(r);
            }
            else
            {
                tmp(cv::Rect(0, src.rows - h, w, h)).copyTo(r);
            }
        }
    }
    else
    {
        tmp.copyTo(dst);
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////

#endif // DESCRIPTORS__GIST_HELPER_HPP


//
//  Copyright (c) 2018-2020, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019-2020, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under 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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//
//  And we acknowledge the support from all contributors.

#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <boost/test/unit_test.hpp>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_tensor_functions)

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

//using test_types = zip<int>::with_t<boost::numeric::ublas::layout::first_order>;

struct fixture
{
    using dynamic_extents_type = boost::numeric::ublas::extents<>;
    fixture()
      : extents {
          dynamic_extents_type{1,1}, // 1
          dynamic_extents_type{2,3}, // 2
          dynamic_extents_type{2,3,1}, // 3
          dynamic_extents_type{4,2,3}, // 4
          dynamic_extents_type{4,2,3,5}} // 5
    {
    }

    std::vector<dynamic_extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_vector, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;
    using vector_type  = typename tensor_type::vector_type;

    for(auto const& n : extents){

        auto a = tensor_type(n, value_type{2});

        for(auto m = 0u; m < ublas::size(n); ++m){

            auto b = vector_type  (n[m], value_type{1} );

            auto c = ublas::prod(a, b, m+1);

            for(auto i = 0u; i < c.size(); ++i)
                BOOST_CHECK_EQUAL( c[i] , value_type( static_cast< inner_type_t<value_type> >(n[m]) ) * a[i] );

        }
    }
  auto n = extents[4];
  auto a = tensor_type(n, value_type{2});
  auto b = vector_type(n[0], value_type{1});

  auto empty = vector_type{};

  BOOST_CHECK_THROW(prod(a, b, 0), std::length_error);
  BOOST_CHECK_THROW(prod(a, b, 9), std::length_error);
  BOOST_CHECK_THROW(prod(a, empty, 2), std::length_error);

}

BOOST_AUTO_TEST_CASE( test_tensor_prod_vector_exception )
{
//    namespace ublas = boost::numeric::ublas;
//    using value_type   = float;
//    using layout_type  = ublas::layout::first_order;
//    using d_tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;
//    using vector_type  = typename d_tensor_type::vector_type;

//    auto t1 = d_tensor_type{ublas::extents<>{},1.f};
//    auto v1 = vector_type{3,value_type{1}};

//    BOOST_REQUIRE_THROW(prod(t1,v1,0),std::length_error);
//    BOOST_REQUIRE_THROW(prod(t1,v1,1),std::length_error);
//    BOOST_REQUIRE_THROW(prod(t1,v1,3),std::length_error);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_matrix, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;
    using matrix_type  = typename tensor_type::matrix_type;

    for(auto const& n : extents) {

        auto a = tensor_type(n, value_type{2});

        for(auto m = 0u; m < ublas::size(n); ++m){

            auto b  = matrix_type  ( n[m], n[m], value_type{1} );

            auto c = ublas::prod(a, b, m+1);

            for(auto i = 0u; i < c.size(); ++i)
                BOOST_CHECK_EQUAL( c[i] , value_type( static_cast< inner_type_t<value_type> >(n[m]) ) * a[i] );

        }
    }

  auto n = extents[4];
  auto a = tensor_type(n, value_type{2});
  auto b = matrix_type(n[0], n[0], value_type{1});

  auto empty = matrix_type{};

  BOOST_CHECK_THROW(prod(a, b, 0), std::length_error);
  BOOST_CHECK_THROW(prod(a, b, 9), std::length_error);
  BOOST_CHECK_THROW(prod(a, empty, 2), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE( test_tensor_prod_matrix_exception )
{
//    namespace ublas = boost::numeric::ublas;
//    using value_type   = float;
//    using layout_type  = ublas::layout::first_order;
//    using d_extents_type = ublas::extents<>;
//    using d_tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;
//    using matrix_type  = typename d_tensor_type::matrix_type;

//    auto t1 = d_tensor_type{d_extents_type{},1.f};
//    auto m1 = matrix_type{3,3,value_type{1}};

//    BOOST_REQUIRE_THROW(prod(t1,m1,0),std::length_error);
//    BOOST_REQUIRE_THROW(prod(t1,m1,1),std::length_error);
//    BOOST_REQUIRE_THROW(prod(t1,m1,3),std::length_error);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_tensor_1, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    // left-hand and right-hand side have the
    // the same number of elements

    for(auto const& na : extents) {

        auto a  = tensor_type( na, value_type{2} );
        auto b  = tensor_type( na, value_type{3} );

        auto const pa = a.rank();

        // the number of contractions is changed.
        for( auto q = 0ul; q <= pa; ++q) { // pa

            auto phi = std::vector<std::size_t> ( q );

            std::iota(phi.begin(), phi.end(), 1ul);

            auto c = ublas::prod(a, b, phi);

            auto acc = value_type(1);
            for(auto i = 0ul; i < q; ++i)
                acc *= value_type( static_cast< inner_type_t<value_type> >( a.extents().at(phi.at(i)-1) ) );

            for(auto i = 0ul; i < c.size(); ++i)
                BOOST_CHECK_EQUAL( c[i] , acc * a[0] * b[0] );

        }
    }
}

BOOST_AUTO_TEST_CASE( test_tensor_prod_tensor_1_exception )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = float;
    using layout_type  = ublas::layout::first_order;
    using d_extents_type = ublas::extents<>;
    using d_tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    std::vector<std::size_t> phia = {1,2,3};
    std::vector<std::size_t> phib = {1,2,3,4,5};

    auto t3 = d_tensor_type{d_extents_type{1,2},1.f};
    auto t4 = d_tensor_type{d_extents_type{1,2},1.f};
    BOOST_REQUIRE_THROW(prod(t3,t4,phia,phib),std::runtime_error);

    auto t5 = d_tensor_type{d_extents_type{1,2,3,4},1.f};
    auto t6 = d_tensor_type{d_extents_type{1,2},1.f};
    BOOST_REQUIRE_THROW(prod(t5,t6,phia,phib),std::runtime_error);

    auto t7 = d_tensor_type{d_extents_type{1,2,3,4,5},1.f};
    auto t8 = d_tensor_type{d_extents_type{1,2,3,4,5},1.f};
    BOOST_REQUIRE_THROW(prod(t7,t8,phia,phib),std::runtime_error);

    std::vector<std::size_t> phia_2 = {1,2,3,5,4};
    std::vector<std::size_t> phib_2 = {1,2,3,4,5};
    auto t9 = d_tensor_type{d_extents_type{1,2,3,4,5,6},1.f};
    auto t10 = d_tensor_type{d_extents_type{1,2,3,4,5,6},1.f};
    BOOST_REQUIRE_THROW(prod(t9,t10,phia_2,phib_2),std::runtime_error);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_prod_tensor_2, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;
    using extents_type = typename tensor_type::extents_type;

    auto compute_factorial = [](auto const& p){
        auto f = 1ul;
        for(auto i = 1u; i <= p; ++i)
            f *= i;
        return f;
    };

    auto permute_extents = [](auto const& pi, auto const& na){
      auto nb_base = na.base();
      assert(pi.size() == ublas::size(na));
      for(auto j = 0u; j < pi.size(); ++j)
        nb_base[pi[j]-1] = na[j];
      return extents_type(nb_base);
    };

    // left-hand and right-hand side have the
    // the same number of elements

    for(auto const& na : extents) {

        auto a  = tensor_type( na, value_type{2} );
        auto const pa = a.rank();

        auto pi   = std::vector<std::size_t>(pa);
        auto fac = compute_factorial(pa);
        std::iota( pi.begin(), pi.end(), 1 );

        for(auto f = 0ul; f < fac; ++f)
        {
            auto nb = permute_extents( pi, na  );
            auto b  = tensor_type( nb, value_type{3} );

            // the number of contractions is changed.
            for( auto q = 0ul; q <= pa; ++q) { // pa

                auto phia = std::vector<std::size_t> ( q );  // concatenation for a
                auto phib = std::vector<std::size_t> ( q );  // concatenation for b

                std::iota(phia.begin(), phia.end(), 1ul);
                std::transform(  phia.begin(), phia.end(), phib.begin(),
                                 [&pi] ( std::size_t i ) { return pi.at(i-1); } );

                auto c = ublas::prod(a, b, phia, phib);

                auto acc = value_type(1);
                for(auto i = 0ul; i < q; ++i)
                    acc *= value_type( static_cast< inner_type_t<value_type> >( a.extents().at(phia.at(i)-1) ) );

                for(auto i = 0ul; i < c.size(); ++i)
                    BOOST_CHECK_EQUAL( c[i] , acc * a[0] * b[0] );

            }

            std::next_permutation(pi.begin(), pi.end());
        }
    }

    auto phia = std::vector<std::size_t >(3);
    auto sphia = std::vector<std::size_t>(2);

//    BOOST_CHECK_THROW(ublas::prod(tensor_type{}, tensor_type({2,1,2}), phia, phia), std::runtime_error);
//        BOOST_CHECK_THROW(ublas::prod(tensor_type({1,2,3}), tensor_type(), phia, phia), std::runtime_error);
        BOOST_CHECK_THROW(ublas::prod(tensor_type{1,2,4}, tensor_type{2,1}, phia, phia), std::runtime_error);
        BOOST_CHECK_THROW(ublas::prod(tensor_type{1,2}, tensor_type{2,1,2}, phia, phia), std::runtime_error);
        BOOST_CHECK_THROW(ublas::prod(tensor_type{1,2}, tensor_type{2,1,3}, sphia, phia), std::runtime_error);
        BOOST_CHECK_THROW(ublas::prod(tensor_type{1,2}, tensor_type{2,2}, phia, sphia), std::runtime_error);
        BOOST_CHECK_THROW(ublas::prod(tensor_type{1,2}, tensor_type{4,4}, sphia, phia), std::runtime_error);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_inner_prod, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    for(auto const& n : extents) {

        auto a  = tensor_type(n, value_type(2));
        auto b  = tensor_type(n, value_type(1));

        auto c = ublas::inner_prod(a, b);
        auto r = std::inner_product(a.begin(),a.end(), b.begin(),value_type(0));

        BOOST_CHECK_EQUAL( c , r );

    }
  BOOST_CHECK_THROW(ublas::inner_prod(tensor_type{1,2,3}, tensor_type{1,2,3,4}), std::length_error); // rank different
//  BOOST_CHECK_THROW(ublas::inner_prod(tensor_type(), tensor_type()), std::length_error); //empty tensor
  BOOST_CHECK_THROW(ublas::inner_prod(tensor_type{1,2,3}, tensor_type{3,2,1}), std::length_error); // different extent
}

BOOST_AUTO_TEST_CASE( test_tensor_inner_prod_exception )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = float;
    using layout_type  = ublas::layout::first_order;
    using d_extents_type = ublas::extents<>;
    using d_tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto t1 = d_tensor_type{d_extents_type{1,2},1.f};
    auto t2 = d_tensor_type{d_extents_type{1,2,3},1.f};
    BOOST_REQUIRE_THROW( ublas::inner_prod(t1, t2), std::length_error);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_norm, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    for(auto const& n : extents) {

        auto a  = tensor_type(n);

        auto one = value_type(1);
        auto v = one;
        for(auto& aa: a)
            aa = v, v += one;

        auto c = ublas::inner_prod(a, a);
        auto r = std::inner_product(a.begin(),a.end(), a.begin(),value_type(0));

        tensor_type var = (a+a)/value_type(2); // std::complex<float>/int not allowed as expression is captured
        auto r2 = ublas::norm( var );

//        BOOST_CHECK_THROW(ublas::norm(tensor_type{}), std::runtime_error);

        BOOST_CHECK_EQUAL( c , r );
        BOOST_CHECK_EQUAL( std::sqrt( c ) , r2 );

    }
}

BOOST_FIXTURE_TEST_CASE( test_tensor_real_imag_conj, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = float;
    using complex_type = std::complex<value_type>;
    using layout_type  = ublas::layout::first_order;

    using tensor_complex_type  = ublas::tensor_dynamic<complex_type, layout_type>;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    for(auto const& n : extents) {

        auto a   = tensor_type(n);
        auto r0  = tensor_type(n);
        auto r00 = tensor_complex_type(n);

        auto one = value_type(1);
        auto v = one;
        for(auto& aa: a)
            aa = v, v += one;

        tensor_type b = (a+a) / value_type( 2 );
        tensor_type r1 = ublas::real( (a+a) / value_type( 2 )  );
        std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::real( l );  }   );
        BOOST_CHECK( (bool) (r0 == r1) );

        tensor_type r2 = ublas::imag( (a+a) / value_type( 2 )  );
        std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::imag( l );  }   );
        BOOST_CHECK( (bool) (r0 == r2) );

        tensor_complex_type r3 = ublas::conj( (a+a) / value_type( 2 )  );
        std::transform(  b.begin(), b.end(), r00.begin(), [](auto const& l){ return std::conj( l );  }   );
        BOOST_CHECK( (bool) (r00 == r3) );

    }

    for(auto const& n : extents) {

        auto a   = tensor_complex_type(n);

        auto r00 = tensor_complex_type(n);
        auto r0  = tensor_type(n);

        auto one = complex_type(1,1);
        auto v = one;
        for(auto& aa: a)
            aa = v, v = v + one;

        tensor_complex_type b = (a+a) / complex_type( 2,2 );

        tensor_type r1 = ublas::real( (a+a) / complex_type( 2,2 )  );
        std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::real( l );  }   );
        BOOST_CHECK( (bool) (r0 == r1) );

        tensor_type r2 = ublas::imag( (a+a) / complex_type( 2,2 )  );
        std::transform(  b.begin(), b.end(), r0.begin(), [](auto const& l){ return std::imag( l );  }   );
        BOOST_CHECK( (bool) (r0 == r2) );

        tensor_complex_type r3 = ublas::conj( (a+a) / complex_type( 2,2 )  );
        std::transform(  b.begin(), b.end(), r00.begin(), [](auto const& l){ return std::conj( l );  }   );
        BOOST_CHECK( (bool) (r00 == r3) );

    }

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_outer_prod, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    for(auto const& n1 : extents) {
        auto a  = tensor_type(n1, value_type(2));
        for(auto const& n2 : extents) {

            auto b  = tensor_type(n2, value_type(1));
            auto c  = ublas::outer_prod(a, b);

            for(auto const& cc : c)
                BOOST_CHECK_EQUAL( cc , a[0]*b[0] );
        }
    }
}

template<class V>
void init(std::vector<V>& a)
{
    auto v = V(1);
    for(auto i = 0u; i < a.size(); ++i, ++v){
        a[i] = v;
    }
}

template<class V>
void init(std::vector<std::complex<V>>& a)
{
    auto v = std::complex<V>(1,1);
    for(auto i = 0u; i < a.size(); ++i){
        a[i] = v;
        v.real(v.real()+1);
        v.imag(v.imag()+1);
    }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_trans, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto fak = [](auto const& p){
        auto f = 1ul;
        for(auto i = 1u; i <= p; ++i)
            f *= i;
        return f;
    };

    auto inverse = [](auto const& pi){
        auto pi_inv = pi;
        for(auto j = 0u; j < pi.size(); ++j)
            pi_inv[pi[j]-1] = j+1;
        return pi_inv;
    };

    for(auto const& n : extents)
    {
      auto const p = ublas::size(n);
      auto const s = ublas::product(n);
        auto aref = tensor_type(n);
        auto v    = value_type{};
        for(auto i = 0u; i < s; ++i, v+=1)
            aref[i] = v;
        auto a    = aref;

        auto pi = std::vector<std::size_t>(p);
        std::iota(pi.begin(), pi.end(), 1);
        a = ublas::trans( a, pi );
        bool res1 = a == aref;
        BOOST_CHECK( res1 );

        auto const pfak = fak(p);
        auto i = 0u;
        for(; i < pfak-1; ++i) {
            std::next_permutation(pi.begin(), pi.end());
            a = ublas::trans( a, pi );
        }
        std::next_permutation(pi.begin(), pi.end());
        for(; i > 0; --i) {
            std::prev_permutation(pi.begin(), pi.end());
            auto pi_inv = inverse(pi);
            a = ublas::trans( a, pi_inv );
        }
        bool res2 = a == aref; // it was an expression. so evaluate into bool
        BOOST_CHECK( res2 );

    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "math_unit_test.hpp"
#include <boost/math/fft/bsl_backend.hpp>
#if defined(__GNUC__)
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#endif
#include <boost/math/constants/constants.hpp>

#include <algorithm>
#include <list>
#include <type_traits>
#include <complex>
#include <vector>
#include <limits>
#include <cmath>
#include <random>

using namespace boost::math::fft;

template<class T>
std::vector<std::complex<T>> random_vector(int N)
{
  using local_vector_complex_type = std::vector<std::complex<T>>;

  std::mt19937 rng;
  std::uniform_real_distribution<T> U(0.0,1.0);
  local_vector_complex_type A(N);
  for(auto& x: A)
  {
    x.real( U(rng) );
    x.imag( U(rng) );
  }
  return A;
}
template<class Container1, class Container2>
typename Container1::value_type::value_type difference(const Container1& A, const Container2& B)
{
  BOOST_MATH_ASSERT_MSG( A.size()==B.size(), "Different container sizes.");
  using ComplexType = typename Container1::value_type;
  using RealType = typename ComplexType::value_type;
  const RealType inv_N = RealType{1}/B.size();
  RealType diff =
    std::inner_product(
      A.begin(),A.end(),
      B.begin(),
      RealType{0.0},
      std::plus<RealType>(),
      [inv_N](const ComplexType & a, const ComplexType& b)
      {
        return std::norm(a-b*inv_N);
      });
  diff = std::sqrt(diff)/A.size();
  return diff;
}

template<class Backend>
void test_inverse(int N, int tolerance)
{
  using ComplexType = typename Backend::value_type;
  using RealType    = typename ComplexType::value_type;

  const RealType tol = tolerance*std::numeric_limits<RealType>::epsilon();
  const std::vector< ComplexType > A{random_vector<RealType>(N)};

  Backend plan(1);
  {
    std::vector<ComplexType> B(N), C(N);

    plan.forward(std::begin(A),std::end(A),std::begin(B));
    plan.backward(std::begin(B),std::end(B),std::begin(C));

    RealType diff{difference(A,C)};
    CHECK_MOLLIFIED_CLOSE(RealType{0.0},diff,tol);
  }
  {
    std::vector<ComplexType>  C(N);
    std::list<ComplexType> B;

    plan.forward(std::begin(A),std::end(A),std::back_inserter(B));
    plan.backward(std::begin(B),std::end(B),std::begin(C));

    RealType diff{difference(A,C)};
    CHECK_MOLLIFIED_CLOSE(RealType{0.0},diff,tol);
  }
  {
    std::list<ComplexType> C;

    plan.forward(std::begin(A),std::end(A),std::back_inserter(C));
    plan.backward(std::begin(C),std::end(C),std::begin(C));

    RealType diff{difference(A,C)};
    CHECK_MOLLIFIED_CLOSE(RealType{0.0},diff,tol);
  }
}

#if defined(__GNUC__)
template<class T>
using complex_fftw_dft = fftw_dft< boost::multiprecision::complex<T>  >;

template<class T>
using complex_gsl_dft = gsl_dft< boost::multiprecision::complex<T>  >;
#endif

template<class T>
using complex_bsl_dft = bsl_dft< boost::multiprecision::complex<T>  >;

int main()
{
  for(int i=1;i<=(1<<12); i*=2)
  {
#if defined(__GNUC__)
    test_inverse<complex_fftw_dft<float>>(i,1);
    test_inverse<complex_fftw_dft<double>>(i,1);
    test_inverse<complex_fftw_dft<long double>>(i,1);

    test_inverse<complex_gsl_dft<double>>(i,1);
#endif

    test_inverse<complex_bsl_dft<float>>(i,1);
    test_inverse<complex_bsl_dft<double>>(i,1);
    test_inverse<complex_bsl_dft<long double>>(i,1);
  }
  return boost::math::test::report_errors();
}


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;
  using size_type = lf::mesh::Mesh::size_type;

  //====================
  // Your code goes here
  // index() method of lf:mesh:Mesh provides the numbering of entities which underlies
  // indexing of the entries of the incidence matrixes.
  // lf::mesh::Mesh::Index() provides a consecutive numbering of all mesh entities of a specific co-dimension
  // lf::mesh::Entity::SubEntities() returns an array of subentities and fix their ordering.
  const std::size_t nnz_row=2;
  const size_type num_edge = mesh.Numentities(1);
  const size_type num_node = mesh.Numentities(2);

  Eigen::SparseMatrix<int, Eigen::RowMajor> G(num_edge, num_node);
  G.reserve(Eigen::VectorXi::Constant(num_edge,nnz_row));

  // to compute G efficiently, we iterate over all edges and
  // check the index of the nodes at its end.
  // this is the efficient way to do the assembly, introduced as distribute scheme
  // in class
  for(lf::mesh::Entity *edge: mesh.Entities(1)){
    nonstd::span<const lf::mesh::Entity *const> node{edge.SubEntities(1)}; // the relative codimension is 1
    size_type edge_index = mesh.Index(*edge);

    size_type node_start_index = mesh.Index(*node[0]);
    size_type node_end_index = mesh.Index(*node[1]); //
    // subentities returned from SubEntities() can be accessed through [] operator using their local index
    G.coeffRef(edge_index, node_start_index) +=1;
    G.coeffRef(edge_index, node_end_index) -=1;
  }

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

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

  //====================
  // Your code goes here
  using size_type = lf::mesh::Mesh::size_type;
  const std::size_t nnz = 4;
  const size_type num_cell = mesh.NumEntities(0);
  const size_type num_edge = mesh.NumEntities(1);
  Eigen::SparseMatrix<int, Eigen::RowMajor> D(num_cell,num_edge);
  D.reserve(Eigen::VectorXi::Constant(num_cell,nnz));
  for(lf::mesh::Entity *cell: mesh.Entities(0)){
    // get edges and their orientations of a cell
    nonstd::span<const Entity* const> edges = cell->SubEntities(1);
    nonstd::span<const Entity* const> orientations = cell->RelativeOrientations();
    size_type cell_index = mesh.Index(*cell);
    auto edge_start = edges.begin();
    auto orientation_start = orientation.begin();
    // get the index of each edge and add their orientations to D
    for(; edge_start !=edges.end() && orientation_start != orientation.end(); edge_start++, orientation++){
      size_type edge_index=mesh.Index(*edge[edge_start]);
      D.coeffRef(cell_index,edge_index) += lf::mesh::to_sign(orientation_start);
    }
  }

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

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

  //====================
  // Your code goes here
  // returns true whenever the two incidence matrices of the 2D hybrid mesh satisfy
  // the relationship asserted in 2.6.6
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(*mesh);
  Eigen::SparseMatrix<int> D = computeCellEdgeIncidenceMatrix(*mesh);
  auto O = G*D;

  isZero = O.norm()==0;
  }
  //====================
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_2_algorithms.h>
#include <CGAL/Straight_skeleton_builder_2.h>
#include <CGAL/Polygon_offset_builder_2.h>
#include <CGAL/compute_outer_frame_margin.h>
#include "print.h"

#include <boost/shared_ptr.hpp>

#include <vector>
#include <cassert>

//
// This example illustrates how to use the CGAL Straight Skeleton package
// to construct an offset contour on the outside of a polygon
//

// This is the recommended kernel
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef Kernel::Point_2 Point_2;
typedef CGAL::Polygon_2<Kernel>    Contour;
typedef boost::shared_ptr<Contour> ContourPtr;
typedef std::vector<ContourPtr>    ContourSequence ;

typedef CGAL::Straight_skeleton_2<Kernel> Ss;

typedef Ss::Halfedge_iterator Halfedge_iterator;
typedef Ss::Halfedge_handle   Halfedge_handle;
typedef Ss::Vertex_handle     Vertex_handle;

typedef CGAL::Straight_skeleton_builder_traits_2<Kernel>      SsBuilderTraits;
typedef CGAL::Straight_skeleton_builder_2<SsBuilderTraits,Ss> SsBuilder;

typedef CGAL::Polygon_offset_builder_traits_2<Kernel>                  OffsetBuilderTraits;
typedef CGAL::Polygon_offset_builder_2<Ss,OffsetBuilderTraits,Contour> OffsetBuilder;

int main()
{
  // A start-shaped polygon, oriented counter-clockwise as required for outer contours.
  Point_2 pts[] = { Point_2(-1,-1)
                  , Point_2(0,-12)
                  , Point_2(1,-1)
                  , Point_2(12,0)
                  , Point_2(1,1)
                  , Point_2(0,12)
                  , Point_2(-1,1)
                  , Point_2(-12,0)
                  } ;

  std::vector<Point_2> star(pts,pts+8);

  assert(CGAL::orientation_2(pts,pts+8,Kernel()) == CGAL::COUNTERCLOCKWISE);

  // We want an offset contour in the outside.
  // Since the package doesn't support that operation directly, we use the following trick:
  // (1) Place the polygon as a hole of a big outer frame.
  // (2) Construct the skeleton on the interior of that frame (with the polygon as a hole)
  // (3) Construc the offset contours
  // (4) Identify the offset contour that corresponds to the frame and remove it from the result

  double offset = 3 ; // The offset distance

  // First we need to determine the proper separation between the polygon and the frame.
  // We use this helper function provided in the package.
  boost::optional<double> margin = CGAL::compute_outer_frame_margin(star.begin(),star.end(),offset);

  // Proceed only if the margin was computed (an extremely sharp corner might cause overflow)
  if ( margin )
  {
    // Get the bbox of the polygon
    CGAL::Bbox_2 bbox = CGAL::bbox_2(star.begin(),star.end());

    // Compute the boundaries of the frame
    double fxmin = bbox.xmin() - *margin ;
    double fxmax = bbox.xmax() + *margin ;
    double fymin = bbox.ymin() - *margin ;
    double fymax = bbox.ymax() + *margin ;

    // Create the rectangular frame
    Point_2 frame[4]= { Point_2(fxmin,fymin)
                      , Point_2(fxmax,fymin)
                      , Point_2(fxmax,fymax)
                      , Point_2(fxmin,fymax)
                      } ;

    // Instantiate the skeleton builder
    SsBuilder ssb ;

    // Enter the frame
    ssb.enter_contour(frame,frame+4);

    // Enter the polygon as a hole of the frame (NOTE: as it is a hole we insert it in the opposite orientation)
    ssb.enter_contour(star.rbegin(),star.rend());

    // Construct the skeleton
    boost::shared_ptr<Ss> ss = ssb.construct_skeleton();

    // Proceed only if the skeleton was correctly constructed.
    if ( ss )
    {
      print_straight_skeleton(*ss);

      // Instantiate the container of offset contours
      ContourSequence offset_contours ;

      // Instantiate the offset builder with the skeleton
      OffsetBuilder ob(*ss);

      // Obtain the offset contours
      ob.construct_offset_contours(offset, std::back_inserter(offset_contours));

      // Locate the offset contour that corresponds to the frame
      // That must be the outmost offset contour, which in turn must be the one
      // with the largetst unsigned area.
      ContourSequence::iterator f = offset_contours.end();
      double lLargestArea = 0.0 ;
      for (ContourSequence::iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i  )
      {
        double lArea = CGAL_NTS abs( (*i)->area() ) ; //Take abs() as  Polygon_2::area() is signed.
        if ( lArea > lLargestArea )
        {
          f = i ;
          lLargestArea = lArea ;
        }
      }

      // Remove the offset contour that corresponds to the frame.
      offset_contours.erase(f);

      print_polygons(offset_contours);
    }
  }

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// File: dense_lu_test.cpp
#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Matrix>
void singularity_test(const Matrix& A)
{
     try {
	Matrix B(lu_f(A));

    } catch (mtl::matrix_singular excp) {
	std::cout << "Exception for singularity successfully caught\n"; return;
    }
    throw "Singularity not detected";
}

int main(int , char**)
{
    using namespace mtl;
    dense2D<double, mat::parameters<tag::col_major> > A(5, 5);
    dense2D<double, mat::parameters<tag::col_major> > B(5, 5);
    dense2D<float, mat::parameters<tag::col_major> > bsp(4, 4);
    dense2D<float, mat::parameters<tag::col_major> > bsp1(4, 4);

    bsp(0,0)=0;
    bsp(0,1)=0;
    bsp(0,2)=1;
    bsp(0,3)=1;
    bsp(1,0)=2;
    bsp(1,1)=2;
    bsp(1,2)=2;
    bsp(1,3)=2;
    bsp(2,0)=1;
    bsp(2,1)=2;
    bsp(2,2)=2;
    bsp(2,3)=2;
    bsp(3,0)=1;
    bsp(3,1)=2;
    bsp(3,2)=3;
    bsp(3,3)=6;

    // Assign a three times the identity to A
    A= 3;
    A(2,3)=7.0;
    A(3,0)=7.0;
    A(4,1)=7.0;
    B=A;
    std::cout << "bsp is \n" << bsp << "\n";
    bsp1 = bsp;
    singularity_test(bsp1);
    // bsp1=lu_f(bsp1); // throws exception

    mtl::dense_vector<int> P;
    lu(bsp, P);
    std::cout << "LU(bsp) with pivoting \n" << bsp << "Permutation is " << P << "\n";
    std::cout << "LU_f(bsp) \n" << bsp1 << "\n";

    std::cout << "A is \n" << A << "\n";
    A=lu_f(A);
    std::cout << "LU_f(A) is \n" << A << "\n";
    lu(B, P);
    std::cout << "LU with pivoting is \n" << with_format(B, 5, 2) << "Permutation is " << P << "\n";

    return 0;
}


#include <cmath>
#include <string>
#include <iostream>
#include <stdexcept>
#include <limits>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ARPoly.h"
#include "GaussSeq.h"
#include "utils.h"
using utils::my_float;

/*
 *  Constructors and destructors
 */
ARPoly::ARPoly(std::map<int, my_float> mp, std::map<int, my_float> powers):
    ARSeq(mp),
    powers{powers}
{}
ARPoly::ARPoly(std::map<int, my_float> mp, std::map<int, my_float> powers, my_float bias):
    ARSeq(mp, bias),
    powers{powers}
{}

/*
 *  Member functions
 */
my_float ARPoly::next() {
    namespace mp = boost::multiprecision;
    std::vector<my_float> copy_prev = prev_val;

    for (const auto& ele : powers) {
        float possible_val = mp::pow(prev_val[ele.first - 1], ele.second).convert_to<float>();
        if (std::fpclassify(possible_val == FP_NAN) {
            throw std::runtime_error("Exponentiation returns complex value!");
        }
        if (std::fabs(possible_val - std::numeric_limits<float>::max()) < std::numeric_limits<float>::epsilon()) {
            throw std::runtime_error("Exponentiation returns infinite value!");
        }
        // subtract one since the zero-index is the previous value
        copy_prev[ele.first - 1] = mp::pow(prev_val[ele.first - 1], ele.second);
    }
    // Since the lag coefficients are only non-zero at non-zero constants, take the dot-product
    my_float result = utils::dot_product(copy_prev, lag_coeff) + g.next() + bias;
    utils::shift_vector(prev_val);
    prev_val[0] = result;
    return result;
}