#include "lelantus_test_fixture.h"

#include "../range_prover.h"
#include "../range_verifier.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

// All versions to be tested
unsigned int test_versions[] = {
    LELANTUS_TX_VERSION_4,
    SIGMA_TO_LELANTUS_JOINSPLIT,
    LELANTUS_TX_VERSION_4_5,
    SIGMA_TO_LELANTUS_JOINSPLIT_FIXED,
    LELANTUS_TX_TPAYLOAD,
    SIGMA_TO_LELANTUS_TX_TPAYLOAD
};

BOOST_FIXTURE_TEST_SUITE(lelantus_range_proof_tests, LelantusTestingSetup)

// A single valid aggregated range proof
BOOST_AUTO_TEST_CASE(prove_verify_single)
{
    // Parameters
    std::size_t n = 64;
    std::size_t max_m = 8;

    // Generators
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();

    auto prove_verify = [&] (std::size_t m, unsigned int version) {
        auto g_ = RandomizeGroupElements(n * m);
        auto h_ = RandomizeGroupElements(n * m);

        // Input data
        auto serials = RandomizeScalars(m);
        auto randoms = RandomizeScalars(m);

        std::vector<secp_primitives::Scalar> v_s;
        std::vector<secp_primitives::GroupElement> V;
        for (std::size_t i = 0; i < m; ++i){
            v_s.emplace_back(i);
            V.push_back(g_gen * v_s.back() +  h_gen1 * randoms[i] + h_gen2 * serials[i]);
        }

        // Prove
        RangeProver rangeProver(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        RangeProof proof;
        rangeProver.proof(v_s, serials, randoms, V, proof);

        // Verify
        RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));
    };

    // Test powers of 2
    std::size_t i = 1;
    while (i <= max_m) {
        for (auto version : test_versions)
            prove_verify(i, version);
        i *= 2;
    }

}

// A batch of valid aggregated range proofs of different size
BOOST_AUTO_TEST_CASE(prove_verify_batch)
{
    // Parameters
    const std::size_t n = 64;
    const std::vector<std::size_t> m = {1,2,4,8};

    // Generators
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();
    std::size_t max_m = *std::max_element(m.begin(), m.end());
    auto g_ = RandomizeGroupElements(n * max_m);
    auto h_ = RandomizeGroupElements(n * max_m);

    for (auto version : test_versions)
    {
        // Proofs
        std::vector<std::vector<GroupElement> > V_batch;
        V_batch.reserve(m.size());
        std::vector<RangeProof> proof_batch;
        proof_batch.reserve(m.size());
        for (std::size_t i = 0; i < m.size(); i++) {
            RangeProver rangeProver(g_gen, h_gen1, h_gen2, std::vector<GroupElement>(g_.begin(), g_.begin() + n * m[i]), std::vector<GroupElement>(h_.begin(), h_.begin() + n * m[i]), n, version);

            // Input data
            auto serials = RandomizeScalars(m[i]);
            auto randoms = RandomizeScalars(m[i]);

            std::vector<secp_primitives::Scalar> v_s;
            std::vector<secp_primitives::GroupElement> V;
            for (std::size_t j = 0; j < m[i]; ++j){
                v_s.emplace_back(j);
                V.push_back(g_gen * v_s.back() +  h_gen1 * randoms[j] + h_gen2 * serials[j]);
            }

            // Prove
            RangeProof proof;
            rangeProver.proof(v_s, serials, randoms, V, proof);
            V_batch.emplace_back(V);
            proof_batch.emplace_back(proof);
        }

        // Verify
        RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        BOOST_CHECK(rangeVerifier.verify(V_batch, V_batch, proof_batch));
    }
}

// A single out-of-range aggregated range proof
BOOST_AUTO_TEST_CASE(out_of_range_single_proof)
{
    // Parameters
    std::size_t n = 4;
    std::size_t m = 4;

    // Generators
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();
    auto g_ = RandomizeGroupElements(n * m);
    auto h_ = RandomizeGroupElements(n * m);

    // Input data
    auto randoms = RandomizeScalars(m);
    auto serials = RandomizeScalars(m);

    auto testF = [&] (std::vector<Scalar> const v_s, unsigned int version) {
        std::vector<GroupElement> V;
        for (std::size_t i = 0; i < m; ++i) {
            V.push_back(g_gen * v_s[i] +  h_gen1 * randoms[i] + h_gen2 * serials[i]);
        }

        lelantus::RangeProver rangeProver(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        lelantus::RangeProof proof;
        rangeProver.proof(v_s, serials, randoms, V, proof );

        lelantus::RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
    };

    for (auto version : test_versions)
    {
        // All values are out of range
        std::vector<Scalar> vs;
        for(std::size_t i = 0; i < m; ++i){
            vs.emplace_back((1 << n) + i);
        }
        testF(vs, version);

        // [0, 2 ^ n - 1]
        Scalar l(uint64_t(0));
        Scalar r((1 << n) - 1);

        // One value is out of range
        vs = {l, l + 1, r, r + 1};
        testF(vs, version);

        vs = {l - 1, l, r - 1, r};
        testF(vs, version);
    }
}

// A single aggreated range proof, with successively invalid proof elements
BOOST_AUTO_TEST_CASE(invalid_elements)
{
    // Parameters
    std::size_t n = 64;
    std::size_t m = 4;

    // Generators
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();
    auto g_ = RandomizeGroupElements(n * m);
    auto h_ = RandomizeGroupElements(n * m);

    // Inputs
    auto randoms = RandomizeScalars(m);
    auto serials = RandomizeScalars(m);

    // Set up valid proof
    std::vector<secp_primitives::Scalar> v_s;
    std::vector<secp_primitives::GroupElement> V;
    for(std::size_t i = 0; i < m; ++i){
        v_s.emplace_back(i);
        V.push_back(g_gen * v_s.back() +  h_gen1 * randoms[i] + h_gen2 * serials[i]);
    }

    for (auto version : test_versions)
    {
        // Initial correctness check
        RangeProver rangeProver(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        RangeProof proof;
        rangeProver.proof(v_s, serials, randoms, V, proof);
        RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        // Invalidate successive values and then restore them
        GroupElement group;
        Scalar scalar;

        group = GroupElement(proof.A);
        proof.A.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.A = GroupElement(group);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        group = GroupElement(proof.S);
        proof.S.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.S = GroupElement(group);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        group = GroupElement(proof.T1);
        proof.T1.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.T1 = GroupElement(group);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        group = GroupElement(proof.T2);
        proof.T2.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.T2 = GroupElement(group);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        for (std::size_t j = 0; j < proof.innerProductProof.L_.size(); j++) {
            group = GroupElement(proof.innerProductProof.L_[j]);
            proof.innerProductProof.L_[j].randomize();
            BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
            proof.innerProductProof.L_[j] = GroupElement(group);
            BOOST_CHECK(rangeVerifier.verify(V, V, proof));

            group = GroupElement(proof.innerProductProof.R_[j]);
            proof.innerProductProof.R_[j].randomize();
            BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
            proof.innerProductProof.R_[j] = GroupElement(group);
            BOOST_CHECK(rangeVerifier.verify(V, V, proof));
        }

        scalar = Scalar(proof.T_x1);
        proof.T_x1.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.T_x1 = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        scalar = Scalar(proof.T_x2);
        proof.T_x2.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.T_x2 = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        scalar = Scalar(proof.u);
        proof.u.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.u = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        scalar = Scalar(proof.innerProductProof.a_);
        proof.innerProductProof.a_.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.innerProductProof.a_ = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        scalar = Scalar(proof.innerProductProof.b_);
        proof.innerProductProof.b_.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.innerProductProof.b_ = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));

        scalar = Scalar(proof.innerProductProof.c_);
        proof.innerProductProof.c_.randomize();
        BOOST_CHECK(!rangeVerifier.verify(V, V, proof));
        proof.innerProductProof.c_ = Scalar(scalar);
        BOOST_CHECK(rangeVerifier.verify(V, V, proof));
    }
}

// A batch of range proofs, one of which is invalid
BOOST_AUTO_TEST_CASE(invalid_batch)
{
    // Parameters
    const std::size_t n = 64;
    const std::vector<std::size_t> m = {1,2,4,8};

    // Generators
    secp_primitives::GroupElement g_gen, h_gen1, h_gen2;
    g_gen.randomize();
    h_gen1.randomize();
    h_gen2.randomize();
    std::size_t max_m = *std::max_element(m.begin(), m.end());
    auto g_ = RandomizeGroupElements(n * max_m);
    auto h_ = RandomizeGroupElements(n * max_m);

    for (auto version : test_versions)
    {
        // Proofs
        std::vector<std::vector<GroupElement> > V_batch;
        V_batch.reserve(m.size());
        std::vector<RangeProof> proof_batch;
        proof_batch.reserve(m.size());
        for (std::size_t i = 0; i < m.size(); i++) {
            RangeProver rangeProver(g_gen, h_gen1, h_gen2, std::vector<GroupElement>(g_.begin(), g_.begin() + n * m[i]), std::vector<GroupElement>(h_.begin(), h_.begin() + n * m[i]), n, version);

            // Input data
            auto serials = RandomizeScalars(m[i]);
            auto randoms = RandomizeScalars(m[i]);

            std::vector<secp_primitives::Scalar> v_s;
            std::vector<secp_primitives::GroupElement> V;
            for (std::size_t j = 0; j < m[i]; ++j){
                v_s.emplace_back(j);
                V.push_back(g_gen * v_s.back() +  h_gen1 * randoms[j] + h_gen2 * serials[j]);
            }

            // Prove
            RangeProof proof;
            rangeProver.proof(v_s, serials, randoms, V, proof);
            V_batch.emplace_back(V);
            proof_batch.emplace_back(proof);
        }

        // Invalidate one of the proofs
        proof_batch[0].A.randomize();

        // Verify
        RangeVerifier rangeVerifier(g_gen, h_gen1, h_gen2, g_, h_, n, version);
        BOOST_CHECK(!rangeVerifier.verify(V_batch, V_batch, proof_batch));
    }
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/harmonic_map.h>
#include <cinolib/laplacian.h>
#include <Eigen/Sparse>

namespace cinolib
{

template<class M, class V, class E, class P>
CINO_INLINE
ScalarField harmonic_map(const AbstractMesh<M,V,E,P> & m,
                         const std::map<unsigned int,double> & bc,
                         const unsigned int                    n,
                         const int                     laplacian_mode,
                         const int                     solver)
{
    assert(n > 0);
    assert(bc.size() > 0);
    assert(laplacian_mode == COTANGENT || laplacian_mode == UNIFORM);
    assert(solver == SIMPLICIAL_LLT || solver == SIMPLICIAL_LDLT || solver == SparseLU || solver == BiCGSTAB);

    ScalarField f(m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode);
    Eigen::SparseMatrix<double> Ln = -L;
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(m.num_verts());

    for(unsigned int i=1; i<n; ++i) Ln  = Ln * (-L); // keep it PSD

    solve_square_system_with_bc(Ln, rhs, f, bc, solver);

    return f;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<vec3d> harmonic_map_3d(const AbstractMesh<M,V,E,P> & m,
                                   const std::map<unsigned int,vec3d>  & bc,
                                   const unsigned int                    n,
                                   const int                     laplacian_mode,
                                   const int                     solver)
{
    assert(n > 0);
    assert(bc.size() > 0);
    assert(laplacian_mode == COTANGENT || laplacian_mode == UNIFORM);
    assert(solver == SIMPLICIAL_LLT || solver == SIMPLICIAL_LDLT || solver == SparseLU || solver == BiCGSTAB);

    ScalarField f(3*m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode, 3);
    Eigen::SparseMatrix<double> Ln = -L;
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(3*m.num_verts());

    for(unsigned int i=1; i<n; ++i) Ln  = Ln * (-L); // keep it PSD

    unsigned int y_off = m.num_verts();
    unsigned int z_off = m.num_verts() + y_off;
    std::map<unsigned int,double> bc_1d;
    for(auto obj : bc)
    {
        unsigned int  vid = obj.first;
        vec3d pos = obj.second;
        bc_1d[      vid] = pos.x();
        bc_1d[y_off+vid] = pos.y();
        bc_1d[z_off+vid] = pos.z();
    }

    solve_square_system_with_bc(Ln, rhs, f, bc_1d, solver);

    std::vector<vec3d> res(m.num_verts());
    for(unsigned int vid=0; vid<m.num_verts(); ++vid)
    {
        res.at(vid) = vec3d(f[vid], f[y_off+vid], f[z_off+vid]);
    }

    return res;
}

}


// Copyright (C) INRIA 1999-2008
//
// This program is free software; you can redistribute it and/or modify it
// under the terms of the GNU General Public License version 2 as published
// by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//%
// @file ActuationModel/NoDynamics/TaskFunctionControl/ControlLaw.cpp
// @author Florence Billet
//
// Affiliation(s): INRIA, team BIPOP
//
// Email(s): Florence.Billet@inria.fr
//
// @brief Compute the voltage required to follow the reference trajectory
//
#ifdef _WIN32
#define SICONOS_EXPORT extern "C" __declspec(dllexport)
#else
#define SICONOS_EXPORT extern "C"
#endif
#include <iostream>
#include <string>

#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/ublas/io.hpp>

extern "C" {
#include "Trajectory.h"
#include "TaskFunctionDefinition.h"

#include "LagrangianModel.h"
#include "SomeDefinitions.h"
}

#include "KernelSomeDefinitions.hpp"
#include "Utils.hpp"

//#include "../../../LagrangianDynamics/Complete/util.hpp" //a mettre ailleurs un jour

SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
  int ndof;
  int contacts;
  char lagrangianModelName[20] = "";

  getLagrangianModelName(lagrangianModelName);
  getActiveDOFsize(&ndof);
  vector<int> activeDOF(ndof);
  if (ndof > 0)
    getActiveDOF(&(activeDOF[0]));
  else
    ndof = *NDOF;

  ///////////////////////////////////////////
  // Computation of the Lagrangian model
  ///////////////////////////////////////////

  matrix<double, column_major> M(*NDOF, *NDOF);
  vector<double> N(*NDOF);

  if (strcmp(lagrangianModelName, "Human36") == 0)
  {
    double L[31];
    double addl[84];
    double mass;
    GetModelMass(&mass);
    GetAnatomicalLengths(L);
    GetTag2JointLengths(addl);
    InertiaH36(&(M(0, 0)), q, L, addl, mass);
    NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
  }
  else
  {
    Inertia(&(M(0, 0)), q);
    NLEffects(&(N[0]), q, qdot);
  }

  //////////////////////////////////////////////////
  //Additionnal forces
  /////////////////////////////////////////////////

  vector<double> fAdditionnal(*NDOF);
  if (strcmp(lagrangianModelName, "PA10") == 0)
    Friction(&(fAdditionnal[0]), q, qdot);
  else if (strcmp(lagrangianModelName, "RX90") == 0)
    SpringForce(&(fAdditionnal[0]), q);
  else
    fAdditionnal.clear();

  ///////////////////////////////////////////////////
  // Limitation to the selected degrees of freedom
  ///////////////////////////////////////////////////
  reduceMatrix(M, activeDOF, activeDOF);
  N = reduceVector(N, activeDOF);
  fAdditionnal = reduceVector(fAdditionnal, activeDOF);

  //////////////////////////////////////////////////
  // Proportionnel-derivee in the task space
  //////////////////////////////////////////////////

  vector<double> sDesired(*NDOF);
  vector<double> sdotDesired(*NDOF);
  vector<double> sddotDesired(*NDOF);

  trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);

  //v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
  vector<double> TF(*NDOF);
  vector<double> h(*NDOF);
  matrix<double, column_major> H(*NDOF, *NDOF);

  TaskFunction(&(TF[0]), q);
  TaskJacobian(&(H(0, 0)), q);
  TaskNLEffects(&(h[0]), q, qdot);

  double Kp = 100.0;
  double Kv = 30.0;
  vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
  sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;

  /////////////////////////////////////////////////////////////
  // Generalized Torques to make the realisation of the command
  /////////////////////////////////////////////////////////////

  //qddot = inv(H)*v;
  vector<int> ipiv(*NDOF);
  boost::numeric::bindings::atlas::getrf(H, ipiv);
  boost::numeric::bindings::atlas::getri(H, ipiv);
  sddotDesired = prod(H, sddotDesired);

  //D = M*qddot(ActiveDOF) + N + FAdditionnal;
  sddotDesired = reduceVector(sddotDesired, activeDOF);

  N += prod(M, sddotDesired) + fAdditionnal;

  //nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
  //Torques = zeros(q);
  //Torques(ActiveDOF(1:nmot)) = D(1:nmot);
  for (int i = 0; i < *NDOF; i++)
    torques[i] = 0;
  expandVector(torques, N, activeDOF);

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_point_2d
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_POINT_2D

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>

/*< Somewhere, any legacy point struct is defined >*/
struct legacy_point
{
    double x, y;
};

BOOST_GEOMETRY_REGISTER_POINT_2D(legacy_point, double, cs::cartesian, x, y) /*< The magic: adapt it to Boost.Geometry Point Concept >*/

int main()
{
    legacy_point p1, p2;

    namespace bg = boost::geometry;

    /*< Any Boost.Geometry function can be used for legacy point now. Here: assign_values and distance >*/
    bg::assign_values(p1, 1, 1);
    bg::assign_values(p2, 2, 2);

    double d = bg::distance(p1, p2);

    std::cout << "Distance: " << d << std::endl;

    return 0;
}

//]

//[register_point_2d_output
/*`
Output:
[pre
Distance: 1.41421
]
*/
//]


/**
  * @file nmf_test.cpp
  * @author Wenhao Huang
  *
  * Test mlpackMain() of nmf_main.cpp
  *
  * mlpack is free software; you may redistribute it and/or modify it under the
  * terms of the 3-clause BSD license.  You should have received a copy of the
  * 3-clause BSD license along with mlpack.  If not, see
  * http://www.opensource.org/licenses/BSD-3-Clause for more information.
  */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST

static const std::string testName = "NonNegativeMatrixFactorization";

#include <mlpack/core.hpp>
#include <mlpack/methods/nmf/nmf_main.cpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"

#include <boost/test/unit_test.hpp>

using namespace mlpack;
using namespace arma;

struct NMFTestFixture
{
 public:
  NMFTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~NMFTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

static void ResetSettings()
{
  bindings::tests::CleanMemory();
  CLI::ClearSettings();
  CLI::RestoreSettings(testName);
}

BOOST_FIXTURE_TEST_SUITE(NMFMainTest, NMFTestFixture);

/**
 * Ensure the resulting matrices W, H have expected shape.
 * Multdist update rule (Default Case).
 */
BOOST_AUTO_TEST_CASE(NMFMultdistShapeTest)
{
  mat v = randu<mat>(8, 10);
  int r = 5;

  SetInputParam("update_rules", std::string("multdist"));
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  // Perform NMF.
  mlpackMain();

  // Get resulting matrices.
  const mat& w = CLI::GetParam<mat>("w");
  const mat& h = CLI::GetParam<mat>("h");

  // Check the shapes of W and H.
  BOOST_REQUIRE_EQUAL(w.n_rows, 8);
  BOOST_REQUIRE_EQUAL(w.n_cols, 5);
  BOOST_REQUIRE_EQUAL(h.n_rows, 5);
  BOOST_REQUIRE_EQUAL(h.n_cols, 10);
}

/**
 * Ensure the resulting matrices W, H have expected shape.
 * Multdiv update rule.
 */
BOOST_AUTO_TEST_CASE(NMFMultdivShapeTest)
{
  mat v = randu<mat>(8, 10);
  int r = 5;

  SetInputParam("update_rules", std::string("multdiv"));
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  // Perform NMF.
  mlpackMain();

  // Get resulting matrices.
  const mat& w = CLI::GetParam<mat>("w");
  const mat& h = CLI::GetParam<mat>("h");

  // Check the shapes of W and H.
  BOOST_REQUIRE_EQUAL(w.n_rows, 8);
  BOOST_REQUIRE_EQUAL(w.n_cols, 5);
  BOOST_REQUIRE_EQUAL(h.n_rows, 5);
  BOOST_REQUIRE_EQUAL(h.n_cols, 10);
}

/**
 * Ensure the resulting matrices W, H have expected shape.
 * Als update rule.
 */
BOOST_AUTO_TEST_CASE(NMFAlsShapeTest)
{
  mat v = randu<mat>(8, 10);
  int r = 5;

  SetInputParam("update_rules", std::string("als"));
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  // Perform NMF.
  mlpackMain();

  // Get resulting matrices.
  const mat& w = CLI::GetParam<mat>("w");
  const mat& h = CLI::GetParam<mat>("h");

  // Check the shapes of W and H.
  BOOST_REQUIRE_EQUAL(w.n_rows, 8);
  BOOST_REQUIRE_EQUAL(w.n_cols, 5);
  BOOST_REQUIRE_EQUAL(h.n_rows, 5);
  BOOST_REQUIRE_EQUAL(h.n_cols, 10);
}

/**
 * Ensure the rank is positive.
 */
BOOST_AUTO_TEST_CASE(NMFRankBoundTest)
{
  mat v = randu<mat>(10, 10);
  int r;

  // Rank should not be negative.
  r = -1;
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;

  // Rank should not be 0.
  r = 0;
  SetInputParam("rank", r);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure the max_iterations is non-negative.
 */
BOOST_AUTO_TEST_CASE(NMFMaxIterartionBoundTest)
{
  mat v = randu<mat>(10, 10);
  int r = 5;

  // max_iterations should be non-negative.
  SetInputParam("max_iterations", int(-1));
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure the update rule is one of
 * {"multdist", "multdiv", "als"}.
 */
BOOST_AUTO_TEST_CASE(NMFUpdateRuleTest)
{
  mat v = randu<mat>(10, 10);
  int r = 5;

  // Invalid update rule.
  SetInputParam("update_rules", std::string("invalid_rule"));
  SetInputParam("input", std::move(v));
  SetInputParam("rank", r);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure min_residue is used, by testing that
 * min_resude makes a difference to the program.
 */
BOOST_AUTO_TEST_CASE(NMFMinResidueTest)
{
  mat v = arma::randu(10, 10);
  mat initialW = arma::randu(10, 5);
  mat initialH = arma::randu(5, 10);
  int r = 5;

  // Set a larger min_residue.
  SetInputParam("min_residue", double(1));
  SetInputParam("input", v);
  SetInputParam("rank", r);
  SetInputParam("initial_w", initialW);
  SetInputParam("initial_h", initialH);

  mlpackMain();

  const mat w1 = CLI::GetParam<mat>("w");
  const mat h1 = CLI::GetParam<mat>("h");

  ResetSettings();

  // Set a smaller min_residue.
  SetInputParam("min_residue", double(1e-3));
  SetInputParam("input", v);
  SetInputParam("rank", r);
  SetInputParam("initial_w", initialW);
  SetInputParam("initial_h", initialH);

  mlpackMain();

  const mat w2 = CLI::GetParam<mat>("w");
  const mat h2 = CLI::GetParam<mat>("h");

  // The resulting matrices should be different.
  BOOST_REQUIRE_GT(arma::norm(w1 - w2), 1e-5);
  BOOST_REQUIRE_GT(arma::norm(h1 - h2), 1e-5);
}

/**
 * Ensure max_iterations is used, by testing that
 * max_iterations makes a difference to the program.
 */
BOOST_AUTO_TEST_CASE(NMFMaxIterationTest)
{
  mat v = arma::randu(10, 10);
  mat initialW = arma::randu(10, 5);
  mat initialH = arma::randu(5, 10);
  int r = 5;

  // Set a larger max_iterations.
  SetInputParam("max_iterations", int(100));
  // Remove the influence of min_residue.
  SetInputParam("min_residue", double(0));
  SetInputParam("input", v);
  SetInputParam("rank", r);
  SetInputParam("initial_w", initialW);
  SetInputParam("initial_h", initialH);

  mlpackMain();

  const mat w1 = CLI::GetParam<mat>("w");
  const mat h1 = CLI::GetParam<mat>("h");

  ResetSettings();

  // Set a smaller max_iterations.
  SetInputParam("max_iterations", int(5));
  // Remove the influence of min_residue.
  SetInputParam("min_residue", double(0));
  SetInputParam("input", v);
  SetInputParam("rank", r);
  SetInputParam("initial_w", initialW);
  SetInputParam("initial_h", initialH);

  mlpackMain();

  const mat w2 = CLI::GetParam<mat>("w");
  const mat h2 = CLI::GetParam<mat>("h");

  // The resulting matrices should be different.
  BOOST_REQUIRE_GT(arma::norm(w1 - w2), 1e-5);
  BOOST_REQUIRE_GT(arma::norm(h1 - h2), 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


//=======================================================================
// Copyright (C) 2012 Flavio De Lorenzi (fdlorenzi@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)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
using namespace boost;

int main()
{

    typedef adjacency_list< setS, vecS, bidirectionalS > graph_type;

    // Build graph1
    int num_vertices1 = 8;
    graph_type graph1(num_vertices1);
    add_edge(0, 6, graph1);
    add_edge(0, 7, graph1);
    add_edge(1, 5, graph1);
    add_edge(1, 7, graph1);
    add_edge(2, 4, graph1);
    add_edge(2, 5, graph1);
    add_edge(2, 6, graph1);
    add_edge(3, 4, graph1);

    // Build graph2
    int num_vertices2 = 9;
    graph_type graph2(num_vertices2);
    add_edge(0, 6, graph2);
    add_edge(0, 8, graph2);
    add_edge(1, 5, graph2);
    add_edge(1, 7, graph2);
    add_edge(2, 4, graph2);
    add_edge(2, 7, graph2);
    add_edge(2, 8, graph2);
    add_edge(3, 4, graph2);
    add_edge(3, 5, graph2);
    add_edge(3, 6, graph2);

    // Create callback to print mappings
    vf2_print_callback< graph_type, graph_type > callback(graph1, graph2);

    // Print out all subgraph isomorphism mappings between graph1 and graph2.
    // Vertices and edges are assumed to be always equivalent.
    vf2_subgraph_iso(graph1, graph2, callback);

    return 0;
}


#include "SMPSInput.hpp"
#include "CbcLagrangeSolver.hpp"
#include "CbcRecourseSolver.hpp"
#include "conicBundleDriver.hpp"
#include "combineScenarios.hpp"
#include <boost/scoped_ptr.hpp>

using namespace std;
using boost::scoped_ptr;

int main(int argc, char **argv) {

	MPI_Init(&argc, &argv);

	int mype;
	MPI_Comm_rank(MPI_COMM_WORLD,&mype);

	if (argc != 3) {
		if (mype == 0) printf("Usage: %s [SMPS root name] [scenarios per subproblem]\n",argv[0]);
		return 1;
	}

	string smpsrootname(argv[1]);
	int nper = atoi(argv[2]);

	SMPSInput input(smpsrootname+".cor",smpsrootname+".tim",smpsrootname+".sto");

	combinedInput in = combineScenarios(input,nper,true);
	conicBundleDriver<CbcLagrangeSolver,CbcRecourseSolver>(in);

	MPI_Finalize();

	return 0;

}


#include "Surface.h"
#include "Ray.h"
#include "Light.h"
#include "SurfaceList.h"
#include "Intersection.h"
#include "Scene.h"
#include "KD.h"

#include <algorithm>

#include <Eigen/Dense>

void calculate_Ld_Ls(const Ray& ray, const Ray& lightray, float I,
                     const Eigen::Vector3f& n, const Material& mat,
                     Color& Ld, Color& Ls) {
    auto v = -ray.d.normalized();
    auto l = lightray.d.normalized();
    auto h = (v+l).normalized();

    Ld = mat.kd * I * std::max(0.0f, n.dot(l));
    Ls = mat.ks * I * std::pow(std::max(0.0f, n.dot(h)), mat.sp);
}

Color Surface::shade(const Ray& ray, const Eigen::Vector3f& point, const Eigen::Vector3f& n,
                     const Light& light, const Scene& scene, int depth, bool shadows, bool kd) {
    Color La, Ld, Ls, Lm;

    La = mat.ka * light.intensity;

    float I = light.intensity;

    auto corrected_point = point + n*0.01f;
    auto lightray = Ray(corrected_point, light.pos - corrected_point);

    // Apply diffuse and specular shading, with shadows, if applicable
    if (shadows) {
        std::unique_ptr<Intersection> lighthit;
        if (kd)
            lighthit = std::unique_ptr<Intersection>(kd_intersect(ray, kd_tree, 0, mat).intersect(lightray));
        else
            lighthit = std::unique_ptr<Intersection>(scene.intersect(lightray));

        if (!lighthit)
            calculate_Ld_Ls(ray, lightray, I, n, mat, Ld, Ls);
    } else {
        calculate_Ld_Ls(ray, lightray, I, n, mat, Ld, Ls);
    }

    // Apply recursive raytracing, if applicable
    if (mat.a != 0.0f && depth != 0) {
        auto v = -ray.d.normalized();
        auto r = -v - 2*((-v).dot(n)*n);

        auto reflected_ray = Ray(corrected_point, r);
        auto hit = scene.intersect(reflected_ray);
        if (hit)
            Lm = scene.shade(reflected_ray, *hit, depth-1, shadows, kd);
    }

    return (La + Ld + Ls)*(1-mat.a) + (Lm)*(mat.a);
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 5, 5> test_double_5_5(const Eigen::Matrix<double, 5, 5> & M)
{
	return M;
}
void export_double_5_5()
{
	boost::python::def("test_double_5_5",test_double_5_5);
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_k0.hpp>
#include <eve/function/diff/cyl_bessel_k0.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_k0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_k0(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_k0(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_k0 on wide"
        , eve::test::simd::ieee_reals
         , eve::test::generate( eve::test::randoms(0.0, 0.5)
                              , eve::test::randoms(0.5, 1.5)
                              , eve::test::randoms(1.5, 500.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_k0 =  [](auto x) { return eve::cyl_bessel_k0(x); };
#if defined(__cpp_lib_math_special_functions)
  auto std__cyl_bessel_k0 =  [](auto x)->v_t { return std::cyl_bessel_k(v_t(0), x); };
#else
  auto std__cyl_bessel_k0 =  [](auto x)->v_t { return boost::math::cyl_bessel_k(v_t(0), x); };
#endif
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::zero(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::inf(eve::as<T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_k0(eve::zero(eve::as<T>())),  eve::inf(eve::as< T>()), 0);
  }
  TTS_IEEE_EQUAL(eve__cyl_bessel_k0(v_t(-1)), eve::nan(eve::as<v_t>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(500)), std__cyl_bessel_k0(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(10)), std__cyl_bessel_k0(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(5)),  std__cyl_bessel_k0(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(2)),  std__cyl_bessel_k0(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(1.5)),std__cyl_bessel_k0(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(0.5)),std__cyl_bessel_k0(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(0.05)),std__cyl_bessel_k0(v_t(0.05)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(1)),  std__cyl_bessel_k0(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(v_t(0)),  eve::inf(eve::as<v_t>()), 0.0);

  TTS_IEEE_EQUAL(eve__cyl_bessel_k0(T(-1)), eve::nan(eve::as<T>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(500)),  T(std__cyl_bessel_k0(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(10)) ,  T(std__cyl_bessel_k0( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(5))  ,  T(std__cyl_bessel_k0( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(2))  ,  T(std__cyl_bessel_k0( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(1.5)),  T(std__cyl_bessel_k0( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(0.5)),  T(std__cyl_bessel_k0( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(1))  ,  T(std__cyl_bessel_k0( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0( T(0))  , eve::inf(eve::as<T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_k0(a0), map(std__cyl_bessel_k0, a0), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_k0(a1), map(std__cyl_bessel_k0, a1), 10.0);
  TTS_RELATIVE_EQUAL(eve__cyl_bessel_k0(a2), map(std__cyl_bessel_k0, a2), 0.001);
};

EVE_TEST( "Check behavior of cyl_bessel_k0 on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_k0 =  [](auto x) { return eve::diff(eve::cyl_bessel_k0)(x); };
  auto std__diff_bessel_k0 =  [](auto x)->v_t { return boost::math::cyl_bessel_k_prime(0, x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_k0(a0),   map(std__diff_bessel_k0, a0)   , 1.0e-3);

};


/*
 * @Description: LIO key frame
 * @Author: Ren Qian
 * @Date: 2020-02-28 19:13:26
 */
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_

#include <Eigen/Dense>

#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp"

namespace lidar_localization {

struct KeyFrame {
public:
    double time = 0.0;

    // key frame ID:
    unsigned int index = 0;

    // a. position & orientation:
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    // b. velocity:
    struct {
      Eigen::Vector3f v = Eigen::Vector3f::Zero();
      Eigen::Vector3f w = Eigen::Vector3f::Zero();
    } vel;
    // c. bias:
    struct {
      // c.1. accelerometer:
      Eigen::Vector3f accel = Eigen::Vector3f::Zero();
      // c.2. gyroscope:
      Eigen::Vector3f gyro = Eigen::Vector3f::Zero();
    } bias;

    KeyFrame() {}

    explicit KeyFrame(const int vertex_id, const g2o::PRVAG &prvag) {
      // set time:
      time = prvag.time;
      // set seq. ID:
      index = vertex_id;
      // set state:
      pose.block<3, 1>(0, 3) = prvag.pos.cast<float>();
      pose.block<3, 3>(0, 0) = prvag.ori.matrix().cast<float>();
      vel.v = prvag.vel.cast<float>();
      bias.accel = prvag.b_a.cast<float>();
      bias.gyro = prvag.b_g.cast<float>();
    }

    Eigen::Quaternionf GetQuaternion() const;
    Eigen::Vector3f GetTranslation() const;
};

}

#endif


#include <crab/config.h>

#include "../common.hpp"
#include "../program_options.hpp"
#ifdef HAVE_LDD
#include <crab/domains/ldd/ldd.hpp>
#endif
#include <boost/optional.hpp>
#include <crab/domains/boxes.hpp>
using namespace std;
using namespace ikos;
using namespace crab::cfg_impl;
using namespace crab::domain_impl;
#ifdef HAVE_LDD
using namespace crab::domains::ldd;
#endif

#define RATIONALS

#ifdef RATIONALS
using number_t = q_number;
#else
using number_t = z_number;
#endif

using linear_constraint_t = linear_constraint<number_t, varname_t>;
using linear_expression_t = linear_expression<number_t, varname_t>;
using interval_t = interval<number_t>;

#ifdef HAVE_LDD

LddManager *create_ldd_man(size_t num_vars) {
  DdManager *cudd = Cudd_Init(0, 0, CUDD_UNIQUE_SLOTS, 127, 0);
#ifndef RATIONALS
  theory_t *theory = tvpi_create_boxz_theory(num_vars);
#else
  theory_t *theory = tvpi_create_box_theory(num_vars);
#endif

  LddManager *ldd = Ldd_Init(cudd, theory);

  const bool dvo = true;
  if (dvo)
    Cudd_AutodynEnable(cudd, CUDD_REORDER_GROUP_SIFT);
  return ldd;
}

void destroy_ldd_man(LddManager *ldd) {
  DdManager *cudd = NULL;
  theory_t *theory = NULL;
  if (ldd) {
    cudd = Ldd_GetCudd(ldd);
    theory = Ldd_GetTheory(ldd);
    Ldd_Quit(ldd);
  }

  if (theory)
    tvpi_destroy_theory(theory);
  if (cudd)
    Cudd_Quit(cudd);
}

void dump(LddNodePtr val, LddManager *man) {
  DdManager *cudd = Ldd_GetCudd(man);
  FILE *fp = Cudd_ReadStdout(cudd);
  Cudd_SetStdout(cudd, stderr);
  if (val.get() == Ldd_GetTrue(man))
    crab::outs() << "true\n";
  else if (val.get() == Ldd_GetFalse(man))
    crab::outs() << "false\n";
  else
    Ldd_PrintMinterm(man, val.get());
  Cudd_SetStdout(cudd, fp);
}

LddNodePtr top(LddManager *man) { return lddPtr(man, Ldd_GetTrue(man)); }

LddNodePtr bot(LddManager *man) { return lddPtr(man, Ldd_GetFalse(man)); }

bool isBot(LddManager *man, LddNodePtr v) { return &*v == Ldd_GetFalse(man); }

bool isTop(LddManager *man, LddNodePtr v) { return &*v == Ldd_GetTrue(man); }

LddNodePtr approx(LddManager *man, LddNodePtr v) {
  return lddPtr(man, Ldd_TermMinmaxApprox(man, &*v));
}

LddNodePtr meet(LddManager *man, LddNodePtr n1, LddNodePtr n2) {
  return lddPtr(man, Ldd_And(man, &*n1, &*n2));
}

LddNodePtr convex_join(LddManager *man, LddNodePtr n1, LddNodePtr n2) {
  return approx(man, lddPtr(man, Ldd_Or(man, &*n1, &*n2)));
}

LddNodePtr join(LddManager *man, LddNodePtr n1, LddNodePtr n2) {
  return lddPtr(man, Ldd_Or(man, &*n1, &*n2));
}

bool isLeq(LddManager *man, LddNodePtr n1, LddNodePtr n2) {
  return Ldd_TermLeq(man, &(*n1), &(*n2));
}

LddNodePtr widen(LddManager *man, LddNodePtr n1, LddNodePtr n2) {
  return lddPtr(man, Ldd_IntervalWiden(man, &*n1, &*n2));
}

int getVarId(varname_t v) { return v.index(); }

/** return term for variable v, neg for negation of variable */
linterm_t termForVal(LddManager *man, varname_t v, bool neg = false) {
  int varId = getVarId(v);
  int sgn = neg ? -1 : 1;
  linterm_t term =
      Ldd_GetTheory(man)->create_linterm_sparse_si(&varId, &sgn, 1);
  return term;
}

/** v := k, where k is a constant */
LddNodePtr assign(LddManager *man, LddNodePtr n, varname_t v, number_t k) {
  if (isBot(man, n))
    return n;

  q_number q(k);
  linterm_t t = termForVal(man, v);
  constant_t kkk = (constant_t)tvpi_create_cst(q.get_mpq_t());
  LddNodePtr newVal =
      lddPtr(man, Ldd_TermReplace(man, &(*n), t, NULL, NULL, kkk, kkk));
  Ldd_GetTheory(man)->destroy_cst(kkk);

  assert(!isBot(man, newVal));
  return newVal;
}

/** v := k, where k is an interval */
LddNodePtr assign(LddManager *man, LddNodePtr n, varname_t v, interval_t ival) {

  if (isBot(man, n))
    return n;

  constant_t kmin = NULL, kmax = NULL;

  if (boost::optional<number_t> l = ival.lb().number()) {
    q_number q(*l);
    kmin = (constant_t)tvpi_create_cst(q.get_mpq_t());
  }

  if (boost::optional<number_t> u = ival.ub().number()) {
    q_number q(*u);
    kmax = (constant_t)tvpi_create_cst(q.get_mpq_t());
  }

  linterm_t t = termForVal(man, v);
  LddNodePtr newVal =
      lddPtr(man, Ldd_TermReplace(man, &(*n), t, NULL, NULL, kmin, kmax));

  if (kmin)
    Ldd_GetTheory(man)->destroy_cst(kmin);
  if (kmax)
    Ldd_GetTheory(man)->destroy_cst(kmax);

  assert(!isBot(man, newVal));
  return newVal;
}

/** v := a * u + k, where a, k are constants and u variable */
LddNodePtr apply(LddManager *man, const LddNodePtr n, varname_t v, varname_t u,
                 number_t a, number_t k) {
  if (isTop(man, n) || isBot(man, n))
    return n;

  linterm_t t = termForVal(man, v);
  linterm_t r = termForVal(man, u);

  q_number qa(a);
  q_number qk(k);

  constant_t aaa = (constant_t)tvpi_create_cst(qa.get_mpq_t());
  constant_t kkk = (constant_t)tvpi_create_cst(qk.get_mpq_t());
  LddNodePtr newVal =
      lddPtr(man, Ldd_TermReplace(man, &(*n), t, r, aaa, kkk, kkk));
  Ldd_GetTheory(man)->destroy_cst(aaa);
  Ldd_GetTheory(man)->destroy_cst(kkk);

  assert(!isBot(man, newVal));
  return newVal;
}

/** v := u */
LddNodePtr assign(LddManager *man, const LddNodePtr n, varname_t v,
                  varname_t u) {
  return apply(man, n, v, u, 1, 0);
}
#endif

int main(int argc, char **argv) {
#ifdef HAVE_LDD

  bool stats_enabled = false;
  if (!crab_tests::parse_user_options(argc, argv, stats_enabled)) {
    return 0;
  }

  LddManager *man = create_ldd_man(3000);
  Ldd_SanityCheck(man);

  variable_factory_t vfac;
  varname_t x = vfac["x"];
  varname_t y = vfac["y"];

  {
    LddNodePtr s1 = top(man);
    LddNodePtr s11 = assign(man, s1, x, 5.5);
    LddNodePtr s111 = assign(man, s11, y, 8.3);

    LddNodePtr s2 = top(man);
    LddNodePtr s22 = assign(man, s2, x, 5.5);
    LddNodePtr s222 = assign(man, s22, y, 12.2);

    dump(s111, man);
    dump(s222, man);
    crab::outs() << "Join \n";
    LddNodePtr s3 = join(man, s111, s222);
    dump(s3, man);
    crab::outs() << "Widening \n";
    LddNodePtr s4 = widen(man, s111, s222);
    dump(s4, man);
  }

#if 1
  {
    // To reproduce an old bug with boxes.
    // Assertion failed: (level < (unsigned)
    // cuddI(unique,Cudd_Regular(E)->index)), function cuddUniqueInter, file
    // /Users/E30338/Repos/crab/build_crab2/ldd/src/ldd/cudd-2.4.2/cudd/cuddTable.c,
    // line 1143.

    using boxes_domain_t = boxes_domain<number_t, varname_t>;
    using var_t = typename boxes_domain_t::variable_t;

    boxes_domain_t s1;
    s1 += linear_constraint_t(var_t(x) >= number_t(0));
    s1 += linear_constraint_t(var_t(x) <= number_t(1, 4));
    s1 += linear_constraint_t(var_t(y) >= number_t(1, 4));
    s1 += linear_constraint_t(var_t(y) <= number_t(1, 2));
    boxes_domain_t s2;
    s2 += linear_constraint_t(var_t(x) == number_t(0));
    s2 += linear_constraint_t(var_t(y) == number_t(1, 2));

    boxes_domain_t s3 = s2 | s1;
    ///////////////////
    boxes_domain_t s4;
    s4 += linear_constraint_t(var_t(x) >= number_t(1, 4));
    s4 += linear_constraint_t(var_t(x) <= number_t(1));
    s4 += linear_constraint_t(var_t(y) >= number_t(1, 8));
    s4 += linear_constraint_t(var_t(y) <= number_t(1, 4));

    boxes_domain_t s5 = s4 | s2;
    //////
    crab::outs() << "Widening of \n" << s3;
    crab::outs() << "and\n" << s5 << " = \n";
    boxes_domain_t s6 = s3 || s5;
    crab::outs() << s6 << "\n";
  }
#endif

  // Ldd_NodeSanityCheck(man, &(*s4));

  // FIXME: seg fault here
  // destroy_ldd_man(man);
#endif
  return 0;
}


#include <NTL/lzz_pX.h>

#include <NTL/new.h>

NTL_START_IMPL

long divide(zz_pX& q, const zz_pX& a, const zz_pX& b)
{
   if (IsZero(b)) {
      if (IsZero(a)) {
         clear(q);
         return 1;
      }
      else
         return 0;
   }

   zz_pX lq, r;
   DivRem(lq, r, a, b);
   if (!IsZero(r)) return 0;
   q = lq;
   return 1;
}

long divide(const zz_pX& a, const zz_pX& b)
{
   if (IsZero(b)) return IsZero(a);
   zz_pX lq, r;
   DivRem(lq, r, a, b);
   if (!IsZero(r)) return 0;
   return 1;
}

void zz_pXMatrix::operator=(const zz_pXMatrix& M)
{
   elts[0][0] = M.elts[0][0];
   elts[0][1] = M.elts[0][1];
   elts[1][0] = M.elts[1][0];
   elts[1][1] = M.elts[1][1];
}

void RightShift(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   if (n < 0) {
      if (n < -NTL_MAX_LONG) Error("overflow in RightShift");
      LeftShift(x, a, -n);
      return;
   }

   long da = deg(a);
   long i;

   if (da < n) {
      clear(x);
      return;
   }

   if (&x != &a)
      x.rep.SetLength(da-n+1);

   for (i = 0; i <= da-n; i++)
      x.rep[i] = a.rep[i+n];

   if (&x == &a)
      x.rep.SetLength(da-n+1);

   x.normalize();
}

void LeftShift(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   if (n < 0) {
      if (n < -NTL_MAX_LONG)
         clear(x);
      else
         RightShift(x, a, -n);
      return;
   }

   if (NTL_OVERFLOW(n, 1, 0))
      Error("overflow in LeftShift");

   long m = a.rep.length();

   x.rep.SetLength(m+n);

   long i;
   for (i = m-1; i >= 0; i--)
      x.rep[i+n] = a.rep[i];

   for (i = 0; i < n; i++)
      clear(x.rep[i]);
}

void ShiftAdd(zz_pX& U, const zz_pX& V, long n)
// assumes input does not alias output
{
   if (IsZero(V))
      return;

   long du = deg(U);
   long dv = deg(V);

   long d = max(du, n+dv);

   U.rep.SetLength(d+1);
   long i;

   for (i = du+1; i <= d; i++)
      clear(U.rep[i]);

   for (i = 0; i <= dv; i++)
      add(U.rep[i+n], U.rep[i+n], V.rep[i]);

   U.normalize();
}

void ShiftSub(zz_pX& U, const zz_pX& V, long n)
// assumes input does not alias output
{
   if (IsZero(V))
      return;

   long du = deg(U);
   long dv = deg(V);

   long d = max(du, n+dv);

   U.rep.SetLength(d+1);
   long i;

   for (i = du+1; i <= d; i++)
      clear(U.rep[i]);

   for (i = 0; i <= dv; i++)
      sub(U.rep[i+n], U.rep[i+n], V.rep[i]);

   U.normalize();
}

void mul(zz_pX& U, zz_pX& V, const zz_pXMatrix& M)
// (U, V)^T = M*(U, V)^T
{
   long d = deg(U) - deg(M(1,1));
   long k = NextPowerOfTwo(d - 1);

   // When the GCD algorithm is run on polynomials of degree n, n-1,
   // where n is a power of two, then d-1 is likely to be a power of two.
   // It would be more natural to set k = NextPowerOfTwo(d+1), but this
   // would be much less efficient in this case.

   long n = (1L << k);
   long xx;
   zz_p a0, a1, b0, b1, c0, d0, u0, u1, v0, v1, nu0, nu1, nv0;
   zz_p t1, t2;

   if (n == d-1)
      xx = 1;
   else if (n == d)
      xx = 2;
   else
      xx = 3;

   switch (xx) {
   case 1:
      GetCoeff(a0, M(0,0), 0);
      GetCoeff(a1, M(0,0), 1);
      GetCoeff(b0, M(0,1), 0);
      GetCoeff(b1, M(0,1), 1);
      GetCoeff(c0, M(1,0), 0);
      GetCoeff(d0, M(1,1), 0);

      GetCoeff(u0, U, 0);
      GetCoeff(u1, U, 1);
      GetCoeff(v0, V, 0);
      GetCoeff(v1, V, 1);

      mul(t1, (a0), (u0));
      mul(t2, (b0), (v0));
      add(t1, t1, t2);
      nu0 = t1;

      mul(t1, (a1), (u0));
      mul(t2, (a0), (u1));
      add(t1, t1, t2);
      mul(t2, (b1), (v0));
      add(t1, t1, t2);
      mul(t2, (b0), (v1));
      add(t1, t1, t2);
      nu1 = t1;

      mul(t1, (c0), (u0));
      mul(t2, (d0), (v0));
      add (t1, t1, t2);
      nv0 = t1;

      break;

   case 2:
      GetCoeff(a0, M(0,0), 0);
      GetCoeff(b0, M(0,1), 0);

      GetCoeff(u0, U, 0);
      GetCoeff(v0, V, 0);

      mul(t1, (a0), (u0));
      mul(t2, (b0), (v0));
      add(t1, t1, t2);
      nu0 = t1;

      break;

   case 3:
      break;

   }

   fftRep RU(INIT_SIZE, k), RV(INIT_SIZE, k), R1(INIT_SIZE, k),
          R2(INIT_SIZE, k);

   TofftRep(RU, U, k);
   TofftRep(RV, V, k);

   TofftRep(R1, M(0,0), k);
   mul(R1, R1, RU);
   TofftRep(R2, M(0,1), k);
   mul(R2, R2, RV);
   add(R1, R1, R2);
   FromfftRep(U, R1, 0, d);

   TofftRep(R1, M(1,0), k);
   mul(R1, R1, RU);
   TofftRep(R2, M(1,1), k);
   mul(R2, R2, RV);
   add(R1, R1, R2);
   FromfftRep(V, R1, 0, d-1);

   // now fix-up results

   switch (xx) {
   case 1:
      GetCoeff(u0, U, 0);
      sub(u0, u0, nu0);
      SetCoeff(U, d-1, u0);
      SetCoeff(U, 0, nu0);

      GetCoeff(u1, U, 1);
      sub(u1, u1, nu1);
      SetCoeff(U, d, u1);
      SetCoeff(U, 1, nu1);

      GetCoeff(v0, V, 0);
      sub(v0, v0, nv0);
      SetCoeff(V, d-1, v0);
      SetCoeff(V, 0, nv0);

      break;

   case 2:
      GetCoeff(u0, U, 0);
      sub(u0, u0, nu0);
      SetCoeff(U, d, u0);
      SetCoeff(U, 0, nu0);

      break;

   }
}

void mul(zz_pXMatrix& A, zz_pXMatrix& B, zz_pXMatrix& C)
// A = B*C, B and C are destroyed
{
   long db = deg(B(1,1));
   long dc = deg(C(1,1));
   long da = db + dc;

   long k = NextPowerOfTwo(da+1);

   fftRep B00, B01, B10, B11, C0, C1, T1, T2;

   TofftRep(B00, B(0,0), k); B(0,0).kill();
   TofftRep(B01, B(0,1), k); B(0,1).kill();
   TofftRep(B10, B(1,0), k); B(1,0).kill();
   TofftRep(B11, B(1,1), k); B(1,1).kill();

   TofftRep(C0, C(0,0), k);  C(0,0).kill();
   TofftRep(C1, C(1,0), k);  C(1,0).kill();

   mul(T1, B00, C0);
   mul(T2, B01, C1);
   add(T1, T1, T2);
   FromfftRep(A(0,0), T1, 0, da);

   mul(T1, B10, C0);
   mul(T2, B11, C1);
   add(T1, T1, T2);
   FromfftRep(A(1,0), T1, 0, da);

   TofftRep(C0, C(0,1), k);  C(0,1).kill();
   TofftRep(C1, C(1,1), k);  C(1,1).kill();

   mul(T1, B00, C0);
   mul(T2, B01, C1);
   add(T1, T1, T2);
   FromfftRep(A(0,1), T1, 0, da);

   mul(T1, B10, C0);
   mul(T2, B11, C1);
   add(T1, T1, T2);
   FromfftRep(A(1,1), T1, 0, da);
}

void IterHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red)
{
   M_out(0,0).SetMaxLength(d_red);
   M_out(0,1).SetMaxLength(d_red);
   M_out(1,0).SetMaxLength(d_red);
   M_out(1,1).SetMaxLength(d_red);

   set(M_out(0,0));   clear(M_out(0,1));
   clear(M_out(1,0)); set(M_out(1,1));

   long goal = deg(U) - d_red;

   if (deg(V) <= goal)
      return;

   zz_pX Q, t(INIT_SIZE, d_red);

   while (deg(V) > goal) {
      PlainDivRem(Q, U, U, V);
      swap(U, V);

      mul(t, Q, M_out(1,0));
      sub(t, M_out(0,0), t);
      M_out(0,0) = M_out(1,0);
      M_out(1,0) = t;

      mul(t, Q, M_out(1,1));
      sub(t, M_out(0,1), t);
      M_out(0,1) = M_out(1,1);
      M_out(1,1) = t;
   }
}

void HalfGCD(zz_pXMatrix& M_out, const zz_pX& U, const zz_pX& V, long d_red)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long n = deg(U) - 2*d_red + 2;
   if (n < 0) n = 0;

   zz_pX U1, V1;

   RightShift(U1, U, n);
   RightShift(V1, V, n);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      IterHalfGCD(M_out, U1, V1, d_red);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U1, V1, d1);
   mul(U1, V1, M1);

   long d2 = deg(V1) - deg(U) + n + d_red;

   if (IsZero(V1) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   DivRem(Q, U1, U1, V1);
   swap(U1, V1);

   HalfGCD(M2, U1, V1, d2);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void XHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long du = deg(U);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      IterHalfGCD(M_out, U, V, d_red);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U, V, d1);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   DivRem(Q, U, U, V);
   swap(U, V);

   XHalfGCD(M2, U, V, d2);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void HalfGCD(zz_pX& U, zz_pX& V)
{
   long d_red = (deg(U)+1)/2;

   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      return;
   }

   long du = deg(U);

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U, V, d1);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      return;
   }

   M1(0,0).kill();
   M1(0,1).kill();
   M1(1,0).kill();
   M1(1,1).kill();

   zz_pX Q;

   DivRem(Q, U, U, V);
   swap(U, V);

   HalfGCD(M1, U, V, d2);

   mul(U, V, M1);
}

void GCD(zz_pX& d, const zz_pX& u, const zz_pX& v)
{
   zz_pX u1, v1;

   u1 = u;
   v1 = v;

   if (deg(u1) == deg(v1)) {
      if (IsZero(u1)) {
         clear(d);
         return;
      }

      rem(v1, v1, u1);
   }
   else if (deg(u1) < deg(v1)) {
      swap(u1, v1);
   }

   // deg(u1) > deg(v1)

   while (deg(u1) > NTL_zz_pX_GCD_CROSSOVER && !IsZero(v1)) {
      HalfGCD(u1, v1);

      if (!IsZero(v1)) {
         rem(u1, u1, v1);
         swap(u1, v1);
      }
   }

   PlainGCD(d, u1, v1);
}

void XGCD(zz_pX& d, zz_pX& s, zz_pX& t, const zz_pX& a, const zz_pX& b)
{
   zz_p w;

   if (IsZero(a) && IsZero(b)) {
      clear(d);
      set(s);
      clear(t);
      return;
   }

   zz_pX U, V, Q;

   U = a;
   V = b;

   long flag = 0;

   if (deg(U) == deg(V)) {
      DivRem(Q, U, U, V);
      swap(U, V);
      flag = 1;
   }
   else if (deg(U) < deg(V)) {
      swap(U, V);
      flag = 2;
   }

   zz_pXMatrix M;

   XHalfGCD(M, U, V, deg(U)+1);

   d = U;

   if (flag == 0) {
      s = M(0,0);
      t = M(0,1);
   }
   else if (flag == 1) {
      s = M(0,1);
      mul(t, Q, M(0,1));
      sub(t, M(0,0), t);
   }
   else {  /* flag == 2 */
      s = M(0,1);
      t = M(0,0);
   }

   // normalize

   inv(w, LeadCoeff(d));
   mul(d, d, w);
   mul(s, s, w);
   mul(t, t, w);
}

void IterBuild(zz_p* a, long n)
{
   long i, k;
   zz_p b, t;

   if (n <= 0) return;

   negate(a[0], a[0]);

   for (k = 1; k <= n-1; k++) {
      negate(b, a[k]);
      add(a[k], b, a[k-1]);
      for (i = k-1; i >= 1; i--) {
         mul(t, a[i], b);
         add(a[i], t, a[i-1]);
      }
      mul(a[0], a[0], b);
   }
}

void mul(zz_p* x, const zz_p* a, const zz_p* b, long n)
{
   zz_p t, accum;

   long i, j, jmin, jmax;

   long d = 2*n-1;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-(n-1));
      jmax = min(n-1, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
         mul(t, (a[j]), (b[i-j]));
         add(accum, accum, t);
      }
      if (i >= n) {
         add(accum, accum, (a[i-n]));
         add(accum, accum, (b[i-n]));
      }

      x[i] = accum;
   }
}

void BuildFromRoots(zz_pX& x, const vec_zz_p& a)
{
   long n = a.length();

   if (n == 0) {
      set(x);
      return;
   }

   long k0 = NextPowerOfTwo(NTL_zz_pX_MUL_CROSSOVER)-1;
   long crossover = 1L << k0;

   if (n <= NTL_zz_pX_MUL_CROSSOVER) {
      x.rep.SetMaxLength(n+1);
      x.rep = a;
      IterBuild(&x.rep[0], n);
      x.rep.SetLength(n+1);
      SetCoeff(x, n);
      return;
   }

   long k = NextPowerOfTwo(n);

   long m = 1L << k;
   long i, j;
   long l, width;

   zz_pX b(INIT_SIZE, m+1);

   b.rep = a;
   b.rep.SetLength(m+1);
   for (i = n; i < m; i++)
      clear(b.rep[i]);

   set(b.rep[m]);

   fftRep R1(INIT_SIZE, k), R2(INIT_SIZE, k);

   zz_p t1, one;
   set(one);

   vec_zz_p G(INIT_SIZE, crossover), H(INIT_SIZE, crossover);
   zz_p *g = G.elts();
   zz_p *h = H.elts();
   zz_p *tmp;

   for (i = 0; i < m; i+= crossover) {
      for (j = 0; j < crossover; j++)
         negate(g[j], b.rep[i+j]);

      if (k0 > 0) {
         for (j = 0; j < crossover; j+=2) {
            mul(t1, g[j], g[j+1]);
            add(g[j+1], g[j], g[j+1]);
            g[j] = t1;
         }
      }

      for (l = 1; l < k0; l++) {
         width = 1L << l;

         for (j = 0; j < crossover; j += 2*width)
            mul(&h[j], &g[j], &g[j+width], width);

         tmp = g; g = h; h = tmp;
      }

      for (j = 0; j < crossover; j++)
         b.rep[i+j] = g[j];
   }

   for (l = k0; l < k; l++) {
      width = 1L << l;
      for (i = 0; i < m; i += 2*width) {
         t1 = b.rep[i+width];
         set(b.rep[i+width]);
         TofftRep(R1, b, l+1, i, i+width);
         b.rep[i+width] = t1;
         t1 = b.rep[i+2*width];
         set(b.rep[i+2*width]);
         TofftRep(R2, b, l+1, i+width, i+2*width);
         b.rep[i+2*width] = t1;
         mul(R1, R1, R2);
         FromfftRep(&b.rep[i], R1, 0, 2*width-1);
         sub(b.rep[i], b.rep[i], one);
      }
   }

   x.rep.SetLength(n+1);
   long delta = m-n;
   for (i = 0; i <= n; i++)
     x.rep[i] = b.rep[i+delta];

   // no need to normalize
}

void eval(zz_p& b, const zz_pX& f, zz_p a)
// does a Horner evaluation
{
   zz_p acc;
   long i;

   clear(acc);
   for (i = deg(f); i >= 0; i--) {
      mul(acc, acc, a);
      add(acc, acc, f.rep[i]);
   }

   b = acc;
}

void eval(vec_zz_p& b, const zz_pX& f, const vec_zz_p& a)
// naive algorithm:  repeats Horner
{
   if (&b == &f.rep) {
      vec_zz_p bb;
      eval(bb, f, a);
      b = bb;
      return;
   }

   long m = a.length();
   b.SetLength(m);
   long i;
   for (i = 0; i < m; i++)
      eval(b[i], f, a[i]);
}

void interpolate(zz_pX& f, const vec_zz_p& a, const vec_zz_p& b)
{
   long m = a.length();
   if (b.length() != m) Error("interpolate: vector length mismatch");

   if (m == 0) {
      clear(f);
      return;
   }

   vec_zz_p prod;
   prod = a;

   zz_p t1, t2;

   long k, i;

   vec_zz_p res;
   res.SetLength(m);

   for (k = 0; k < m; k++) {

      const zz_p& aa = a[k];

      set(t1);
      for (i = k-1; i >= 0; i--) {
         mul(t1, t1, aa);
         add(t1, t1, prod[i]);
      }

      clear(t2);
      for (i = k-1; i >= 0; i--) {
         mul(t2, t2, aa);
         add(t2, t2, res[i]);
      }

      inv(t1, t1);
      sub(t2, b[k], t2);
      mul(t1, t1, t2);

      for (i = 0; i < k; i++) {
         mul(t2, prod[i], t1);
         add(res[i], res[i], t2);
      }

      res[k] = t1;

      if (k < m-1) {
         if (k == 0)
            negate(prod[0], prod[0]);
         else {
            negate(t1, a[k]);
            add(prod[k], t1, prod[k-1]);
            for (i = k-1; i >= 1; i--) {
               mul(t2, prod[i], t1);
               add(prod[i], t2, prod[i-1]);
            }
            mul(prod[0], prod[0], t1);
         }
      }
   }

   while (m > 0 && IsZero(res[m-1])) m--;
   res.SetLength(m);
   f.rep = res;
}

void InnerProduct(zz_pX& x, const vec_zz_p& v, long low, long high,
                   const vec_zz_pX& H, long n, vec_zz_p& t)
{
   zz_p s;
   long i, j;

   zz_p *tp = t.elts();

   for (j = 0; j < n; j++)
      clear(tp[j]);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   high = min(high, v.length()-1);
   for (i = low; i <= high; i++) {
      const vec_zz_p& h = H[i-low].rep;
      long m = h.length();
      zz_p w = (v[i]);

      long W = rep(w);
      mulmod_precon_t Wpinv = PrepMulModPrecon(W, p, pinv); // ((double) W)*pinv;
      const zz_p *hp = h.elts();

      for (j = 0; j < m; j++) {
         long S = MulModPrecon(rep(hp[j]), W, p, Wpinv);
         S = AddMod(S, rep(tp[j]), p);
         tp[j].LoopHole() = S;
      }
   }

   x.rep = t;
   x.normalize();
}

void CompMod(zz_pX& x, const zz_pX& g, const zz_pXArgument& A,
             const zz_pXModulus& F)
{
   if (deg(g) <= 0) {
      x = g;
      return;
   }

   zz_pX s, t;
   vec_zz_p scratch(INIT_SIZE, F.n);

   long m = A.H.length() - 1;
   long l = ((g.rep.length()+m-1)/m) - 1;

   zz_pXMultiplier M;
   build(M, A.H[m], F);

   InnerProduct(t, g.rep, l*m, l*m + m - 1, A.H, F.n, scratch);
   for (long i = l-1; i >= 0; i--) {
      InnerProduct(s, g.rep, i*m, i*m + m - 1, A.H, F.n, scratch);
      MulMod(t, t, M, F);
      add(t, t, s);
   }

   x = t;
}

void build(zz_pXArgument& A, const zz_pX& h, const zz_pXModulus& F, long m)
{
   if (m <= 0 || deg(h) >= F.n) Error("build: bad args");

   if (m > F.n) m = F.n;

   long i;

   if (zz_pXArgBound > 0) {
      double sz = 1;
      sz = sz*F.n;
      sz = sz+6;
      sz = sz*(sizeof (long));
      sz = sz/1024;
      m = min(m, long(zz_pXArgBound/sz));
      m = max(m, 1);
   }

   zz_pXMultiplier M;

   build(M, h, F);

   A.H.SetLength(m+1);

   set(A.H[0]);
   A.H[1] = h;
   for (i = 2; i <= m; i++)
      MulMod(A.H[i], A.H[i-1], M, F);
}

long zz_pXArgBound = 0;

void CompMod(zz_pX& x, const zz_pX& g, const zz_pX& h, const zz_pXModulus& F)
   // x = g(h) mod f
{
   long m = SqrRoot(g.rep.length());

   if (m == 0) {
      clear(x);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   CompMod(x, g, A, F);
}

void Comp2Mod(zz_pX& x1, zz_pX& x2, const zz_pX& g1, const zz_pX& g2,
              const zz_pX& h, const zz_pXModulus& F)

{
   long m = SqrRoot(g1.rep.length() + g2.rep.length());

   if (m == 0) {
      clear(x1);
      clear(x2);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   zz_pX xx1, xx2;

   CompMod(xx1, g1, A, F);
   CompMod(xx2, g2, A, F);

   x1 = xx1;
   x2 = xx2;
}

void Comp3Mod(zz_pX& x1, zz_pX& x2, zz_pX& x3,
              const zz_pX& g1, const zz_pX& g2, const zz_pX& g3,
              const zz_pX& h, const zz_pXModulus& F)

{
   long m = SqrRoot(g1.rep.length() + g2.rep.length() + g3.rep.length());

   if (m == 0) {
      clear(x1);
      clear(x2);
      clear(x3);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   zz_pX xx1, xx2, xx3;

   CompMod(xx1, g1, A, F);
   CompMod(xx2, g2, A, F);
   CompMod(xx3, g3, A, F);

   x1 = xx1;
   x2 = xx2;
   x3 = xx3;
}

static void StripZeroes(vec_zz_p& x)
{
   long n = x.length();
   while (n > 0 && IsZero(x[n-1]))
      n--;
   x.SetLength(n);
}

void PlainUpdateMap(vec_zz_p& xx, const vec_zz_p& a,
                    const zz_pX& b, const zz_pX& f)
{
   long n = deg(f);
   long i, m;

   if (IsZero(b)) {
      xx.SetLength(0);
      return;
   }

   m = n-1 - deg(b);

   vec_zz_p x(INIT_SIZE, n);

   for (i = 0; i <= m; i++)
      InnerProduct(x[i], a, b.rep, i);

   if (deg(b) != 0) {
      zz_pX c(INIT_SIZE, n);
      LeftShift(c, b, m);

      for (i = m+1; i < n; i++) {
         MulByXMod(c, c, f);
         InnerProduct(x[i], a, c.rep);
      }
   }

   xx = x;
}

void UpdateMap(vec_zz_p& x, const vec_zz_p& aa,
               const zz_pXMultiplier& B, const zz_pXModulus& F)
{
   long n = F.n;

   vec_zz_p a;
   a = aa;
   StripZeroes(a);

   if (a.length() > n) Error("UpdateMap: bad args");
   long i;

   if (!B.UseFFT) {
      PlainUpdateMap(x, a, B.b, F.f);
      StripZeroes(x);
      return;
   }

   fftRep R1(INIT_SIZE, F.k), R2(INIT_SIZE, F.l);
   vec_zz_p V1(INIT_SIZE, n);

   RevTofftRep(R1, a, F.k, 0, a.length()-1, 0);
   mul(R2, R1, F.FRep);
   RevFromfftRep(V1, R2, 0, n-2);
   for (i = 0; i <= n-2; i++)  negate(V1[i], V1[i]);
   RevTofftRep(R2, V1, F.l, 0, n-2, n-1);
   mul(R2, R2, B.B1);
   mul(R1, R1, B.B2);

   AddExpand(R2, R1);
   RevFromfftRep(x, R2, 0, n-1);
   StripZeroes(x);
}

void ProjectPowers(vec_zz_p& x, const vec_zz_p& a, long k,
                   const zz_pXArgument& H, const zz_pXModulus& F)

{
   long n = F.n;

   if (a.length() > n || k < 0 || NTL_OVERFLOW(k, 1, 0))
      Error("ProjectPowers: bad args");

   long m = H.H.length()-1;
   long l = (k+m-1)/m - 1;

   zz_pXMultiplier M;
   build(M, H.H[m], F);

   vec_zz_p s(INIT_SIZE, n);
   s = a;
   StripZeroes(s);

   x.SetLength(k);

   for (long i = 0; i <= l; i++) {
      long m1 = min(m, k-i*m);
      zz_p* w = &x[i*m];
      for (long j = 0; j < m1; j++)
         InnerProduct(w[j], H.H[j].rep, s);
      if (i < l)
         UpdateMap(s, s, M, F);
   }
}

void ProjectPowers(vec_zz_p& x, const vec_zz_p& a, long k,
                   const zz_pX& h, const zz_pXModulus& F)

{
   if (a.length() > F.n || k < 0) Error("ProjectPowers: bad args");

   if (k == 0) {
      x.SetLength(0);
      return;
   }

   long m = SqrRoot(k);

   zz_pXArgument H;

   build(H, h, F, m);
   ProjectPowers(x, a, k, H, F);
}

void BerlekampMassey(zz_pX& h, const vec_zz_p& a, long m)
{
   zz_pX Lambda, Sigma, Temp;
   long L;
   zz_p Delta, Delta1, t1;
   long shamt;

   // cerr << "*** " << m << "\n";

   Lambda.SetMaxLength(m+1);
   Sigma.SetMaxLength(m+1);
   Temp.SetMaxLength(m+1);

   L = 0;
   set(Lambda);
   clear(Sigma);
   set(Delta);
   shamt = 0;

   long i, r, dl;

   for (r = 1; r <= 2*m; r++) {
      // cerr << r << "--";
      clear(Delta1);
      dl = deg(Lambda);
      for (i = 0; i <= dl; i++) {
         mul(t1, Lambda.rep[i], a[r-i-1]);
         add(Delta1, Delta1, t1);
      }

      if (IsZero(Delta1)) {
         shamt++;
         // cerr << "case 1: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
      else if (2*L < r) {
         div(t1, Delta1, Delta);
         mul(Temp, Sigma, t1);
         Sigma = Lambda;
         ShiftSub(Lambda, Temp, shamt+1);
         shamt = 0;
         L = r-L;
         Delta = Delta1;
         // cerr << "case 2: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
      else {
         shamt++;
         div(t1, Delta1, Delta);
         mul(Temp, Sigma, t1);
         ShiftSub(Lambda, Temp, shamt);
         // cerr << "case 3: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
   }

   // cerr << "finished: " << L << " " << deg(Lambda) << "\n";

   dl = deg(Lambda);
   h.rep.SetLength(L + 1);

   for (i = 0; i < L - dl; i++)
      clear(h.rep[i]);

   for (i = L - dl; i <= L; i++)
      h.rep[i] = Lambda.rep[L - i];
}

void GCDMinPolySeq(zz_pX& h, const vec_zz_p& x, long m)
{
   long i;
   zz_pX a, b;
   zz_pXMatrix M;
   zz_p t;

   a.rep.SetLength(2*m);
   for (i = 0; i < 2*m; i++) a.rep[i] = x[2*m-1-i];
   a.normalize();

   SetCoeff(b, 2*m);

   HalfGCD(M, b, a, m+1);

   /* make monic */

   inv(t, LeadCoeff(M(1,1)));
   mul(h, M(1,1), t);
}

void MinPolySeq(zz_pX& h, const vec_zz_p& a, long m)
{
   if (m < 0 || NTL_OVERFLOW(m, 1, 0)) Error("MinPoly: bad args");
   if (a.length() < 2*m) Error("MinPoly: sequence too short");

   if (m > NTL_zz_pX_BERMASS_CROSSOVER)
      GCDMinPolySeq(h, a, m);
   else
      BerlekampMassey(h, a, m);
}

void DoMinPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m,
               const vec_zz_p& R)
{
   vec_zz_p x;

   ProjectPowers(x, R, 2*m, g, F);
   MinPolySeq(h, x, m);
}

void ProbMinPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m)
{
   long n = F.n;
   if (m < 1 || m > n) Error("ProbMinPoly: bad args");

   long i;
   vec_zz_p R(INIT_SIZE, n);

   for (i = 0; i < n; i++) random(R[i]);
   DoMinPolyMod(h, g, F, m, R);
}

void MinPolyMod(zz_pX& hh, const zz_pX& g, const zz_pXModulus& F, long m)
{
   zz_pX h, h1;
   long n = F.n;
   if (m < 1 || m > n) Error("MinPoly: bad args");

   /* probabilistically compute min-poly */

   ProbMinPolyMod(h, g, F, m);
   if (deg(h) == m) { hh = h; return; }
   CompMod(h1, h, g, F);
   if (IsZero(h1)) { hh = h; return; }

   /* not completely successful...must iterate */

   long i;

   zz_pX h2, h3;
   zz_pXMultiplier H1;
   vec_zz_p R(INIT_SIZE, n);

   for (;;) {
      R.SetLength(n);
      for (i = 0; i < n; i++) random(R[i]);
      build(H1, h1, F);
      UpdateMap(R, R, H1, F);
      DoMinPolyMod(h2, g, F, m-deg(h), R);

      mul(h, h, h2);
      if (deg(h) == m) { hh = h; return; }
      CompMod(h3, h2, g, F);
      MulMod(h1, h3, H1, F);
      if (IsZero(h1)) { hh = h; return; }
   }
}

void IrredPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m)
{
   vec_zz_p R(INIT_SIZE, 1);
   if (m < 1 || m > F.n) Error("IrredPoly: bad args");

   set(R[0]);
   DoMinPolyMod(h, g, F, m, R);
}

void diff(zz_pX& x, const zz_pX& a)
{
   long n = deg(a);
   long i;

   if (n <= 0) {
      clear(x);
      return;
   }

   if (&x != &a)
      x.rep.SetLength(n);

   for (i = 0; i <= n-1; i++) {
      mul(x.rep[i], a.rep[i+1], i+1);
   }

   if (&x == &a)
      x.rep.SetLength(n);

   x.normalize();
}

void MakeMonic(zz_pX& x)
{
   if (IsZero(x))
      return;

   if (IsOne(LeadCoeff(x)))
      return;

   zz_p t;

   inv(t, LeadCoeff(x));
   mul(x, x, t);
}

void PlainMulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   zz_pX y;
   mul(y, a, b);
   trunc(x, y, n);
}

void FFTMulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   if (IsZero(a) || IsZero(b)) {
      clear(x);
      return;
   }

   long d = deg(a) + deg(b);
   if (n > d + 1)
      n = d + 1;

   long k = NextPowerOfTwo(d + 1);
   fftRep R1(INIT_SIZE, k), R2(INIT_SIZE, k);

   TofftRep(R1, a, k);
   TofftRep(R2, b, k);
   mul(R1, R1, R2);
   FromfftRep(x, R1, 0, n-1);
}

void MulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   if (n < 0) Error("MulTrunc: bad args");

   if (deg(a) <= NTL_zz_pX_MUL_CROSSOVER || deg(b) <= NTL_zz_pX_MUL_CROSSOVER)
      PlainMulTrunc(x, a, b, n);
   else
      FFTMulTrunc(x, a, b, n);
}

void PlainSqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   zz_pX y;
   sqr(y, a);
   trunc(x, y, n);
}

void FFTSqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   long d = 2*deg(a);
   if (n > d + 1)
      n = d + 1;

   long k = NextPowerOfTwo(d + 1);
   fftRep R1(INIT_SIZE, k);

   TofftRep(R1, a, k);
   mul(R1, R1, R1);
   FromfftRep(x, R1, 0, n-1);
}

void SqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   if (n < 0) Error("SqrTrunc: bad args");

   if (deg(a) <= NTL_zz_pX_MUL_CROSSOVER)
      PlainSqrTrunc(x, a, n);
   else
      FFTSqrTrunc(x, a, n);
}

void FastTraceVec(vec_zz_p& S, const zz_pX& f)
{
   long n = deg(f);

   if (n <= 0)
      Error("FastTraceVec: bad args");

   if (n == 0) {
      S.SetLength(0);
      return;
   }

   if (n == 1) {
      S.SetLength(1);
      set(S[0]);
      return;
   }

   long i;
   zz_pX f1;

   f1.rep.SetLength(n-1);
   for (i = 0; i <= n-2; i++)
      f1.rep[i] = f.rep[n-i];
   f1.normalize();

   zz_pX f2;
   f2.rep.SetLength(n-1);
   for (i = 0; i <= n-2; i++)
      mul(f2.rep[i], f.rep[n-1-i], i+1);
   f2.normalize();

   zz_pX f3;
   InvTrunc(f3, f1, n-1);
   MulTrunc(f3, f3, f2, n-1);

   S.SetLength(n);

   S[0] = n;
   for (i = 1; i < n; i++)
      negate(S[i], coeff(f3, i-1));
}

void PlainTraceVec(vec_zz_p& S, const zz_pX& ff)
{
   if (deg(ff) <= 0)
      Error("TraceVec: bad args");

   zz_pX f;
   f = ff;

   MakeMonic(f);

   long n = deg(f);

   S.SetLength(n);

   if (n == 0)
      return;

   long k, i;
   zz_p acc, t;

   const zz_p *fp = f.rep.elts();;
   zz_p *sp = S.elts();

   sp[0] = n;

   for (k = 1; k < n; k++) {
      mul(acc, fp[n-k], k);

      for (i = 1; i < k; i++) {
         mul(t, fp[n-i], rep(sp[k-i]));
         add(acc, acc, t);
      }

      negate(sp[k], acc);
   }
}

void TraceVec(vec_zz_p& S, const zz_pX& f)
{
   if (deg(f) <= NTL_zz_pX_TRACE_CROSSOVER)
      PlainTraceVec(S, f);
   else
      FastTraceVec(S, f);
}

void ComputeTraceVec(const zz_pXModulus& F)
{
   vec_zz_p& S = *((vec_zz_p *) &F.tracevec);

   if (S.length() > 0)
      return;

   if (!F.UseFFT) {
      PlainTraceVec(S, F.f);
      return;
   }

   long i;
   long n = F.n;

   fftRep R;
   zz_pX P, g;

   g.rep.SetLength(n-1);
   for (i = 1; i < n; i++)
      mul(g.rep[n-i-1], F.f.rep[n-i], i);
   g.normalize();

   TofftRep(R, g, F.l);
   mul(R, R, F.HRep);
   FromfftRep(P, R, n-2, 2*n-4);

   S.SetLength(n);

   S[0] = n;
   for (i = 1; i < n; i++)
      negate(S[i], coeff(P, n-1-i));
}

void TraceMod(zz_p& x, const zz_pX& a, const zz_pXModulus& F)
{
   long n = F.n;

   if (deg(a) >= n)
      Error("trace: bad args");

   if (F.tracevec.length() == 0)
      ComputeTraceVec(F);

   InnerProduct(x, a.rep, F.tracevec);
}

void TraceMod(zz_p& x, const zz_pX& a, const zz_pX& f)
{
   if (deg(a) >= deg(f) || deg(f) <= 0)
      Error("trace: bad args");

   project(x, TraceVec(f), a);
}

void PlainResultant(zz_p& rres, const zz_pX& a, const zz_pX& b)
{
   zz_p res;

   if (IsZero(a) || IsZero(b))
      clear(res);
   else if (deg(a) == 0 && deg(b) == 0)
      set(res);
   else {
      long d0, d1, d2;
      zz_p lc;
      set(res);

      long n = max(deg(a),deg(b)) + 1;
      zz_pX u(INIT_SIZE, n), v(INIT_SIZE, n);

      u = a;
      v = b;

      for (;;) {
         d0 = deg(u);
         d1 = deg(v);
         lc = LeadCoeff(v);

         PlainRem(u, u, v);
         swap(u, v);

         d2 = deg(v);
         if (d2 >= 0) {
            power(lc, lc, d0-d2);
            mul(res, res, lc);
            if (d0 & d1 & 1) negate(res, res);
         }
         else {
            if (d1 == 0) {
               power(lc, lc, d0);
               mul(res, res, lc);
            }
            else
               clear(res);

            break;
         }
      }
   }

   rres = res;
}

void ResIterHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red,
                    vec_zz_p& cvec, vec_long& dvec)
{
   M_out(0,0).SetMaxLength(d_red);
   M_out(0,1).SetMaxLength(d_red);
   M_out(1,0).SetMaxLength(d_red);
   M_out(1,1).SetMaxLength(d_red);

   set(M_out(0,0));   clear(M_out(0,1));
   clear(M_out(1,0)); set(M_out(1,1));

   long goal = deg(U) - d_red;

   if (deg(V) <= goal)
      return;

   zz_pX Q, t(INIT_SIZE, d_red);

   while (deg(V) > goal) {
      append(cvec, LeadCoeff(V));
      append(dvec, dvec[dvec.length()-1]-deg(U)+deg(V));
      PlainDivRem(Q, U, U, V);
      swap(U, V);

      mul(t, Q, M_out(1,0));
      sub(t, M_out(0,0), t);
      M_out(0,0) = M_out(1,0);
      M_out(1,0) = t;

      mul(t, Q, M_out(1,1));
      sub(t, M_out(0,1), t);
      M_out(0,1) = M_out(1,1);
      M_out(1,1) = t;
   }
}

void ResHalfGCD(zz_pXMatrix& M_out, const zz_pX& U, const zz_pX& V, long d_red,
                vec_zz_p& cvec, vec_long& dvec)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long n = deg(U) - 2*d_red + 2;
   if (n < 0) n = 0;

   zz_pX U1, V1;

   RightShift(U1, U, n);
   RightShift(V1, V, n);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      ResIterHalfGCD(M_out, U1, V1, d_red, cvec, dvec);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   ResHalfGCD(M1, U1, V1, d1, cvec, dvec);
   mul(U1, V1, M1);

   long d2 = deg(V1) - deg(U) + n + d_red;

   if (IsZero(V1) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   append(cvec, LeadCoeff(V1));
   append(dvec, dvec[dvec.length()-1]-deg(U1)+deg(V1));
   DivRem(Q, U1, U1, V1);
   swap(U1, V1);

   ResHalfGCD(M2, U1, V1, d2, cvec, dvec);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void ResHalfGCD(zz_pX& U, zz_pX& V, vec_zz_p& cvec, vec_long& dvec)
{
   long d_red = (deg(U)+1)/2;

   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      return;
   }

   long du = deg(U);

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   ResHalfGCD(M1, U, V, d1, cvec, dvec);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      return;
   }

   M1(0,0).kill();
   M1(0,1).kill();
   M1(1,0).kill();
   M1(1,1).kill();

   zz_pX Q;

   append(cvec, LeadCoeff(V));
   append(dvec, dvec[dvec.length()-1]-deg(U)+deg(V));
   DivRem(Q, U, U, V);
   swap(U, V);

   ResHalfGCD(M1, U, V, d2, cvec, dvec);

   mul(U, V, M1);
}

void resultant(zz_p& rres, const zz_pX& u, const zz_pX& v)
{
   if (deg(u) <= NTL_zz_pX_GCD_CROSSOVER || deg(v) <= NTL_zz_pX_GCD_CROSSOVER) {
      PlainResultant(rres, u, v);
      return;
   }

   zz_pX u1, v1;

   u1 = u;
   v1 = v;

   zz_p res, t;
   set(res);

   if (deg(u1) == deg(v1)) {
      rem(u1, u1, v1);
      swap(u1, v1);

      if (IsZero(v1)) {
         clear(rres);
         return;
      }

      power(t, LeadCoeff(u1), deg(u1) - deg(v1));
      mul(res, res, t);
      if (deg(u1) & 1)
         negate(res, res);
   }
   else if (deg(u1) < deg(v1)) {
      swap(u1, v1);
      if (deg(u1) & deg(v1) & 1)
         negate(res, res);
   }

   // deg(u1) > deg(v1) && v1 != 0

   vec_zz_p cvec;
   vec_long  dvec;

   cvec.SetMaxLength(deg(v1)+2);
   dvec.SetMaxLength(deg(v1)+2);

   append(cvec, LeadCoeff(u1));
   append(dvec, deg(u1));

   while (deg(u1) > NTL_zz_pX_GCD_CROSSOVER && !IsZero(v1)) {
      ResHalfGCD(u1, v1, cvec, dvec);

      if (!IsZero(v1)) {
         append(cvec, LeadCoeff(v1));
         append(dvec, deg(v1));
         rem(u1, u1, v1);
         swap(u1, v1);
      }
   }

   if (IsZero(v1) && deg(u1) > 0) {
      clear(rres);
      return;
   }

   long i, l;
   l = dvec.length();

   if (deg(u1) == 0) {
      // we went all the way...

      for (i = 0; i <= l-3; i++) {
         power(t, cvec[i+1], dvec[i]-dvec[i+2]);
         mul(res, res, t);
         if (dvec[i] & dvec[i+1] & 1)
            negate(res, res);
      }

      power(t, cvec[l-1], dvec[l-2]);
      mul(res, res, t);
   }
   else {
      for (i = 0; i <= l-3; i++) {
         power(t, cvec[i+1], dvec[i]-dvec[i+2]);
         mul(res, res, t);
         if (dvec[i] & dvec[i+1] & 1)
            negate(res, res);
      }

      power(t, cvec[l-1], dvec[l-2]-deg(v1));
      mul(res, res, t);
      if (dvec[l-2] & dvec[l-1] & 1)
         negate(res, res);

      PlainResultant(t, u1, v1);
      mul(res, res, t);
   }

   rres = res;
}

void NormMod(zz_p& x, const zz_pX& a, const zz_pX& f)
{
   if (deg(f) <= 0 || deg(a) >= deg(f))
      Error("norm: bad args");

   if (IsZero(a)) {
      clear(x);
      return;
   }

   zz_p t;
   resultant(t, f, a);
   if (!IsOne(LeadCoeff(f))) {
      zz_p t1;
      power(t1, LeadCoeff(f), deg(a));
      inv(t1, t1);
      mul(t, t, t1);
   }

   x = t;
}

NTL_END_IMPL


/*
	* Implements leapfrog solving with multiple solvers
	*
	*
*/

#ifdef _MAIN_

#else
#include <armadillo>

#include "../core/UAmoeba.hpp"

#ifdef _ALGEBRA_TOOLS_
#else
#include "../core/algebraTools.hpp"
#endif

#ifdef _AMOEBA_INIT_
#else
#include "../core/amoebaParam.cpp"
#endif

#include "../core/translate.cpp"

using namespace algebraTools;
using namespace arma;

#endif

//cx_mat X0, cx_mat X1, int matSize, long maxAmoebaIters, long nGridPoints, double precision, long maxMainIters,

//size, rank MPI directives
//
template <typename T>
void segSolver(const AmoebaParam<T> amoebaParam, int rank, int size)
{

	cx_mat idMat = eye<T>(amoebaParam.matSize, amoebaParam.matSize);
	arma_rng::set_seed_random();

	// UAmoeba(long maxIters, int nGridPoints, double precision, int matSize, int lieDimension, vector<cx_mat> *inputBasis)

	cout << "Creating amoeba object \n";

	UAmoeba* amoeba = new UAmoeba(amoebaParam.maxAmoebaIters, amoebaParam.nGridPoints, amoebaParam.precision, amoebaParam.matSize, amoebaParam.lieDimension, amoebaParam.basis);

	cout << "Allocating MPI buffers \n";

	ArmadilloMPI* armaMPI = new ArmadilloMPI( amoebaParam.matSize, amoebaParam.matSize);
  vector<T> world;
  int bufferSize = 2 * size;

  if(rank == 0)
  {
   	//get X0
		cx_mat X0 = amoebaParam.startBoundary;
		cx_mat X1 = amoebaParam.endBoundary;

		cout << "Start Boundary = " << X0 << "\n"
				 << "Target Matrix = " << X1 << "\n";

    cx_mat kMid;
    invCayley<T>(X1, idMat, kMid);

   	world.push_back(X0);

		for(int i = 1; i < (bufferSize); ++i)
  	{
			kMid *= -0.5 * (i/static_cast<double>(bufferSize));
   		cayley<T>(kMid, idMat, X0);
  		world.push_back(X0);
  	}

    world.push_back(X1);
    cout << "Number of leap-frog points = "<< world.size() << "\n";
	}

	/*
		* int tag = 4 * rank;
		* need to send/recv 4 unique things
		* send and recieves have tag and tag+1 internally
		*
	*/

	int tag = 4 * rank;
	int offSet = 0;

	double localEnergy;
	double totalEnergy = 0;

	cx_mat bL(amoebaParam.matSize, amoebaParam.matSize);
	cx_mat bU(amoebaParam.matSize, amoebaParam.matSize);
	cx_mat newBound(amoebaParam.matSize, amoebaParam.matSize);

	vector<vec> newGuess;
	newGuess.resize(amoebaParam.nGridPoints);

	for(int i=0; i<amoebaParam.nGridPoints; ++i)
	{
		newGuess[i] = randu<vec>(amoebaParam.lieDimension);
	}
	/*
		* to keep track of where all the solvers are
		* each solver has to advance across the world
		* taking two boundary points
		* and outputing a new boundary
	*/

	int indLow;
	int indUp;
	int indMid;

	bufferSize = bufferSize + 1;

	int iters = 0;

	while(iters < amoebaParam.maxMainIters )
	{
		if(rank==0)
		{
			for(int src = 1; src < size; ++src)
			{
				indLow = (2 * src + offSet)%(bufferSize) ;
				indUp = (indLow + 2)%(bufferSize);
				if(indUp < indLow)
				{
					indLow = 0;
					indUp = indLow + 2;
				}

				//cout << "sending boundaries to " << src << endl;
				//cout << "sending " << indLow << " and " << indUp << endl;

				if(size > 0)
				{
					armaMPI->matDestroySend(world[indLow], src, 4 * src );
					armaMPI->matDestroySend(world[indUp], src, (4 * src) + 2);
				}
			}

			indLow = offSet%bufferSize;
			indUp = (indLow + 2)%(bufferSize);
			if(indUp < indLow)
			{
				indLow = 0;
				indUp = indLow + 2;
			}
			indMid = indLow + 1;
			//cout << "solving on " << indLow << " and " << indUp << endl;

			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, world[indUp]);
			amoeba->solver(newGuess, world[indLow], world[indUp]);
			amoeba->newBoundary(world[indMid]);
			localEnergy = amoeba->getEnergy();

			for(int src = 1; src < size; ++src)
			{
				indLow = (2 * src + offSet)%(bufferSize) ;
				indUp = (indLow + 2)%(bufferSize);
				if(indUp < indLow)
				{
					indLow = 0;
				}
				indMid = indLow + 1;

				world[indMid] = armaMPI->matConstructRecv(src, 4 * src);
				//cout << "recieved updated boundary from" << src << endl;
			}

			offSet +=1;
			offSet = (offSet)%(bufferSize);
			//cout << "offSet is now " << offSet << endl;
		}
		else
		{
			bL = armaMPI->matConstructRecv(0, tag);
			bU = armaMPI->matConstructRecv(0, tag + 2);
			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, bU);

			//cout << "recieved boundary on thread " << rank << endl;

			amoeba->solver(newGuess, bL, bU);
			amoeba->newBoundary(newBound);
			armaMPI->matDestroySend(newBound, 0, tag);
			localEnergy = amoeba->getEnergy();
		}

		MPI_Reduce(&localEnergy, &totalEnergy, size, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);

		if(rank == 0)
		{
			ofstream energyFile;
			energyFile.open("eOut.txt", ios::app);
			energyFile << totalEnergy << endl;
			cout << "iters" << iters << endl;
			cout << "=======================" << endl;
			totalEnergy = 0 ;

		}

		++iters;
	}

	if(rank == 0)
	{
		for(int i = 0; i < (2 * size - 1); ++i)
		{
			cout << "boundary is now" << world[i] << world[i+2];
			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, world[i + 2]);
			amoeba->solver(newGuess, world[i], world[i + 2]);
			amoeba->curvePrint();
			amoeba->newBoundary(world[i+1]);

		}
	}

	delete armaMPI;
}


//------------------------------------------------------------------------------
// \file LazyValuation_tests.cpp
//------------------------------------------------------------------------------

#include <boost/test/unit_test.hpp>

#include "Utilities/LazyValuation.h"

using Utilities::LazyValuation;

BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(LazyValuation_tests)

// cf. http://pedromelendez.com/blog/2015/07/16/recursive-lambdas-in-c14/
// We cannot capture a variable declared using auto in its own initialization.
// lambdas in C++ are unique and unnamed; how to reference the function object
// cannot use this keyword inside body of lambda.
auto fibonacci = [](int x)
{
	auto implementation = [](int x, const auto& implementation) -> int
	{
		if (x == 0 || x == 1)
		{
			return 1;
		}
		else
		{
			return implementation(x - 1, implementation) +
				implementation(x - 2, implementation);
		}
	};

	return implementation(x, implementation);
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(LazyValuationConstructs)
{
	BOOST_TEST(fibonacci(0) == 1);
	BOOST_TEST(fibonacci(1) == 1);
	BOOST_TEST(fibonacci(2) == 2);
	BOOST_TEST(fibonacci(3) == 3);
	BOOST_TEST_REQUIRE(fibonacci(4) == 5);

	auto fibonacci_5 = []()
	{
		return fibonacci(5);
	};

	LazyValuation lazy_fibonacci_5_valuation {fibonacci_5};

	BOOST_TEST(lazy_fibonacci_5_valuation == 8);
}

// \ref https://gitlab.com/manning-fpcpp-book/code-examples/blob/master/chapter-06/lazy-val/main.cpp
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IsInvocableWorks)
{
	constexpr int number {6};

}

BOOST_AUTO_TEST_SUITE_END() // LazyValuation_tests

// cf. https://nalaginrut.com/archives/2019/10/31/8%20essential%20patterns%20you%20should%20know%20about%20functional%20programming%20in%20c%2B%2B14

BOOST_AUTO_TEST_SUITE(Lazy_tests)

// Thunk is a nullary function, say, a function without any parameters.
using thunk_t = std::function<int(void)>;
// Return type is trivial, point is "nullary".
// Why does "nullary" matter? You have closure, so you can capture values you
// need, parameters are unnnecessary for us. May realize that thunk may help you
// to unify interface.

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateLazyPattern)
{
	{
		int x {5};
		auto thunk =
			[x]()
				{
					std::cout << "now it run" << std::endl;
				};

		std::cout << "Thunk will not run before you run it" << std::endl;
		thunk();

		BOOST_TEST(true);
	}
}

template <typename T>
using UnaryF_t = std::function<T(void)>;

// https://stackoverflow.com/questions/265392/why-is-lazy-evaluation-useful
// https://bartoszmilewski.com/2014/04/21/getting-lazy-with-c/
// https://github.com/BartoszMilewski/Okasaki/blob/master/LazyQueue/Queue.h
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SquareAsLazyEvaluation)
{
	{
		double external_x {2};
		auto thunk =
			[&external_x]()
			{
				return external_x * external_x;
			};

		external_x = thunk();
		external_x = thunk();
		external_x = thunk();
		BOOST_TEST(external_x == 256);
	}
}

BOOST_AUTO_TEST_SUITE_END() // Lazy_tests
BOOST_AUTO_TEST_SUITE_END() // Utilities


#include <stan/math/prim.hpp>
#include <test/unit/math/prim/util.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <gtest/gtest.h>
#include <stdexcept>
#include <vector>

TEST(ProbDistributionsInvWishartCholesky, rng) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using Eigen::MatrixXd;

  using stan::math::inv_wishart_cholesky_rng;

  boost::random::mt19937 rng;

  MatrixXd omega(3, 4);
  EXPECT_THROW(inv_wishart_cholesky_rng(3.0, omega, rng), std::domain_error);

  MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;

  Matrix<double, Dynamic, Dynamic> LS = sigma.llt().matrixL();

  EXPECT_NO_THROW(inv_wishart_cholesky_rng(3.0, LS, rng));
  EXPECT_THROW(inv_wishart_cholesky_rng(2, LS, rng), std::domain_error);
  EXPECT_THROW(inv_wishart_cholesky_rng(-1, LS, rng), std::domain_error);
  LS(2, 2) = -1;
  EXPECT_THROW(inv_wishart_cholesky_rng(3.0, LS, rng), std::domain_error);
}

TEST(ProbDistributionsInvWishartCholesky, rng_pos_def) {
  using Eigen::MatrixXd;
  using stan::math::inv_wishart_cholesky_rng;

  boost::random::mt19937 rng;

  MatrixXd Sigma(2, 2);
  MatrixXd Sigma_non_pos_def(2, 2);

  Sigma << 1, 0, 0, 1;
  Sigma_non_pos_def << -1, 0, 0, 1;

  unsigned int dof = 5;

  EXPECT_NO_THROW(inv_wishart_cholesky_rng(dof, Sigma, rng));
  EXPECT_THROW(inv_wishart_cholesky_rng(dof, Sigma_non_pos_def, rng),
               std::domain_error);
}

TEST(ProbDistributionsInvWishartCholesky, marginalTwoChiSquareGoodnessFitTest) {
  using boost::math::chi_squared;
  using boost::math::digamma;
  using Eigen::MatrixXd;
  using stan::math::determinant;
  using stan::math::inv_wishart_cholesky_rng;
  using std::log;

  boost::random::mt19937 rng;
  MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 2.0, -3.0, 4.0, 0.0, 2.0, 0.0, 3.0;

  MatrixXd siginv(3, 3);
  siginv = sigma.inverse();
  int N = 10000;

  double avg = 0;
  double expect = sigma.rows() * log(2.0) + log(determinant(siginv))
                  + digamma(5.0 / 2.0) + digamma(4.0 / 2.0)
                  + digamma(3.0 / 2.0);

  MatrixXd a(sigma.rows(), sigma.rows());
  for (int count = 0; count < N; ++count) {
    a = inv_wishart_cholesky_rng(5.0, stan::math::cholesky_decompose(sigma),
                                 rng);
    avg += stan::math::sum(stan::math::log(a.diagonal()));
  }
  avg /= N;
  double chi = (expect - avg) * (expect - avg) / expect;
  chi_squared mydist(1);
  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsInvWishartCholesky, SpecialRNGTest) {
  // When the scale matrix is an identity matrix and df = k + 2
  // The avg of the samples should also be an identity matrix

  using Eigen::MatrixXd;
  using stan::math::inv_wishart_cholesky_rng;
  using stan::math::multiply_lower_tri_self_transpose;

  boost::random::mt19937 rng(1234U);
  int N = 1e5;
  double tol = 0.1;
  for (int k = 1; k < 5; k++) {
    MatrixXd sigma = MatrixXd::Identity(k, k);
    MatrixXd Z = MatrixXd::Zero(k, k);
    for (int i = 0; i < N; i++) {
      Z += stan::math::crossprod(inv_wishart_cholesky_rng(k + 2, sigma, rng));
    }
    Z /= N;
    for (int j = 0; j < k; j++) {
      for (int i = 0; i < k; i++) {
        if (j == i)
          EXPECT_NEAR(Z(i, j), 1.0, tol);
        else
          EXPECT_NEAR(Z(i, j), 0.0, tol);
      }
    }
  }
}


//
//  tFEM.hpp
//
//  Created by r. on 09/05/14
//

#ifndef round1_tFEM_hpp
#define round1_tFEM_hpp

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

#include "tmesh.hpp"
#include "stopwatch.hpp"
//#include "EVD.hpp"
#include "AWT.hpp"

//#include <algorithm>

namespace spacetime
{
	namespace ublas = boost::numeric::ublas;

	class tFEM
	{
	public:
		typedef ublas::compressed_matrix<double> sparse_matrix;
		typedef ublas::matrix<double> dense_matrix;
		typedef ublas::vector<double> vector;
	public:
		// Should be hidden
		sparse_matrix MtE, AtE;
		sparse_matrix MtF;
		sparse_matrix CtFE, MtFE;
		sparse_matrix EtE;
		dense_matrix VtE;
		vector gamma;
//	public:
//		const sparse_matrix& getMtE() const { return MtE; }
//		const sparse_matrix& getAtE() const { return AtE; }
//		const sparse_matrix& getMtF() const { return MtF; }
//		const sparse_matrix& getCtFE() const { return CtFE; }
//		const sparse_matrix& getMtFE() const { return MtFE; }
//		const sparse_matrix& getEtE() const { return EtE; }
//		const dense_matrix& getVtE() const { return VtE; }
//		const vector& getgamma() const { return gamma; }
	private:
		tmesh TE, TF;
		unsigned int nref;
	public:
		// Should be hidden
		unsigned int dimE;
		unsigned int dimF;
	public:
		const tmesh& refTE() const { return TE; }
		const tmesh& refTF() const { return TF; }
//		unsigned int dimE() const { return (unsigned int)(TE.size()); }
//		unsigned int dimF() const { return (unsigned int)(TF.size() - 1); }
	public:
		void perform_evd()
		{
			stopwatch::Time time(stopwatch::watches, "tFEM::perform_evd()");

			{
				//gsl::EVD evd(AtE, MtE);
				//spacetime::AWT evd(AtE, MtE);
				//VtE = evd.V;
				//gamma = evd.d;
				//assert(gamma.size() == TE.size());

				VtE.resize(0, 0);
				gamma.resize(0);
			}

			for (vector::iterator i = gamma.begin(); i != gamma.end(); ++i)
				(*i) = sqrt(std::max(0.0, *i));
		}
	public:
		void assemble_matrices()
		{
			stopwatch::Time time(stopwatch::watches, "tFEM::assemble_matrices()");

			// nref >= 1 not implemented
			assert(nref == 0);

			typedef ublas::mapped_matrix<double> mapped_matrix;
			mapped_matrix MtE(dimE, dimE, 3*dimE); MtE.clear();
			mapped_matrix AtE(dimE, dimE, 3*dimE); AtE.clear();
			{
				std::vector<tmesh::interval> Is = TE.getIs();
				for (auto n = 0; n != Is.size(); ++n)
				{
					unsigned int m = n + 1;

					double h = Is[n].len;
					MtE(n, n) += (2./6.) * h;
					MtE(m, m) += (2./6.) * h;
					MtE(m, n) += (1./6.) * h;
					MtE(n, m) += (1./6.) * h;

					double g = 1 / h;
					AtE(n, n) += +g;
					AtE(m, m) += +g;
					AtE(m, n) += -g;
					AtE(n, m) += -g;
				}
			}

			mapped_matrix MtF(dimF, dimF, dimF); MtF.clear();
			{
				std::vector<tmesh::interval> Is = TF.getIs();
				for (auto i = 0; i != Is.size(); ++i)
				{
					MtF(i, i) = Is[i].len;
				}
			}

			mapped_matrix CtFE(dimF, dimE, 2*dimF); CtFE.clear();
			mapped_matrix MtFE(dimF, dimE, 2*dimF); MtFE.clear();
			{
				std::vector<tmesh::interval> Is = TE.getIs();
				for (auto n = 0; n != Is.size(); ++n)
				{
					MtFE(n, n+0) += (1./2.) * Is[n].len;
					MtFE(n, n+1) += (1./2.) * Is[n].len;

					CtFE(n, n+0) += -1;
					CtFE(n, n+1) += +1;
				}
			}

			mapped_matrix EtE(1, dimE, 1); EtE.clear();
			{
				EtE(0, 0) = 1;
			}

			this->MtE = MtE;
			this->AtE = AtE;
			this->MtF = MtF;
			this->CtFE = CtFE;
			this->MtFE = MtFE;
			this->EtE = EtE;
		}
	public:
		tFEM(const tmesh& te, unsigned nref = 0, bool perform_evd_now = true)
		:
		TE(te), TF(te), nref(nref), dimE(0), dimF(0)
		{
			for (unsigned int n = 0; n != nref; ++n)
				TF.refine();

			dimE = (unsigned int)(TE.size());
			dimF = (unsigned int)(TF.size() - 1);

			assemble_matrices();

			if (perform_evd_now) perform_evd();
		}
	};

} // namespace spacetime

#endif