//
// Copyright (c) 2018 Sho Hirose
//
// Distributed under the MIT License. (See accompanying file LICENSE.txt or copy
// at https://opensource.org/licenses/MIT)
//
#ifndef SOLVER_INTERFACE_HPP
#define SOLVER_INTERFACE_HPP

#include <Eigen/Core>

/// Interface class to solve a system of linear equation, Ax = b, using Eigen
/// solvers.
template <class MatrixType>
class solver_interface {
 public:
  virtual ~solver_interface()                                   = default;
  virtual void decompose(const Eigen::Ref<const MatrixType>& A) = 0;
  virtual Eigen::VectorXd solve(const Eigen::Ref<const Eigen::VectorXd>& b) = 0;
};

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/comparable/equal_mcd.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/logical/logical.hpp>

#include <stdexcept>

namespace boost { namespace hana {
    struct Function;

    template <typename Domain, typename Codomain, typename F,
        typename = operators<Comparable>
    >
    struct function_type {
        using hana_datatype = Function;

        Domain dom;
        Codomain cod;
        F def;

        friend constexpr auto domain(function_type f)
        { return f.dom; }

        friend constexpr auto codomain(function_type f)
        { return f.cod; }

        template <typename X>
        constexpr auto operator()(X x) const {
            if (!elem(domain(*this), x))
                throw std::domain_error{"use of a hana::function with an argument out of the domain"};
            return def(x);
        }
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto function = [](auto domain, auto codomain) {
        return [=](auto definition) {
            return function_type<decltype(domain), decltype(codomain), decltype(definition)>{
                domain, codomain, definition
            };
        };
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto frange = [](auto f) {
        // Note: that would be better handled by a set data structure, but
        // whatever for now.
        return foldl(fmap(f, domain(f)), list(), [](auto xs, auto x) {
            return if_(elem(xs, x), xs, cons(x, xs));
        });
    };

    template <>
    struct Comparable::instance<Function, Function> : Comparable::equal_mcd {
        template <typename F, typename G>
        static constexpr auto equal_impl(F f, G g) {
            return domain(f) == domain(g) && all(domain(f), demux(equal, f, g));
        }
    };
}} // end namespace boost::hana

// BOOST_HANA_CONSTEXPR_LAMBDA auto is_injective = [](auto f) {
//     auto check = [](auto x, auto y) {
//         return (x != y)     ^implies^   (f(x) != f(y));
//     };
//     return all(product(domain(f), domain(f)), check);
// };

// BOOST_HANA_CONSTEXPR_LAMBDA auto is_onto = [](auto f) {
//     return codomain(f) == range(g);
// };

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/range.hpp>
using namespace boost::hana;
using namespace literals;

int main() {
    auto f = function(list(1_c, 2_c, 3_c), list(1_c, 2_c, 3_c, 4_c, 5_c, 6_c))(
        [](auto x) { return x + 1_c; }
    );

    auto g = function(list(1_c, 2_c, 3_c), list(2_c, 3_c, 4_c))(
        [](auto x) { return x + 1_c; }
    );

    auto h = function(list(1_c, 2_c, 3_c), list(0_c, 1_c, 2_c))(
        [](auto x) { return x - 1_c; }
    );

    BOOST_HANA_CONSTANT_ASSERT(f == g);
    BOOST_HANA_CONSTANT_ASSERT(f != h);
    BOOST_HANA_CONSTEXPR_ASSERT(f(1) == 2);
    try { f(6); throw; } catch (std::domain_error) { }

    BOOST_HANA_CONSTANT_ASSERT(frange(f) == list(4_c, 3_c, 2_c));
    (void)frange;
}


/*!
 * @file
 * Calculating displaced atoms at each frame from a lammps simulation.
 *
 * default run cmd from project directory:
 * `mpirun -n 1 ./bin/displaced "data/lammps/dump6000.txt" "data/lammps/dump.txt" 3.165
 *
 * ![displaced data-flow](../doc/displaced.png)
 *
 * benchmarks at the bottom
 * */
#include <array>
#include <iostream>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>
#include <boost/unordered_map.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/predicates.hpp>
#include <ezl/algorithms/reduces.hpp>
#include <ezl/algorithms/fromFile.hpp>

auto calcDist(std::array<float, 3> p1, std::array<float, 3> p2) {
  auto diff = 0.0F;
  for (auto i : {0, 1, 2}) {
    diff += (p1[i] - p2[i]) * (p1[i] - p2[i]);
  }
  return sqrt(diff);
}

void displaced(int argc, char* argv[]) {
  using std::vector;
  using std::tuple;
  using std::array;

  const std::string outFile = "data/output/nDisp.txt";
  const float toleranceRatio = 0.3;

  if (argc < 4) {
    ezl::Karta::inst().print0("provide file for 1st time_step, file glob for all frames, "
                 "lattice constant as arguments. Continuing with defaults.");
  }
  std::string firstFile = "data/lammps/dump6000.txt";
  if (argc > 1) firstFile = std::string(argv[1]);
  std::string allFiles = "data/lammps/dump.txt";
  if (argc > 2) allFiles = std::string(argv[2]);
  auto latConst = 3.165F;
  if (argc > 3) latConst = float(std::stof(argv[3]));;

  auto partitionFn = [] (const std::tuple<const int&>& x) -> std::size_t {
    return std::get<0>(x)/100;
  };

  // loading first frame atoms in the memory partitioned on atoms-id.
  auto buffer = ezl::rise(ezl::fromFile<int, array<float, 3>, int>(firstFile)
                            .cols({1, 3, 4, 5, 6})  // id, coords
                            .lammps())
                  .filter(ezl::tautology()).partitionBy<1>().prll(1.)
                  .get();

  boost::unordered_map<int, array<float, 3>> firstFrame;
  for(const auto& it :buffer) firstFrame[std::get<0>(it)] = std::get<1>(it);

  ezl::rise(ezl::fromFile<int, array<float, 3>, int>(allFiles)
                .cols({1, 3, 4, 5, 6}) // id, coords, timestep
                .lammps())
      .map<1, 2>([&firstFrame](int id, array<float, 3> coords) {
        return calcDist(coords, firstFrame[id]);
      }).partitionBy<1>().prll(1.).colsTransform()
      .filter<1>(ezl::gt(latConst * toleranceRatio))
      .reduce<2>(ezl::count(), 0).inprocess()
      .reduce<1>(ezl::sum(), 0).partitionBy(partitionFn)
      .reduceAll([](vector<tuple<int, int>> a) {
        sort(a.begin(), a.end());
        return a;
      }).dump(outFile)
      .run();
  ezl::Karta::inst().print0("Output file written in data/output/disp");
}

int main(int argc, char *argv[]) {
  // boost::mpi::environment env(argc, argv, false);
  ezl::Env env{argc, argv, false};
  try {
    displaced(argc, argv);
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}

/*!
 * benchmark results: i7(hdd); input: 25000 atoms/frame (bcc-50x50) 11-frames
 *  *nprocs* | 1   | 2   | 4    |
 *  ---      |---  |---  |---   |
 *  *time(s)*| 46  | 29  | 19   |
 *
 * benchmark results: Linux(nfs-3); input: 25000 atoms/frame (bcc-50x50) 22-frames
 *  *nprocs* | 1x12      | 2x12      |
 *  ---      |---        |---        |
 *  *time(s)*| 169       | 249       |
 */


// ---------------------------------------------------------
//
//  trianglequality.cpp
//  Tyson Brochu 2011
//  Christopher Batty, Fang Da 2014
//
//  Functions for getting various triangle mesh measures.
//
// ---------------------------------------------------------

#include <trianglequality.h>

#include <limits>
#include <surftrack.h>
#include <set>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
// ---------------------------------------------------------
///
/// Determine the "mixed" voronoi and barycentric area of the vertex within the given triangle.
///
// ---------------------------------------------------------

namespace LosTopos {

double mixed_area( size_t vertex_index, size_t triangle_index, const SurfTrack& surf )
{
    const Vec3st& tri = surf.m_mesh.get_triangle(triangle_index);

    Vec2st opposite_edge;
    if ( vertex_index == tri[0] )
    {
        opposite_edge = Vec2st( tri[1], tri[2] );
    }
    else if ( vertex_index == tri[1] )
    {
        opposite_edge = Vec2st( tri[2], tri[0] );
    }
    else
    {
        opposite_edge = Vec2st( tri[0], tri[1] );
    }

    const Vec3d& a = surf.get_position(vertex_index);
    const Vec3d& b = surf.get_position(opposite_edge[0]);
    const Vec3d& c = surf.get_position(opposite_edge[1]);

    bool obtuse_triangle = ( ( dot(b-a, c-a) < 0.0 ) || ( dot(a-b, c-b) < 0.0 ) || ( dot(a-c, b-c) < 0.0 ) );

    if ( obtuse_triangle )
    {
        //std::cout << "obtuse_triangle " << triangle_index << ": " << tri << std::endl;

        if ( dot(b-a, c-a) < 0.0 )
        {
            // obtuse at a
            return 0.5 * surf.get_triangle_area( triangle_index );
        }
        else
        {
            // obtuse somewhere else
            return 0.25 * surf.get_triangle_area( triangle_index );
        }
    }
    else
    {
        // not obtuse, use voronoi area

        double cross_c = mag( cross( a-c, b-c ) );
        double cot_c = dot( a-c, b-c) / cross_c;

        double cross_b = mag( cross( a-b, c-b ) );
        double cot_b = dot( a-b, c-b) / cross_b;

        return 1.0 / 8.0 * (mag2(b-a) * cot_c + mag2(c-a) * cot_b);
    }

}

// ---------------------------------------------------------
///
/// Get Kappa * n, the surface normal multiplied by mean curvature at the specified vertex.
///
// ---------------------------------------------------------

void vertex_mean_curvature_normal( size_t vertex_index, const SurfTrack& surf, Vec3d& out, double& weight_sum )
{
    Vec3d mean_curvature_normal( 0, 0, 0 );
    weight_sum = 0;

    double edge_length_sum = 0.0;

    for ( size_t i = 0; i < surf.m_mesh.m_vertex_to_edge_map[vertex_index].size(); ++i )
    {
        size_t e = surf.m_mesh.m_vertex_to_edge_map[vertex_index][i];
        const Vec2st& curr_edge = surf.m_mesh.m_edges[e];
        Vec3d edge_vector;
        if ( curr_edge[0] == vertex_index )
        {
            edge_vector = surf.get_position( curr_edge[1] ) - surf.get_position( vertex_index );
        }
        else
        {
            assert( curr_edge[1] == vertex_index );
            edge_vector = surf.get_position( curr_edge[0] ) - surf.get_position( vertex_index );
        }

        edge_length_sum += mag( edge_vector );

        if ( surf.m_mesh.m_edge_to_triangle_map[e].size() != 2 )
        {
            // TODO: properly handle more than 2 incident triangles
            out = Vec3d(0,0,0);
            return;
        }

        size_t tri0 = surf.m_mesh.m_edge_to_triangle_map[e][0];
        size_t tri1 = surf.m_mesh.m_edge_to_triangle_map[e][1];

        size_t third_vertex_0 = surf.m_mesh.get_third_vertex( curr_edge[0], curr_edge[1], surf.m_mesh.get_triangle(tri0) );
        size_t third_vertex_1 = surf.m_mesh.get_third_vertex( curr_edge[0], curr_edge[1], surf.m_mesh.get_triangle(tri1) );

        Vec3d v00 = surf.get_position( curr_edge[0] ) - surf.get_position( third_vertex_0 );
        Vec3d v10 = surf.get_position( curr_edge[1] ) - surf.get_position( third_vertex_0 );

        double cross_0 = mag( cross( v00, v10 ) );
        if ( cross_0 < 1e-10 )
        {
            continue;
        }
        double cot_0 = dot(v00, v10) / cross_0;

        Vec3d v01 = surf.get_position( curr_edge[0] ) - surf.get_position( third_vertex_1 );
        Vec3d v11 = surf.get_position( curr_edge[1] ) - surf.get_position( third_vertex_1 );

        double cross_1 = mag( cross( v01, v11 ) );
        if ( cross_1 < 1e-10 )
        {
            continue;
        }

        double cot_1 = dot(v01, v11) / cross_1;

        double weight = cot_0 + cot_1;
        weight_sum += weight;

        mean_curvature_normal += weight * edge_vector;

    }

    double vertex_area = 0.0;
    for ( size_t i = 0; i < surf.m_mesh.m_vertex_to_triangle_map[vertex_index].size(); ++i )
    {
        vertex_area += mixed_area( vertex_index, surf.m_mesh.m_vertex_to_triangle_map[vertex_index][i], surf );
    }

    double coeff = 1.0 / (2.0 * vertex_area);

    weight_sum *= coeff;

    out = coeff * mean_curvature_normal;

}

// ---------------------------------------------------------
///
/// Return an estimate for mean curvature at the given vertex, computed using the Kappa * n estimate above.
///
// ---------------------------------------------------------

double unsigned_vertex_mean_curvature( size_t vertex_index, const SurfTrack& surf )
{
    Vec3d mc_normal;
    double weight_sum;

    vertex_mean_curvature_normal( vertex_index, surf, mc_normal, weight_sum );

    return mag( mc_normal );
}

// ---------------------------------------------------------
///
/// Return an estimate for curvature by computing the minimum radius of a sphere defined by the edge neighbourhood around a vertex,
/// and taking the reciprocal.
///
// ---------------------------------------------------------

double inv_min_radius_curvature( const SurfTrack& surf, size_t vertex )
{

    Vec3d normal = surf.get_vertex_normal( vertex );

    //   double min_radius = BIG_DOUBLE;

    double inv_min_radius = -BIG_DOUBLE;

    for ( size_t i = 0; i < surf.m_mesh.m_vertex_to_edge_map[vertex].size(); ++i )
    {
        size_t edge_index = surf.m_mesh.m_vertex_to_edge_map[vertex][i];

        assert( edge_index < surf.m_mesh.m_edges.size() );

        const Vec2st& edge = surf.m_mesh.m_edges[ edge_index ];

        Vec3d P;
        if ( edge[0] == vertex )
        {
            P = surf.get_position( edge[1] ) - surf.get_position( vertex );
        }
        else
        {
            P = surf.get_position( edge[0] ) - surf.get_position( vertex );
        }

        //      double radius = 0.5 * dot( P, P ) / dot( normal, P );
        //      min_radius = min( min_radius, radius );

        double inv_radius = 2.0 * dot( normal, P ) / dot( P, P );
        inv_min_radius = max( inv_min_radius, inv_radius );

    }

    return inv_min_radius;

}

// ---------------------------------------------------------
///
/// Compute curvature at a vertex using quadric fitting
///
// ---------------------------------------------------------

double estimated_max_curvature(const SurfTrack& surf, size_t vertex) {

    Vec3d normal = surf.get_vertex_normal(vertex);

    Vec3d seed(0,0,0);
    if(abs(normal[0]) < 0.5)
        seed[0] = 1.0;
    else
        seed[1] = 1.0;
    Vec3d u = cross(normal, seed);
    normalize(u);
    Vec3d v = cross(normal, u);

    std::set<size_t> twoneighbors;
    twoneighbors.insert(vertex);
    for(int i=0; i<2; i++)
    {
        std::set<size_t> toadd;
        for(std::set<size_t>::iterator it = twoneighbors.begin(); it != twoneighbors.end(); ++it)
        {
            size_t curVert = *it;
            const std::vector<size_t>& nbrEdges = surf.m_mesh.m_vertex_to_edge_map[curVert];
            for(unsigned int nbrIdx = 0; nbrIdx < nbrEdges.size(); ++nbrIdx)
            {
                size_t edge_id = nbrEdges[nbrIdx];
                Vec2st edge = surf.m_mesh.m_edges[edge_id];
                size_t fv = edge[0];
                if(fv == curVert)
                    toadd.insert(edge[1]);
                else
                    toadd.insert(fv);
            }
        }
        for(std::set<size_t>::iterator it = toadd.begin(); it != toadd.end(); ++it)
            twoneighbors.insert(*it);
    }

    int numneighbors = (int)twoneighbors.size();
    if(numneighbors < 6)
        return 0;
    //TODO I havn't fully verified this after porting to Eigen, so user beware!

    //a u^2 + b v^2 + c uv + d u + e v + f
    Eigen::MatrixXd Mat_Eigen(numneighbors, 6);
    Eigen::VectorXd Rhs_Eigen(numneighbors);

    Vec3d centpt = surf.get_position(vertex);
    int row=0;
    for(std::set<size_t>::iterator it = twoneighbors.begin(); it != twoneighbors.end(); ++it)
    {
        size_t curVert = *it;
        Vec3d adjpt = surf.get_position(curVert);
        Vec3d diff;
        for(int i=0; i<3; i++)
            diff[i] = adjpt[i]-centpt[i];
        double uval = dot(diff,u);
        double vval = dot(diff,v);
        Rhs_Eigen[row] = dot(diff, normal);

        Mat_Eigen(row, 0) = uval*uval;
        Mat_Eigen(row, 1) = vval*vval;
        Mat_Eigen(row, 2) = uval*vval;
        Mat_Eigen(row, 3) = uval;
        Mat_Eigen(row, 4) = vval;
        Mat_Eigen(row, 5) = 1;

        row++;
    }
    assert(row == numneighbors);

    Eigen::VectorXd soln_Eigen = Mat_Eigen.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Rhs_Eigen);

    std::vector<double> coeffs(6);
    for(int id = 0; id < 6; ++id) coeffs[id] = Rhs_Eigen[id];

    double areaElt = sqrt(1+coeffs[3]*coeffs[3]+coeffs[4]*coeffs[4]);
    Vec3d newnormal = (normal - coeffs[3]*u - coeffs[4]*v) / areaElt;

    double E = 1 + coeffs[3]*coeffs[3];
    double F = coeffs[3]*coeffs[4];
    double G = 1 + coeffs[4]*coeffs[4];

    //double fac = normal.dot(newnormal);
    double L = 2*coeffs[0]/areaElt;
    double M = coeffs[2]/areaElt;
    double N = 2*coeffs[1]/areaElt;

    double det = E*G-F*F;
    if(abs(det) < 1e-6)
        return 0;

    Mat22d H(L, M, M, N);

    //LosTopos stores things in column major rather than row major, I believe, hence the transposing
    Mat33d U(u[0], v[0], normal[0],
        u[1], v[1], normal[1],
        u[2], v[2], normal[2]);
    U = U.transpose();

    Mat32d J(1, 0, coeffs[3],
             0, 1, coeffs[4]);

    J = U*J;

    Mat22d shapeOperator = 1/areaElt * inverse(J.transpose()*J) * H;

    //now do an eigensolve on the shape operator to find the principal curvatures/directions
    Eigen::Matrix2d shape_Eigen;
    shape_Eigen << shapeOperator(0, 0), shapeOperator(0, 1), shapeOperator(1, 0), shapeOperator(1, 1);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> es;
    es.compute(shape_Eigen, false);
    if (es.info() != Eigen::Success) assert(0);
    auto eigenvalues_Eigen = es.eigenvalues();

    double max_curvature = max(std::fabs(eigenvalues_Eigen[0]), std::fabs(eigenvalues_Eigen[1]));

    return max_curvature;

}

// ---------------------------------------------------------
///
/// Compute curvatures at all vertices using inv_min_radius_curvature.
///
// ---------------------------------------------------------

void compute_vertex_curvatures( const SurfTrack& surf, std::vector<double>& vertex_curvatures )
{

    vertex_curvatures.resize( surf.get_num_vertices() );

    for ( size_t i = 0; i < surf.get_num_vertices(); ++i )
    {

        if ( surf.m_mesh.m_is_boundary_vertex[i] )
        {
            vertex_curvatures[i] = 1.0;
            continue;
        }

        vertex_curvatures[i] = inv_min_radius_curvature( surf, i );
    }
}

#define USE_INV_MIN_RADIUS

// ---------------------------------------------------------
///
/// Get the length of the specified edge, scaled by an estimate of curvature at each of the vertices.
///
// ---------------------------------------------------------

double get_curvature_scaled_length(const SurfTrack& surf,
                                   size_t vertex_a,
                                   size_t vertex_b,
                                   double min_curvature_multiplier,
                                   double max_curvature_multiplier,
                                   double rest_curvature )
{

    assert( vertex_a < surf.get_num_vertices() );
    assert( vertex_b < surf.get_num_vertices() );

    double length = dist(  surf.get_position( vertex_a ), surf.get_position( vertex_b ) );

    //std::cout << "\n\nTrue length: " << length << std::endl;
#ifdef USE_INV_MIN_RADIUS
    double curv_a = std::fabs( inv_min_radius_curvature( surf, vertex_a ) );
#else
    double curv_a = unsigned_vertex_mean_curvature( vertex_a, surf );
#endif

    curv_a /= rest_curvature;
    curv_a = std::max( min_curvature_multiplier, curv_a );
    curv_a = std::min( max_curvature_multiplier, curv_a );

    //std::cout << "Curv a: " << curv_a << std::endl;

#ifdef USE_INV_MIN_RADIUS
    double curv_b = std::fabs( inv_min_radius_curvature( surf, vertex_b ) );
#else
    double curv_b = unsigned_vertex_mean_curvature( vertex_b, m_surf );
#endif

    curv_b /= rest_curvature;
    curv_b = std::max( min_curvature_multiplier, curv_b );
    curv_b = std::min( max_curvature_multiplier, curv_b );

    //std::cout << "Curv b: " << curv_b << std::endl;

    length *= 0.5 * ( curv_a + curv_b );

    //std::cout << "Multiplier: " << 0.5*(curv_a+curv_b) << std::endl;

    return length;

}

// ---------------------------------------------------------
///
/// Get the length of the specified edge, scaled by an estimate of curvature at each of the vertices.
///
// ---------------------------------------------------------

double get_edge_curvature(const SurfTrack& surf,
  size_t vertex_a,
  size_t vertex_b)
{

  assert( vertex_a < surf.get_num_vertices() );
  assert( vertex_b < surf.get_num_vertices() );

#ifdef USE_INV_MIN_RADIUS
  double curv_a = estimated_max_curvature( surf, vertex_a );
  //double curv_a = std::fabs( inv_min_radius_curvature( surf, vertex_a ) );
#else
  double curv_a = unsigned_vertex_mean_curvature( vertex_a, surf );
#endif

#ifdef USE_INV_MIN_RADIUS
  double curv_b = estimated_max_curvature( surf, vertex_b );
  //double curv_b = std::fabs( inv_min_radius_curvature( surf, vertex_b ) );
#else
  double curv_b = unsigned_vertex_mean_curvature( vertex_b, surf );
#endif

  return max(curv_a, curv_b);//0.5 * ( curv_a + curv_b );

}

// ---------------------------------------------------------
///
/// Return the minimum triangle area in the specified surface.
///
// ---------------------------------------------------------

double min_triangle_area( const SurfTrack& surf )
{
    double min_area = BIG_DOUBLE;
    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        if ( surf.m_mesh.triangle_is_deleted(i) ) { continue; }
        if ( surf.triangle_is_all_solid(i) ) { continue; }

        double area = surf.get_triangle_area(i);
        min_area = std::min( area, min_area );
    }

    return min_area;

}

// ---------------------------------------------------------
///
/// Return the minimun triangle angle in the specified surface.
///
// ---------------------------------------------------------

double min_triangle_angle( const SurfTrack& surf )
{
    double min_angle = BIG_DOUBLE;
    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        if ( surf.m_mesh.triangle_is_deleted(i) ) { continue; }

        const Vec3d& a = surf.get_position( surf.m_mesh.get_triangle(i)[0] );
        const Vec3d& b = surf.get_position( surf.m_mesh.get_triangle(i)[1] );
        const Vec3d& c = surf.get_position( surf.m_mesh.get_triangle(i)[2] );

        double curr_min_angle = min_triangle_angle( a, b, c );

        min_angle = std::min( curr_min_angle, min_angle );
    }

    return min_angle;

}

// ---------------------------------------------------------
///
/// Return the maximum triangle angle in the specified surface.
///
// ---------------------------------------------------------

double max_triangle_angle( const SurfTrack& surf )
{
    double max_angle = -BIG_DOUBLE;

    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        if ( surf.m_mesh.triangle_is_deleted(i) ) { continue; }

        const Vec3d& a = surf.get_position( surf.m_mesh.get_triangle(i)[0] );
        const Vec3d& b = surf.get_position( surf.m_mesh.get_triangle(i)[1] );
        const Vec3d& c = surf.get_position( surf.m_mesh.get_triangle(i)[2] );

        double curr_max_angle = max_triangle_angle( a, b, c );

        max_angle = std::max( curr_max_angle, max_angle );
    }

    return max_angle;
}

// ---------------------------------------------------------
///
/// Count the number of triangle angles below the given threshold.
///
// ---------------------------------------------------------

size_t num_angles_below_threshold( const SurfTrack& surf, double low_threshold )
{
    size_t num_small_angles = 0;

    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        if ( surf.m_mesh.triangle_is_deleted(i) ) { continue; }

        const Vec3d& a = surf.get_position( surf.m_mesh.get_triangle(i)[0] );
        const Vec3d& b = surf.get_position( surf.m_mesh.get_triangle(i)[1] );
        const Vec3d& c = surf.get_position( surf.m_mesh.get_triangle(i)[2] );

        double angle_a, angle_b, angle_c;
        triangle_angles( a, b, c, angle_a, angle_b, angle_c );

        if ( angle_a < low_threshold ) { ++num_small_angles; }
        if ( angle_b < low_threshold ) { ++num_small_angles; }
        if ( angle_c < low_threshold ) { ++num_small_angles; }
    }

    return num_small_angles;

}

// ---------------------------------------------------------
///
/// Count the number of triangle angles above the given threshold.
///
// ---------------------------------------------------------

size_t num_angles_above_threshold( const SurfTrack& surf, double high_threshold )
{
    size_t num_large_angles = 0;

    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        if ( surf.m_mesh.triangle_is_deleted(i) ) { continue; }

        const Vec3d& a = surf.get_position( surf.m_mesh.get_triangle(i)[0] );
        const Vec3d& b = surf.get_position( surf.m_mesh.get_triangle(i)[1] );
        const Vec3d& c = surf.get_position( surf.m_mesh.get_triangle(i)[2] );

        double angle_a, angle_b, angle_c;
        triangle_angles( a, b, c, angle_a, angle_b, angle_c );

        if ( angle_a > high_threshold ) { ++num_large_angles; }
        if ( angle_b > high_threshold ) { ++num_large_angles; }
        if ( angle_c > high_threshold ) { ++num_large_angles; }
    }

    return num_large_angles;

}

// ---------------------------------------------------------
///
/// Compute the aspect ratio of the given triangle
///
// ---------------------------------------------------------

double triangle_aspect_ratio( const SurfTrack& surf, size_t triangle_index )
{
    const Vec3st& tri = surf.m_mesh.get_triangle(triangle_index);
    assert( tri[0] != tri[1] );
    return triangle_aspect_ratio( surf.get_position(tri[0]), surf.get_position(tri[1]), surf.get_position(tri[2]) );
}

// ---------------------------------------------------------
///
/// Find the smallest triangle aspect ratio in the given mesh
///
// ---------------------------------------------------------

double min_triangle_aspect_ratio( const SurfTrack& surf, size_t& output_triangle_index )
{
    double min_ratio = std::numeric_limits<double>::max();
    output_triangle_index = (size_t)~0;

    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        double a_ratio = triangle_aspect_ratio( surf, i );
        if ( a_ratio < min_ratio )
        {
            output_triangle_index = i;
            min_ratio = a_ratio;
        }
    }
    return min_ratio;
}

// ---------------------------------------------------------
///
/// Find the greatest triangle aspect ratio in the given mesh
///
// ---------------------------------------------------------

double max_triangle_aspect_ratio( const SurfTrack& surf, size_t& output_triangle_index )
{
    double max_ratio = -1.0;
    output_triangle_index = (size_t)~0;

    for ( size_t i = 0; i < surf.m_mesh.num_triangles(); ++i )
    {
        double a_ratio = triangle_aspect_ratio( surf, i );
        if ( a_ratio > max_ratio )
        {
            output_triangle_index = i;
            max_ratio = a_ratio;
        }
    }
    return max_ratio;
}

}


#include <scitbx/lstbx/normal_equations.h>
#include <scitbx/constants.h>
#include <scitbx/array_family/ref_reductions.h>
#include <scitbx/array_family/simple_io.h>
#include <boost/lexical_cast.hpp>
#include <scitbx/random/mersenne_twister.h>
#include <boost/random/uniform_real.hpp>
#include <scitbx/random/variate_generator.h>
#include <tbxx/time_accu.hpp>

using namespace scitbx::lstbx::normal_equations;
namespace af = scitbx::af;
using scitbx::constants::pi;

template<typename T, template<typename> class SumOfRank1Updates>
void exercise(int m, int n) {
  typedef scitbx::boost_random::mt19937 engine_t;
  typedef boost::uniform_real<T> distribution_t;
  typedef scitbx::random::variate_generator<engine_t, distribution_t> variate_t;

  engine_t engine(0);
  distribution_t distribution(-10., 10.);
  variate_t variate(engine, distribution);

  af::shared<T> grad_yc(n);
  non_linear_ls_with_separable_scale_factor<T,
                                            SumOfRank1Updates> nls(n);
  tbxx::time_accu building, solving;
  for(int i=0; i<m; i++) {
    af::ref<T> g = grad_yc.ref();
    for(int j=0; j<n; j++) g[j] = variate();
    //SCITBX_EXAMINE(g);
    T yc = i, yo = 2*i;
    building.set_mark();
    nls.add_equation(yc, g, yo, 1.);
    building.accumulate();
  }
  building.set_mark();
  nls.finalise();
  building.accumulate();
  std::cout << "scale factor = " << nls.optimal_scale_factor() << "\n";
  std::cout << "chi^2 = " << nls.chi_sq() << "\n";
  linear_ls<T> step = nls.step_equations();
  solving.set_mark();
  step.solve();
  solving.accumulate();
  af::const_ref<T> s = step.solution().const_ref();
  std::cout << "step = [ " << s[0];
  for(int j=1; j<n; j++) {
    if(j % 50) continue;
    std::cout << " .. " << s[j];
  }
  std::cout << " ]\n";
  std::cout << "\n*** "
            << 8*sizeof(T) << "-bit floats, "
            << m << "x" << n << ", "
            << "building: " << building.as_double()
            << ", solving: " << solving.as_double() << " ***\n";
}

void help() {
  std::cerr << "bench [BLAS-2 | BLAS-3] [single | double] #data #parameters\n";
}

int main(int argc, char * argv[]) {
  using boost::lexical_cast;
  using boost::bad_lexical_cast;

  if(argc != 5) {
    help();
    return 1;
  }
  std::string lvl(argv[1]), precision(argv[2]);
  bool single_precision;
  if(precision == "single") single_precision = true;
  else if (precision == "double") single_precision = false;
  else {
    help();
    return 1;
  }
  try {
    int m = lexical_cast<int>(argv[3]);
    int n = lexical_cast<int>(argv[4]);
    if(lvl == "BLAS-2") {
      std::cout << "Level 2 BLAS implementation\n";
      std::cout << "===========================\n";
      if(single_precision)
        exercise<float, scitbx::matrix::sum_of_symmetric_rank_1_updates>(m, n);
      else
        exercise<double, scitbx::matrix::sum_of_symmetric_rank_1_updates>(m, n);
    }
    else if (lvl == "BLAS-3"){
      std::cout << "Level 3 BLAS implementation\n";
      std::cout << "===========================\n";
      if(single_precision)
        exercise<float, scitbx::matrix::rank_n_update>(m, n);
      else
        exercise<double, scitbx::matrix::rank_n_update>(m, n);
    }
    else {
      help();
      return 1;
    }
  }
  catch(const bad_lexical_cast &) {
    help();
    return 1;
  }
  return 0;
}


// solving A * X = B
// A symmetric/hermitian positive definite in packed format
// factor (potrf()) and solve (potrs())

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/computational/pptrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pptri.hpp>
#include <boost/numeric/bindings/lapack/computational/pptrs.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef float real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::lower, ublas::column_major> symml_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::lower, ublas::column_major> herml_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::upper, ublas::column_major> symmu_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::upper, ublas::column_major> hermu_t;

int main() {

  // for more descriptive comments see ublas_ppsv.cc
  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n = 5;
  size_t nrhs = 2;
  symml_t sal (5, 5);   // symmetric matrix
  symmu_t sau (5, 5);   // symmetric matrix
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  init_symm (sal, 'l');
  init_symm (sau, 'u');

  print_m (sal, "sal");
  cout << endl;
  print_m (sau, "sau");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = prod (sal, x);
  bu = prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  int ierr = lapack::pptrf (sal);
  if (!ierr) {
    symml_t isal (sal);
    lapack::pptrs (sal, bl);
    print_m (bl, "xl");
    cout << endl;
    lapack::pptri (isal);
    print_m (isal, "isal");
  }
  cout << endl;

  ierr = lapack::pptrf (sau);
  if (!ierr) {
    symmu_t isau (sau);
    lapack::pptrs (sau, bu);
    print_m (bu, "xu");
    cout << endl;
    lapack::pptri (isau);
    print_m (isau, "isau");
  }
  cout << endl;

  //////////////////////////////////////////////////////////
  // hermitian
  cout << "\n==========================================\n" << endl;
  cout << "complex hermitian\n" << endl;

  herml_t hal (3, 3);   // hermitian matrix
  hermu_t hau (3, 3);   // hermitian matrix
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  hal (0, 0) = cmplx_t (25, 0);
  hal (1, 0) = cmplx_t (-5, 5);
  hal (1, 1) = cmplx_t (51, 0);
  hal (2, 0) = cmplx_t (10, -5);
  hal (2, 1) = cmplx_t (4, 6);
  hal (2, 2) = cmplx_t (71, 0);

  hau (0, 0) = cmplx_t (25, 0);
  hau (0, 1) = cmplx_t (-5, -5);
  hau (0, 2) = cmplx_t (10, 5);
  hau (1, 1) = cmplx_t (51, 0);
  hau (1, 2) = cmplx_t (4, -6);
  hau (2, 2) = cmplx_t (71, 0);

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  cm_t cbl2 (3, 2);
  cbl2 (0, 0) = cmplx_t (60, -55);
  cbl2 (1, 0) = cmplx_t (34, 58);
  cbl2 (2, 0) = cmplx_t (13, -152);
  cbl2 (0, 1) = cmplx_t (70, 10);
  cbl2 (1, 1) = cmplx_t (-51, 110);
  cbl2 (2, 1) = cmplx_t (75, 63);
  cm_t cbu2 (cbl2);
  print_m (cbl2, "cbl");
  cout << endl;

  ierr = lapack::pptrf (hal);
  if (ierr == 0) {
    herml_t ihal (hal);
    lapack::pptrs (hal, cbl2);
    print_m (cbl2, "cxl");
    cout << endl;
    lapack::pptri (ihal);
    print_m (ihal, "ihal");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

  ierr = lapack::pptrf (hau);
  if (ierr == 0) {
    hermu_t ihau (hau);
    ierr = lapack::pptrs (hau, cbu2);
    print_m (cbu2, "cxu");
    cout << endl;
    lapack::pptri (ihau);
    print_m (ihau, "ihau");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

}


#include <cmath>
#include <armadillo>

#include "double_pendulum.hpp"

/*
* Default constructor for a system with two identical pendulums of size (20, 200), uniforn weight distribution (cenmter of mass at (10, 100))
* and initial thetas of 90º with respect to the downward vertical position.
* The pivots are placed so that both the pivots the center of mass are colinear (one at the top, the other at the bottom of pendulum 1).
*/
DoublePendulum::DoublePendulum() {

    gravity = 9.81;

    theta1 = M_PI / 2;
    theta2 = M_PI / 2;

    R1Length = 200;
    R2Length = 200;

    m1 = 10;
    m2 = 10;

    R1 = { R1Length * sin(theta1), -R1Length * cos(theta1) };
    R2 = { R2Length * sin(theta2), -R2Length * cos(theta2) };

    // Offset of the pivot (y axis)
    double offset = 15;

    // Place pivots exactly at the pendulum's x axis center, and respecting the offset in the y axis
    P1 = { R1Length * 0.05, offset };
    P2 = { R1Length * 0.05, R1Length - offset };

    L1 = { fabs(P1[0] - P2[0]), fabs(P1[1] - P2[1]) };
    L1Length = arma::norm(L1);

    X1 = { R1Length * 0.1 / 2, R1Length / 2 };
    X2 = L1 + R2;

    phi = acos(arma::dot(L1, R1) / (arma::norm(L1) * arma::norm(R1)));

    X1 = R1;
    X2 = L1 + R2;

    I1 = 0;
    I2 = 0;

    sf::Color colorGray(128, 128, 128);

    // Initialize graphical elements that represent the system
    pend1.setSize(sf::Vector2f(R1Length * 0.1, R1Length));
    pend1.setOrigin(P1[0], P1[1]);
    pend1.setFillColor(colorGray);
    pend1.setPosition(1280 / 2, 720 / 2);
    pend1.setOutlineColor(sf::Color::Black);
    pend1.setOutlineThickness(2);

    pivot1.setRadius(3);
    pivot1.setOutlineThickness(1.25);
    pivot1.setFillColor(colorGray);
    pivot1.setOutlineColor(sf::Color::Black);
    pivot1.setOrigin(pivot1.getRadius(), pivot1.getRadius());

    pivot2.setRadius(3);
    pivot2.setOutlineThickness(1.25);
    pivot2.setFillColor(colorGray);
    pivot2.setOutlineColor(sf::Color::Black);
    pivot2.setOrigin(pivot2.getRadius(), pivot2.getRadius());

    pend2.setSize(sf::Vector2f(R2Length * 0.1, R2Length));
    pend2.setOrigin(P2[0], R1Length - P2[1]);
    pend2.setFillColor(colorGray);
    pend2.setOutlineColor(sf::Color::Black);
    pend2.setOutlineThickness(2);

}

void DoublePendulum::draw(sf::RenderWindow *window) {

    // Change positions according to attributes
    pend1.setRotation(-theta1 * 180 / M_PI); // Negative angles to match the references' illustration
    pend2.setRotation(-theta2 * 180 / M_PI);
    pivot1.setPosition(pend1.getTransform().transformPoint(P1[0], P1[1]));
    pivot2.setPosition(pend1.getTransform().transformPoint(P2[0], P2[1]));
    pend2.setPosition(pend1.getTransform().transformPoint(P2[0], P2[1]));

    // Draw new state on `window`
    window->draw(pend1);
    window->draw(pend2);
    window->draw(pivot1);
    window->draw(pivot2);
}


//          Copyright Gavin Band 2008 - 2012.
// 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 <iostream>
#include <boost/bind.hpp>
#include <boost/noncopyable.hpp>
#include "test_case.hpp"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include "metro/ModifiedCholesky.hpp"

// #define DEBUG 1

AUTO_TEST_CASE( test_modified_cholesky_positive_definite ) {
	typedef Eigen::VectorXd Vector ;
	typedef Eigen::MatrixXd Matrix ;
    metro::ModifiedCholesky< Matrix > comp ;
	Eigen::LDLT< Matrix > ldlt ;
	Matrix const identity = Matrix::Identity( 2, 2 ) ;

	{
		Matrix const identity = Matrix::Identity( 1, 1 ) ;
		Matrix test( 1, 1 ) ;
		test << 1 ;
		comp.compute( test ) ;
		ldlt.compute( test ) ;
		BOOST_CHECK_EQUAL( comp.matrixP() * identity, ldlt.transpositionsP() * identity ) ;
		BOOST_CHECK_EQUAL( comp.vectorD(), ldlt.vectorD() ) ;
		BOOST_CHECK_EQUAL( comp.matrixL() * identity, ldlt.matrixL() * identity ) ;
		BOOST_CHECK_EQUAL( comp.solve( identity ) * test, identity ) ;
	}

	{
		Matrix test( 2, 2 ) ;
		test << 1, 0, 0, 1 ;
		comp.compute( test ) ;
		ldlt.compute( test ) ;
		BOOST_CHECK_EQUAL( comp.matrixP() * identity, ldlt.transpositionsP() * identity ) ;
		BOOST_CHECK_EQUAL( comp.vectorD(), ldlt.vectorD() ) ;
		BOOST_CHECK_EQUAL( comp.matrixL() * identity, ldlt.matrixL() * identity ) ;
		BOOST_CHECK_EQUAL( comp.solve( identity ) * test, identity ) ;
	}

	{
		Matrix test( 2, 2 ) ;
		test << 1, 0.5, 0.5, 1 ;
		comp.compute( test ) ;
		ldlt.compute( test ) ;
		BOOST_CHECK_EQUAL( comp.matrixP() * identity, ldlt.transpositionsP() * identity ) ;
		BOOST_CHECK_EQUAL( comp.vectorD(), ldlt.vectorD() ) ;
		BOOST_CHECK_EQUAL( comp.matrixL() * identity, ldlt.matrixL() * identity ) ;
		BOOST_CHECK_EQUAL( comp.solve( identity ) * test, identity ) ;
	}
}

AUTO_TEST_CASE( test_modified_cholesky_nonpositive_definite ) {
	typedef Eigen::VectorXd Vector ;
	typedef Eigen::MatrixXd Matrix ;
    metro::ModifiedCholesky< Matrix > comp ;
	Eigen::LDLT< Matrix > ldlt ;

	Matrix const identity = Matrix::Identity( 2, 2 ) ;

	Matrix test( 2, 2 ) ;
	test << 1, 2, 2, 1 ;

	{
		double offs[9] = { 0, 0.5, 0.9, 0.99, 1.0, 1.01, 1.05, 1.1, 2 } ;
		for( std::size_t i = 0; i < 9; ++i ) {
			test(0,1) = test(1,0) = offs[i] ;
			comp.compute( test ) ;
			BOOST_CHECK_EQUAL( comp.matrixP() * identity, identity ) ;
			// Diagonal should be strictly positive.
			for( int j = 0; j < 2; ++j ) {
				TEST_ASSERT( comp.vectorD()(j) >= std::numeric_limits< double >::epsilon() ) ;
			}

#if DEBUG
			std::cerr << "---------\n" ;
			std::cerr << "test_modified_cholesky_nonpositive_definite(): off-diagonal = " << test(0,1) << ",\n" ;
			std::cerr << "test_modified_cholesky_nonpositive_definite(): original matrix is:\n"
				<< test << ".\n" ;
#endif
			Matrix reconstructed = comp.matrixL() * Matrix( Vector( comp.vectorD() ).asDiagonal() ) * comp.matrixL().transpose() ;
#if DEBUG
			std::cerr << "test_modified_cholesky_nonpositive_definite(): reconstructed matrix is:\n"
				<< reconstructed << ".\n" ;
#endif
			// Modified cholesky is cholesky of a modified matrix with additions of +ve elements
			// to the diagonal.  Check changes are +ve
			BOOST_CHECK( (reconstructed.diagonal() - test.diagonal()).minCoeff() >= 0 ) ;
			// Check that diagonal changes are within error bound
			// (as in Fang & Leary (2006), formula (3)):
			double const beta = std::sqrt( std::max( 1.0, offs[i] / std::sqrt( test.rows() * test.rows() - 1 )) ) ;
			double const errorBound = (
				std::pow( offs[i] / beta + ( test.rows() - 1.0 ) * beta, 2 )
					+ 2 * ( test.diagonal().maxCoeff() + ( test.rows() - 1.0 ) * beta*beta )
					+ std::numeric_limits< double >::epsilon()
			) ;
			BOOST_CHECK( (reconstructed.diagonal() - test.diagonal()).maxCoeff() <= errorBound ) ;
#if DEBUG
			std::cerr << "test_modified_cholesky_nonpositive_definite(): bound is " << errorBound << ".\n" ;
#endif
			// Check that error is confined to diagonal
			{
				Matrix a = reconstructed.triangularView< Eigen::UnitLower >() ;
				Matrix b = test.triangularView< Eigen::UnitLower >() ;
				Matrix c = reconstructed.triangularView< Eigen::UnitUpper >() ;
				Matrix d = test.triangularView< Eigen::UnitUpper >() ;
				BOOST_CHECK( a == b ) ;
				BOOST_CHECK( c == d ) ;
			}

			if( test(0,1) < 1.0 ) {
				// Matrix is +ve definite, we should be able to reconstruct original matrix exactly
				BOOST_CHECK( (reconstructed.diagonal() - test.diagonal()).minCoeff() == 0 ) ;
				Matrix solved = comp.solve( identity ) ;
				BOOST_CHECK_EQUAL( solved * test, identity ) ;
			}
		}
	}
}

AUTO_TEST_CASE( test_modified_cholesky_special_test_cases ) {
	typedef Eigen::VectorXd Vector ;
	typedef Eigen::MatrixXd Matrix ;
	Eigen::LDLT< Matrix > ldlt ;

	Matrix const identity = Matrix::Identity( 2, 2 ) ;

	Matrix test( 4, 4 ) ;

	// This is a problematic 4x4 matrix identified by Schnabel and Eskow.
	// It is discussed in section 6.2 of Fang & Leary (2006), who list
	// its smallest eigenvalue as -0.378 and the maximum error / min eigenvalue
	// as 2.733 for the GMW method.
	test <<
		 1890.3, -1705.6, -315.8,  3000.3,
		-1705.6,  1538.3,  284.9, -2706.6,
		 -315.8,   284.9,   52.5,  -501.2,
		 3000.3, -2706.6, -501.2,  4760.8 ;

	{
		metro::ModifiedCholesky< Matrix > comp ;
		comp.compute( test ) ;

		// Check diagonal is strictly positive.
		for( int j = 0; j < 2; ++j ) {
			TEST_ASSERT( comp.vectorD()(j) >= std::numeric_limits< double >::epsilon() ) ;
		}

#if DEBUG
		std::cerr << "---------\n" ;
		std::cerr << "test_modified_cholesky_special_test_cases(): off-diagonal = " << test(0,1) << ",\n" ;
		std::cerr << "test_modified_cholesky_special_test_cases(): original matrix is:\n"
			<< test << ".\n" ;
#endif
		Matrix reconstructed =
			comp.matrixL()
			* Matrix( Vector( comp.vectorD() ).asDiagonal() )
			* comp.matrixL().transpose() ;
#if DEBUG
		std::cerr << "test_modified_cholesky_special_test_cases(): reconstructed matrix (1) is:\n"
			<< reconstructed << ".\n" ;
#endif
		reconstructed = comp.matrixP().transpose() * reconstructed ;
#if DEBUG
		std::cerr << "test_modified_cholesky_special_test_cases(): reconstructed matrix (2) is:\n"
			<< reconstructed << ".\n" ;
#endif
		reconstructed = reconstructed * comp.matrixP() ;
#if DEBUG
		std::cerr << "test_modified_cholesky_special_test_cases(): reconstructed matrix (full) is:\n"
			<< reconstructed << ".\n" ;
		std::cerr << "test_modified_cholesky_special_test_cases(): permutation matrix is:\n"
			<< ( comp.matrixP() * Matrix::Identity( test.rows(), test.rows() ) ) << ".\n" ;
#endif
		// Modified cholesky is cholesky of a modified matrix with additions of +ve elements
		// to the diagonal.  Check changes are +ve
		BOOST_CHECK( (reconstructed.diagonal() - test.diagonal()).minCoeff() >= 0 ) ;
		// Check that diagonal changes are within error bound
		// (as in Fang & Leary (2006), formula (3)):
		double const beta = std::sqrt( std::max( 4760.8, 3000.3 / std::sqrt( test.rows() * test.rows() - 1 )) ) ;
		double const errorBound = (
			std::pow( 3000.3 / beta + ( test.rows() - 1.0 ) * beta, 2 )
				+ 2 * ( 4760.8 + ( test.rows() - 1.0 ) * beta*beta )
				+ std::numeric_limits< double >::epsilon()
		) ;
		double const fangLearyR2 = 2.7335 ; // Add 0.0005 for rounding
		Matrix const E = reconstructed - test ;
#if DEBUG
		std::cerr << "test_modified_cholesky_special_test_cases(): bound is " << errorBound << ", predicted r2 is: " << fangLearyR2 << "\n" ;
		std::cerr << "test_modified_cholesky_special_test_cases(): max error is " << E.maxCoeff() << ".\n" ;
#endif
		BOOST_CHECK( E.maxCoeff() <= errorBound ) ;
		BOOST_CHECK( E.maxCoeff() / 0.3780759 <= fangLearyR2 ) ;

		// Check that error is confined to diagonal
		{
			Matrix a = reconstructed.triangularView< Eigen::UnitLower >() ;
			Matrix b = test.triangularView< Eigen::UnitLower >() ;
			Matrix c = reconstructed.triangularView< Eigen::UnitUpper >() ;
			Matrix d = test.triangularView< Eigen::UnitUpper >() ;
			BOOST_CHECK( (a - b).array().abs().maxCoeff() < 1E-9 ) ;
			BOOST_CHECK( (c - d).array().abs().maxCoeff() < 1E-9 ) ;
		}
	}
}


//
// Created by chen-tian on 7/26/17.
//

#include "EdgeSE3ProjectDirect.h"
#include <Eigen/Core>
#include <g2o/core/base_unary_edge.h>
#include <g2o/types/sba/types_sba.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace cv;
using namespace g2o;

EdgeSE3ProjectDirect::EdgeSE3ProjectDirect(Eigen::Vector3d point, float fx, float fy, float cx, float cy, Mat *image)
        :x_world_( point ), fx_( fx ), fy_( fy ), cx_( cx ), cy_(cy), image_(image) {};

void EdgeSE3ProjectDirect::computeError()
{
    const VertexSE3Expmap* v = static_cast<const VertexSE3Expmap*> (_vertices[0]);
    Eigen::Vector3d x_local = v->estimate().map( x_world_ );
    float x = x_local[0]*fx_/x_local[2] + cx_;
    float y = x_local[1]*fy_/x_local[2] + cy_;
    //check x,y is in the image
    if ( x-4<0 || (x+4)>image_->cols || (y-4)<0 || (y+4)>image_->rows )
    {
        _error( 0, 0 ) = 0.0;
        this->setLevel(1);
    }

    else
    {
        _error( 0,0 ) = getPixelValue(x, y) - _measurement;
    }

    //plus in manifold
}

void EdgeSE3ProjectDirect::linearizeOplus()
{
    if ( level() == 1 )
    {
        _jacobianOplusXi = Eigen::Matrix< double , 1, 6>::Zero();
        return;
    }

    VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*>(_vertices[0] );
    Eigen::Vector3d xyz_trans = vtx->estimate().map( x_world_ ); //q in book

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double invz = 1.0/xyz_trans[2];
    double invz_2 = invz*invz;

    float u = x*fx_*invz + cx_;
    float v = y*fy_*invz + cy_;

    //jacobian from se3 to u,v
    //NOTE that in g2o the Lie algebra is(\omega, \epsilon), where \omega is so(3) and \epsilon the transilation

    Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

    jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
    jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
    jacobian_uv_ksai ( 0,3 ) = x*invz *fx_;
    jacobian_uv_ksai ( 0,4 ) = 0;
    jacobian_uv_ksai ( 0,5 ) = - x*invz_2 *fx_;

    jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
    jacobian_uv_ksai ( 1,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    jacobian_uv_ksai ( 1,2 ) = x*invz *fx_;
    jacobian_uv_ksai ( 1,3 ) = 0;
    jacobian_uv_ksai ( 1,4 ) = - invz *fy_;
    jacobian_uv_ksai ( 1,5 ) = -y*invz_2*fy_;

    Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;

    jacobian_pixel_uv ( 0,0 ) = ( getPixelValue( u+1,v )-getPixelValue(u-1,v))/2;
    jacobian_pixel_uv ( 0,1 ) = ( getPixelValue( u,v+1 )-getPixelValue(u,v-1))/2;

    _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}

//Bilinear interpolation
inline float EdgeSE3ProjectDirect::getPixelValue(float x, float y)
{
    uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
    float xx = x -floor( x );
    float yy = y -floor( y );
    return float (
            (1-xx) * (1-yy) * data[0] +
            xx* (1-yy) * data[1] +
            (1-xx) * yy * data[ image_->step ] +
            xx*yy*data[image_->step+1]
    );
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing asecd capabilities

     inverse secant in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = asecd(x);
    @endcode

    is similar to:

    @code
    T r =  acosd(rec(x));;
    @endcode

    @see asec

  **/
  const boost::dispatch::functor<tag::asecd_> asecd = {};
} }
#endif

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

#endif


////////////////////////////////////////////////////////////////////////////////
// CircularSector.cc
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Generates a triangle mesh of a unit square with a circular sector cut
//      out. By default the circle is defined by the 2-norm, but a different
//      p-norm can be chosen by the optional p argument.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  10/19/2015 13:59:08
////////////////////////////////////////////////////////////////////////////////
#include <Triangulate.h>
#include <MeshIO.hh>
#include <iostream>

#include <cmath>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

using namespace MeshIO;
using namespace std;
namespace po = boost::program_options;

void usage(int exitVal, const po::options_description &visible_opts) {
    cerr << "Usage: CircularSector.cc [options] mesh" << endl;
    cerr << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("outMesh",       po::value<string>(),                     "output mesh")
        ;
    po::positional_options_description p;
    p.add("outMesh",                1);

    po::options_description visible_opts;

    visible_opts.add_options()("help", "Produce this help message")
        ("sector,s",  po::value<size_t>(),                       "Angular size of sector in units of 2PI/nsubdiv (defaults to full circle)")
        ("radius,r",  po::value<double>()->default_value(0.5),   "Hole radius in (0, 1)")
        ("nsubdiv,n", po::value<size_t>()->default_value(64),    "Number of circle subdivisions")
        ("area,a",    po::value<double>()->default_value(0.001), "Minimum triangle area (for meshing)")
        ("pnorm,p",   po::value<double>()->default_value(2),     "Which lp norm to use in defining circle")
        ("centerx,x", po::value<double>()->default_value(0),     "Allows reloation of 'center' point of sector")
        ("centery,y", po::value<double>()->default_value(0),     "Allows reloation of 'center' point of sector")
        ("skip,S",    po::value<size_t>()->default_value(0),     "number of vertices to skip clockwise and counterclockwise of start (hack to get shape with single reentrant corner)")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cerr << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("outMesh") == 0) {
        cerr << "Error: must specify output mesh" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<IOVertex> holes, outVertices;
    vector<IOElement> outTriangles;

    // Create the square
    vector<IOVertex> inVertices = { {-1, -1, 0},
                                    { 1, -1, 0},
                                    { 1,  1, 0},
                                    {-1,  1, 0} };
    vector<pair<size_t, size_t>> inEdges = { {0, 1}, {1, 2}, {2, 3}, {3, 0} };

    size_t nsubdivs = args["nsubdiv"].as<size_t>();
    double radius = args["radius"].as<double>();
    double p = args["pnorm"].as<double>();
    double degreesPerSubdiv = 2 * M_PI / nsubdivs;
    size_t holeBoundarySegments = nsubdivs; // default to a full circle
    if (args.count("sector"))
        holeBoundarySegments = args["sector"].as<size_t>();

    size_t firstHoleVertex = inVertices.size();
    size_t nskip = args["skip"].as<size_t>();
    // Add the hole if it exists (at least one hole segment).
    if (holeBoundarySegments > 0) {
        inVertices.emplace_back(radius, 0);
        // Add all segments 1..holeBoundarySegments, but if we're drawing a full
        // circle (holeBoundarySegments == nsubdivs), omit the last segment
        for (size_t i = 1 + nskip; i <= (size_t) std::max(int(holeBoundarySegments) - int(nskip), 0) && i < nsubdivs; ++i) {
            double theta = degreesPerSubdiv * i;
            // https://www.mathworks.com/matlabcentral/newsreader/view_thread/279050
            double x = radius * copysign(pow(fabs(cos(theta)), 2 / p), cos(theta)),
                   y = radius * copysign(pow(fabs(sin(theta)), 2 / p), sin(theta));
            inVertices.emplace_back(x, y);
            inEdges.push_back({inVertices.size() - 2,
                               inVertices.size() - 1});
        }
        // If it's not a full circle, we need to add the point at the origin and
        // a segment to it.
        if (holeBoundarySegments < nsubdivs) {
            inVertices.emplace_back(args["centerx"].as<double>(), args["centery"].as<double>());
            inEdges.push_back({inVertices.size() - 2, inVertices.size() - 1});
        }

        // Close the path (draw the last segment in the full circle case, or
        // the last sector edge in the sector case).
        inEdges.push_back({inVertices.size() - 1, firstHoleVertex});

        // Pick a point in the hole: the first segment forms a triangle with the
        // origin that lies entirely within the hole. Choose its barycenter.
        holes.emplace_back(((1 / 3.0) * (inVertices.at(firstHoleVertex    ).point +
                                         inVertices.at(firstHoleVertex + 1).point)).eval());
    }
    // If there's a full circle (except for "skips"), report the corner angle at
    // the first vertex.
    if (holeBoundarySegments == nsubdivs) {
        auto p1 = inVertices.at(firstHoleVertex).point;
        auto p2 = inVertices.at(firstHoleVertex + 1).point;
        auto p3 = inVertices.back().point;
        VectorND<3> e1(p3 - p1), e2(p2 - p1);
        double angle = acos(e1.dot(e2) / (e1.norm() * e2.norm()));
        std::cerr << "corner angle:\t" << angle * (180.0 / M_PI) << std::endl;
    }
    // // Remove holes ourselves--if we have triangle do it, it won't subdivide the
    // // hole boundary...
    // holes.clear();

    // triangulatePSLC(inVertices, inEdges, holes, outVertices, outTriangles, args["area"].as<double>(), "Y");
    triangulatePSLC(inVertices, inEdges, holes, outVertices, outTriangles, args["area"].as<double>(), "Q");
    save(args["outMesh"].as<string>(), outVertices, outTriangles);

    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2006 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, Texas A&M University, 2006
 */

// @sect3{Include files}

// 我们从通常的各种各样的包含文件开始，我们在以前的许多测试中都看到过。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

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

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// 这里是仅有的三个有一些新兴趣的包含文件。第一个文件已经被使用了，例如，用于 VectorTools::interpolate_boundary_values 和 MatrixTools::apply_boundary_values 函数。然而，我们在这里使用该类中的另一个函数， VectorTools::project 来计算我们的初始值，作为连续初始值的 $L^2$ 投影。此外，我们使用  VectorTools::create_right_hand_side  来生成积分  $(f^n,\phi^n_i)$  。这些以前总是由 assemble_system 或应用程序代码中的类似函数手工生成。然而，我们太懒了，不能在这里这么做，所以干脆使用库函数。

#include <deal.II/numerics/vector_tools.h>

// 与此非常相似，我们也懒得写代码来组装质量矩阵和拉普拉斯矩阵，尽管这只需要从以前的任何一个教程程序中复制相关代码。相反，我们想把重点放在这个程序中真正新的东西上，因此使用了 MatrixCreator::create_mass_matrix 和 MatrixCreator::create_laplace_matrix 函数。它们被声明在这里。

#include <deal.II/numerics/matrix_tools.h>

// 最后，这里有一个include文件，它包含了人们有时需要的各种工具函数。特别是，我们需要 Utilities::int_to_string 类，该类在给定一个整数参数后，返回它的字符串表示。它特别有用，因为它允许第二个参数，表明我们希望结果用前导零填充的数字数。我们将用它来写输出文件，其形式为 solution-XXX.vtu where XXX 表示时间步数，并且总是由三位数组成，即使我们仍然处于个位或两位数的时间步数中。

#include <deal.II/base/utilities.h>

// 最后一步和以前所有的程序一样。

namespace Step23
{
  using namespace dealii;
// @sect3{The WaveEquation class}

// 接下来是主类的声明。它的公共函数接口与其他大多数教程程序一样。值得一提的是，我们现在必须存储四个矩阵，而不是一个：质量矩阵  $M$  ，拉普拉斯矩阵  $A$  ，用于求解  $U^n$  的矩阵  $M+k^2\theta^2A$  ，以及用于求解  $V^n$  的带有边界条件的质量矩阵副本。请注意，在周围有一个额外的质量矩阵副本是有点浪费的。我们将在可能的改进部分讨论如何避免这种情况的策略。

// 同样，我们需要 $U^n,V^n$ 的解向量，以及前一个时间步骤 $U^{n-1},V^{n-1}$ 的相应向量。 system_rhs 将用于我们在每个时间步骤中求解两个线性系统之一时的任何右手向量。这些将在两个函数  solve_u  和  solve_v  中解决。

// 最后，变量 theta 用来表示参数 $\theta$ ，该参数用于定义使用哪种时间步进方案，这在介绍中已经说明。剩下的就不言而喻了。

  template <int dim>
  class WaveEquation
  {
  public:
    WaveEquation();
    void run();

  private:
    void setup_system();
    void solve_u();
    void solve_v();
    void output_results() const;

    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> matrix_u;
    SparseMatrix<double> matrix_v;

    Vector<double> solution_u, solution_v;
    Vector<double> old_solution_u, old_solution_v;
    Vector<double> system_rhs;

    double       time_step;
    double       time;
    unsigned int timestep_number;
    const double theta;
  };

//  @sect3{Equation data}

// 在我们继续填写主类的细节之前，让我们定义与问题相对应的方程数据，即解 $u$ 及其时间导数 $v$ 的初始值和边界值，以及一个右手类。我们使用从Function类模板派生出来的类来做这件事，这个模板以前已经用过很多次了，所以下面的内容不应该是一个惊喜。

// 我们从初始值开始，对数值 $u$ 以及它的时间导数，即速度 $v$ 都选择零。

  template <int dim>
  class InitialValuesU : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

  template <int dim>
  class InitialValuesV : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

// 其次，我们有右手边的强制项。无聊的是，我们在这里也选择零。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

// 最后，我们有  $u$  和  $v$  的边界值。它们与介绍中描述的一样，一个是另一个的时间导数。

  template <int dim>
  class BoundaryValuesU : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      if ((this->get_time() <= 0.5) && (p[0] < 0) && (p[1] < 1. / 3) &&
          (p[1] > -1. / 3))
        return std::sin(this->get_time() * 4 * numbers::PI);
      else
        return 0;
    }
  };

  template <int dim>
  class BoundaryValuesV : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      if ((this->get_time() <= 0.5) && (p[0] < 0) && (p[1] < 1. / 3) &&
          (p[1] > -1. / 3))
        return (std::cos(this->get_time() * 4 * numbers::PI) * 4 * numbers::PI);
      else
        return 0;
    }
  };

//  @sect3{Implementation of the WaveEquation class}

// 实际逻辑的实现实际上是相当短的，因为我们把组装矩阵和右手边的向量等事情交给了库。其余的实际代码不超过130行，其中相当一部分是可以从以前的例子程序中获取的模板代码（例如，解决线性系统的函数，或生成输出的函数）。

// 我们从构造函数开始（关于时间步长的选择的解释，请参见介绍中关于Courant, Friedrichs, and Lewy的部分）。

  template <int dim>
  WaveEquation<dim>::WaveEquation()
    : fe(1)
    , dof_handler(triangulation)
    , time_step(1. / 64)
    , time(time_step)
    , timestep_number(1)
    , theta(0.5)
  {}
// @sect4{WaveEquation::setup_system}

// 下一个函数是在程序开始时，也就是在第一个时间步骤之前，设置网格、DoFHandler以及矩阵和向量。如果你已经阅读了至少到 step-6 为止的教程程序，那么前几行是相当标准的。

  template <int dim>
  void WaveEquation<dim>::setup_system()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(7);

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl;

    dof_handler.distribute_dofs(fe);

    std::cout << "Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl
              << std::endl;

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

// 然后，我们必须初始化程序过程中需要的3个矩阵：质量矩阵、拉普拉斯矩阵和在每个时间步长中求解 $M+k^2\theta^2A$ 时使用的矩阵 $U^n$ 。

// 在设置这些矩阵时，请注意它们都是利用了相同的稀疏模式对象。最后，在deal.II中矩阵和稀疏模式是独立对象的原因（与其他许多有限元或线性代数类不同）变得很清楚：在相当一部分应用中，我们必须持有几个恰好具有相同稀疏模式的矩阵，它们没有理由不共享这一信息，而不是重新建立并多次浪费内存。

// 在初始化所有这些矩阵后，我们调用库函数来建立拉普拉斯和质量矩阵。它们所需要的只是一个DoFHandler对象和一个将用于数值积分的正交公式对象。请注意，在许多方面，这些函数比我们通常在应用程序中做的要好，例如，如果一台机器有多个处理器，它们会自动并行构建矩阵：更多信息见WorkStream的文档或 @ref threads "多处理器并行计算 "模块。解决线性系统的矩阵将在run()方法中被填充，因为我们需要在每个时间步长中重新应用边界条件。

    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    matrix_u.reinit(sparsity_pattern);
    matrix_v.reinit(sparsity_pattern);

    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);

// 该函数的其余部分用于将矢量大小设置为正确的值。最后一行关闭了悬挂的节点约束对象。由于我们在一个均匀细化的网格上工作，所以不存在或没有计算过约束条件（即没有必要像其他程序那样调用 DoFTools::make_hanging_node_constraints ），但无论如何，我们需要在下面的一个地方进一步设置一个约束对象。

    solution_u.reinit(dof_handler.n_dofs());
    solution_v.reinit(dof_handler.n_dofs());
    old_solution_u.reinit(dof_handler.n_dofs());
    old_solution_v.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());

    constraints.close();
  }

//  @sect4{WaveEquation::solve_u and WaveEquation::solve_v}

// 接下来的两个函数是解决与  $U^n$  和  $V^n$  的方程有关的线性系统。这两个函数并不特别有趣，因为它们基本沿用了前面所有教程程序中的方案。

// 我们可以对我们要反转的两个矩阵的预处理程序做一些小实验。然而，事实证明，对于这里的矩阵，使用雅可比或SSOR预处理器可以稍微减少解决线性系统所需的迭代次数，但由于应用预处理器的成本，在运行时间方面并不占优势。这也不是什么损失，但让我们保持简单，只做不做。

  template <int dim>
  void WaveEquation<dim>::solve_u()
  {
    SolverControl            solver_control(1000, 1e-8 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(matrix_u, solution_u, system_rhs, PreconditionIdentity());

    std::cout << "   u-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

  template <int dim>
  void WaveEquation<dim>::solve_v()
  {
    SolverControl            solver_control(1000, 1e-8 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(matrix_v, solution_v, system_rhs, PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

//  @sect4{WaveEquation::output_results}

// 同样地，下面的函数也和我们之前做的差不多。唯一值得一提的是，这里我们使用 Utilities::int_to_string 函数的第二个参数，生成了一个用前导零填充的时间步长的字符串表示，长度为3个字符。

  template <int dim>
  void WaveEquation<dim>::output_results() const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution_u, "U");
    data_out.add_data_vector(solution_v, "V");

    data_out.build_patches();

    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtu";

// 像  step-15  一样，由于我们在每个时间步长写输出（而且我们要解决的系统相对简单），我们指示DataOut使用zlib压缩算法，该算法针对速度而不是磁盘使用进行了优化，因为否则绘制输出会成为一个瓶颈。

    DataOutBase::VtkFlags vtk_flags;
    vtk_flags.compression_level =
      DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
    data_out.set_flags(vtk_flags);
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{WaveEquation::run}

// 下面是程序中唯一有趣的功能。它包含了所有时间步骤的循环，但在这之前我们必须设置网格、DoFHandler和矩阵。此外，我们必须以某种方式从初始值开始。为此，我们使用 VectorTools::project 函数，该函数接收一个描述连续函数的对象，并计算该函数在DoFHandler对象所描述的有限元空间的 $L^2$ 投影。没有比这更简单的了。

  template <int dim>
  void WaveEquation<dim>::run()
  {
    setup_system();

    VectorTools::project(dof_handler,
                         constraints,
                         QGauss<dim>(fe.degree + 1),
                         InitialValuesU<dim>(),
                         old_solution_u);
    VectorTools::project(dof_handler,
                         constraints,
                         QGauss<dim>(fe.degree + 1),
                         InitialValuesV<dim>(),
                         old_solution_v);

// 接下来是循环所有的时间步骤，直到我们到达结束时间（本例中为 $T=5$ ）。在每个时间步骤中，我们首先要解决 $U^n$ ，使用方程  $(M^n + k^2\theta^2 A^n)U^n =$  。
// $(M^{n,n-1} - k^2\theta(1-\theta) A^{n,n-1})U^{n-1} + kM^{n,n-1}V^{n-1} +$
// $k\theta \left[k \theta F^n + k(1-\theta) F^{n-1} \right]$  . 请注意，我们在所有的时间步骤中使用相同的网格，因此， $M^n=M^{n,n-1}=M$  和  $A^n=A^{n,n-1}=A$  。因此，我们首先要做的是将 $MU^{n-1} - k^2\theta(1-\theta) AU^{n-1} + kMV^{n-1}$ 和强制项相加，并将结果放入 system_rhs 向量中。(对于这些加法，我们需要在循环之前声明一个临时向量，以避免在每个时间步骤中重复分配内存)。

// 这里需要意识到的是我们如何将时间变量传达给描述右手边的对象：每个从函数类派生出来的对象都有一个时间字段，可以用 Function::set_time 来设置，用 Function::get_time. 来读取。 实质上，使用这种机制，所有空间和时间的函数因此被认为是在某个特定时间评估的空间的函数。这与我们在有限元程序中的典型需求非常吻合，在有限元程序中，我们几乎总是在一个时间步长上工作，而且从来没有发生过，例如，人们想在任何给定的空间位置上为所有时间评估一个时空函数。

    Vector<double> tmp(solution_u.size());
    Vector<double> forcing_terms(solution_u.size());

    for (; time <= 5; time += time_step, ++timestep_number)
      {
        std::cout << "Time step " << timestep_number << " at t=" << time
                  << std::endl;

        mass_matrix.vmult(system_rhs, old_solution_u);

        mass_matrix.vmult(tmp, old_solution_v);
        system_rhs.add(time_step, tmp);

        laplace_matrix.vmult(tmp, old_solution_u);
        system_rhs.add(-theta * (1 - theta) * time_step * time_step, tmp);

        RightHandSide<dim> rhs_function;
        rhs_function.set_time(time);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);
        forcing_terms = tmp;
        forcing_terms *= theta * time_step;

        rhs_function.set_time(time - time_step);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);

        forcing_terms.add((1 - theta) * time_step, tmp);

        system_rhs.add(theta * time_step, forcing_terms);

// 如此构建了第一个方程的右手向量后，我们要做的就是应用正确的边界值。至于右手边，这是一个在特定时间评估的时空函数，我们在边界节点插值，然后像通常那样用结果来应用边界值。然后将结果交给solve_u()函数。

        {
          BoundaryValuesU<dim> boundary_values_u_function;
          boundary_values_u_function.set_time(time);

          std::map<types::global_dof_index, double> boundary_values;
          VectorTools::interpolate_boundary_values(dof_handler,
                                                   0,
                                                   boundary_values_u_function,
                                                   boundary_values);

// solve_u()的矩阵在每个时间步骤中都是相同的，所以人们可以认为只在模拟开始时做一次就足够了。然而，由于我们需要对线性系统应用边界值（消除了一些矩阵的行和列，并对右手边做出了贡献），在实际应用边界数据之前，我们必须在每个时间步骤中重新填充该矩阵。实际内容非常简单：它是质量矩阵和加权拉普拉斯矩阵的总和。

          matrix_u.copy_from(mass_matrix);
          matrix_u.add(theta * theta * time_step * time_step, laplace_matrix);
          MatrixTools::apply_boundary_values(boundary_values,
                                             matrix_u,
                                             solution_u,
                                             system_rhs);
        }
        solve_u();

// 第二步，即求解 $V^n$ ，工作原理类似，只是这次左边的矩阵是质量矩阵（我们再次复制，以便能够应用边界条件，而右边是 $MV^{n-1} - k\left[ \theta A U^n + (1-\theta) AU^{n-1}\right]$ 加上强制项。边界值的应用方式与之前相同，只是现在我们必须使用BoundaryValuesV类。

        laplace_matrix.vmult(system_rhs, solution_u);
        system_rhs *= -theta * time_step;

        mass_matrix.vmult(tmp, old_solution_v);
        system_rhs += tmp;

        laplace_matrix.vmult(tmp, old_solution_u);
        system_rhs.add(-time_step * (1 - theta), tmp);

        system_rhs += forcing_terms;

        {
          BoundaryValuesV<dim> boundary_values_v_function;
          boundary_values_v_function.set_time(time);

          std::map<types::global_dof_index, double> boundary_values;
          VectorTools::interpolate_boundary_values(dof_handler,
                                                   0,
                                                   boundary_values_v_function,
                                                   boundary_values);
          matrix_v.copy_from(mass_matrix);
          MatrixTools::apply_boundary_values(boundary_values,
                                             matrix_v,
                                             solution_v,
                                             system_rhs);
        }
        solve_v();

// 最后，在计算完两个解的组成部分后，我们输出结果，计算解中的能量，并在将现在的解移入持有上一个时间步长的解的向量后，继续下一个时间步长。注意函数 SparseMatrix::matrix_norm_square 可以在一个步骤中计算 $\left<V^n,MV^n\right>$ 和 $\left$ ，为我们节省了一个临时向量和几行代码的费用。

        output_results();

        std::cout << "   Total energy: "
                  << (mass_matrix.matrix_norm_square(solution_v) +
                      laplace_matrix.matrix_norm_square(solution_u)) /
                       2
                  << std::endl;

        old_solution_u = solution_u;
        old_solution_v = solution_v;
      }
  }
} // namespace Step23
// @sect3{The main function}

//剩下的就是程序的主要功能了。这里没有什么是在前面几个程序中没有展示过的。

int main()
{
  try
    {
      using namespace Step23;

      WaveEquation<2> wave_equation_solver;
      wave_equation_solver.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// 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 <mpllibs/metamonad/lazy_metafunction.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/returns.hpp>
#include <mpllibs/metamonad/if_.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/assert.hpp>

using boost::mpl::int_;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::equal_to;
using boost::mpl::less;

using mpllibs::metamonad::lazy;
using mpllibs::metamonad::returns;
using mpllibs::metamonad::if_;

namespace
{
  MPLLIBS_LAZY_METAFUNCTION(not_using_arg, (T)) ((int_<13>));

  MPLLIBS_LAZY_METAFUNCTION(double_value, (N)) ((times<N, int_<2> >));

  MPLLIBS_LAZY_METAFUNCTION(fact, (N))
  ((
    if_<
      less<N, int_<1> >,
      int_<1>,
      lazy<times<fact<minus<N, int_<1> > >, N> >
    >
  ));

  MPLLIBS_LAZY_METAFUNCTION(mult, (A)(B)) ((times<A, B>));
}

BOOST_AUTO_TEST_CASE(test_lazy_metafunction)
{
  using boost::mpl::apply_wrap1;
  using boost::mpl::apply_wrap2;

  // test_lazy_metafunction
  BOOST_MPL_ASSERT((equal_to<int_<6>, double_value<int_<3> >::type>));

  // test_lazy_metafunction_with_two_arguments
  BOOST_MPL_ASSERT((equal_to<int_<6>, mult<int_<2>, int_<3> >::type>));

  // test_currying
  BOOST_MPL_ASSERT((
    equal_to<int_<6>, apply_wrap1<mult<int_<2> >::type, int_<3> >::type>
  ));

  // test_using_metafunction_as_metafunction_class
  BOOST_MPL_ASSERT((
    equal_to<int_<6>, apply_wrap2<mult<>, int_<2>, int_<3> >::type>
  ));

  // test_nested_lazy_metafunction_call
  BOOST_MPL_ASSERT((
    equal_to<int_<24>, mult<mult<int_<2>, int_<3> >, int_<4> >::type>
  ));

  // test_lazy_rec_metafunction
  BOOST_MPL_ASSERT((equal_to<int_<6>, fact<int_<3> >::type>));

  // test_lazy_mf_not_using_arg
  BOOST_MPL_ASSERT((equal_to<int_<13>, not_using_arg<int_<8> >::type>));
}


#ifndef VI_PID_CONTROLLER_HPP
#define VI_PID_CONTROLLER_HPP

// Component
#include "PIDConfig.hpp"
#include "VehicleInterfaceData.hpp"

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/circular_buffer.hpp>

// Ros
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>

// Standard
#include <cstdint>
#include <string>

namespace vi
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class to store config values for vehicle interface
class PIDController
{
public:
   /// @brief Default constructor
   explicit PIDController(const PIDConfig& cfg);

   /// @brief Destructor for forward declares
   ~PIDController();

   /// @brief Main driving function
   /// @param now_s Current program time
   void update(const ros::Time& now_s);

   /// @brief Accessor for command
   /// @return Command
   float64_t getOutput() const noexcept {return m_output;}

   /// @brief Mutator
   /// @param val Val
   /// @{
   void setFeedback(const float64_t val)        noexcept {m_feedback = val;}
   void setCommandSetpoint(const float64_t val) noexcept {m_setpoint = val;}
   /// @}

private:
    /// @brief Helper function to update the delta in time
    /// @param now_s Current real time
    void updateDtS(const ros::Time& now_s) noexcept;

    /// @brief Helper function to update the error
    void updateError() noexcept;

    /// @brief Helpers to calculate various components of the controller
    /// @return Component value
    /// @{
    float64_t calculateP() const noexcept;
    float64_t calculateI()       noexcept;
    float64_t calculateD() const noexcept;
    /// @}

    /// @brief ctl filter helper function
    /// @param input The input to filter into output
    float64_t filterOutput(const float64_t input);

    float64_t m_setpoint{0.0}; ///< Setpoint
    float64_t m_feedback{0.0}; ///< Feedback
    float64_t m_output{0.0};   ///< Output

    ros::Duration m_dt_s;          ///< Current delta time, in seconds
    ros::Time     m_last_time_s;   ///< Last timestamp, used to track deltas

    float64_t m_error{0.0};         ///< Current error
    float64_t m_integral{0.0};      ///< Integral
    float64_t m_last_error{0.0};    ///< Last error (previous iteration)

    boost::circular_buffer<float64_t> m_ctl_buffer; ///< Circular buffer for ctl filter

    PIDConfig m_cfg;           ///< PID Config
};

} // namespace vi

#endif // VI_PID_controller_HPP


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <cassert>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int>> Graph;

void testcase()
{
  int n, m, k, x, y;
  std::cin >> n >> m >> k >> x >> y;
  assert(n >= 1 && n <= 1e4 && m >= 0 && m <= 1e5 && k >= 1 && k <= 10);
  assert(x >= 0 && x < n && y >= 0 && y < n);

  int total_nodes = n * (k + 1);
  auto node_at = [n, k](int index, int layer) {
    assert(index >= 0 && index < n && layer >= 0 && layer <= k);
    return n * layer + index;
  };

  Graph G(total_nodes);
  auto weights = boost::get(boost::edge_weight, G);
  auto add_or_shorten_edge = [&G, weights](int from, int to, int weight) {
    Graph::edge_descriptor test;
    auto existing = boost::edge(from, to, G);
    if (!existing.second || weights[existing.first] > weight)
    {
      boost::add_edge(from, to, weight, G);
    }
  };

  for (int i = 0; i < m; i++)
  {
    int a, b, c, d;
    std::cin >> a >> b >> c >> d;
    assert(a != b && a >= 0 && a < n && b >= 0 && b < n && c >= 1 && c < (2 << 9) && (d == 0 || d == 1));

    for (int j = 0; j <= k; j++)
    {
      if (d == 0 || j == k) // normal road or extra river
      {
        add_or_shorten_edge(node_at(a, j), node_at(b, j), c);
      }
      else // required river
      {
        add_or_shorten_edge(node_at(a, j), node_at(b, j + 1), c);
        add_or_shorten_edge(node_at(a, j + 1), node_at(b, j), c);
      }
    }
  }

  std::vector<int> distances(total_nodes);
  boost::dijkstra_shortest_paths(G, boost::vertex(node_at(x, 0), G), boost::distance_map(boost::make_iterator_property_map(distances.begin(), boost::get(boost::vertex_index, G))));

  std::cout << distances.at(node_at(y, k)) << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


//
// Created by dch on 21/02/18.
//

#include "ImageIOAnalyze.h"
#include "GadgetronTimer.h"
#include "hoNDHarrWavelet.h"
#include "hoNDRedundantWavelet.h"
#include "hoNDArray_math.h"
#include "simplexLagariaSolver.h"
#include "twoParaExpDecayOperator.h"
#include "twoParaExpRecoveryOperator.h"
#include "curveFittingCostFunction.h"
#include "cmr_t1_mapping.h"
#include <gtest/gtest.h>
#include <boost/random.hpp>
#include <chrono>

#include <dlib/optimization.h>

#include <ceres/ceres.h>
#include <numeric>
#define ITERATIONS 10000

class twoParaExpRecovery  {
public:

    twoParaExpRecovery(std::vector<double> x,std::vector<double> y) : x_(x), y_(y) {}
    template <typename T>
            bool operator()(const T* const b, T* e) const {

//        T sign_b1 = (b[1])/ceres::abs(b[1]);
//        T rb = 1.0 / ( (ceres::abs(b[1])< T(FLT_EPSILON)) ? sign_b1*T(FLT_EPSILON) : b[1] );
        T rb = 1.0 /b[1];

        size_t ii;

//        T result = T(0);
        for (ii=0; ii< x_.size(); ii++)
        {
            T tmp = b[0] - b[0] * exp( -1 * x_[ii] * rb);
            tmp -= y_[ii];
//            result += tmp*tmp;
            e[ii] = tmp;
//            e[ii] = tmp*tmp;
        }
//        e[0] = result/T(x_.size());
        return true;
    }

private:
    std::vector<double> x_;
    std::vector<double> y_;
};

void time_ceres(){
    std::vector<double> b;

 auto start = std::chrono::high_resolution_clock::now();
    for (int  i = 0; i < ITERATIONS; i++) {
        ceres::Problem problem;

        std::vector<double> y = {178, 185, 182, 189, 178, 180, 187, 179, 177, 177, 471};
        auto x = std::vector<double>(11, 545);
        x[10] = 10000;
        b = {*std::max_element(y.begin(), y.end()), x[5]};
        auto cost_function = new ceres::AutoDiffCostFunction<twoParaExpRecovery, 11, 2>(
                new twoParaExpRecovery(x, y));
//        auto cost_function = new ceres::NumericDiffCostFunction<twoParaExpRecovery,ceres::RIDDERS,ceres::DYNAMIC,2>(
//                new twoParaExpRecovery(x,y),ceres::DO_NOT_TAKE_OWNERSHIP,1);
        problem.AddResidualBlock(cost_function, NULL, b.data());

        ceres::Solver::Options options;
//        options.max_num_iterations = 15000;
        options.linear_solver_type = ceres::DENSE_QR;
//        options.use_explicit_schur_complement = true;
    options.function_tolerance = 1e-4;
    options.gradient_tolerance = 1e-4;
        options.parameter_tolerance = 1e-4;
//        options.preconditioner_type = ceres::IDENTITY;
//    options.minimizer_type = ceres::LINE_SEARCH;
//        options.line_search_direction_type = ceres::BFGS;
//        options.trust_region_strategy_type = ceres::DOGLEG;
//    options.dogleg_type = ceres::SUBSPACE_DOGLEG;

        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
//    std::cout << summary.FullReport() << std::endl;

//        std::cout << "B" << b[0] << " " << b[1] << std::endl;

    }

     typedef Gadgetron::twoParaExpRecoveryOperator<std::vector<double> > SignalType;
        typedef Gadgetron::leastSquareErrorCostFunction<std::vector<double> > CostType;

        // define solver

        // define signal model
        SignalType t1_sr;

        // define cost function
        CostType lse;
 std::vector<double> y = {178, 185, 182, 189, 178, 180, 187, 179, 177, 177, 471};
        auto x = std::vector<double>(11, 545);
        x[10] = 10000;
      auto cost_function2 = [&y, &x, &t1_sr,&lse](std::vector<double> b) {
//            auto b2 = std::vector<double>{b(0), b(1)};
            auto result = std::vector<double>(2);
            t1_sr.magnitude(x, b, result);

            double tmp = lse.eval(y,result);
            return tmp;

        };

    std::cout <<"Cost " <<  cost_function2(b) << std::endl;

    auto end = std::chrono::high_resolution_clock::now();

    std::cout << "Fitting tookz " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count() << std::endl;
    std::cout << "B " << b[0] << " " << b[1] << std::endl;

}
void time_dlib(){

    double best_cost;

    auto start = std::chrono::high_resolution_clock::now();

    dlib::matrix<double, 2, 1> b;
    for (auto i  = 0; i < ITERATIONS; i++) {
        typedef Gadgetron::twoParaExpRecoveryOperator<std::vector<double> > SignalType;
        typedef Gadgetron::leastSquareErrorCostFunction<std::vector<double> > CostType;

        // define solver

        // define signal model
        SignalType t1_sr;

        // define cost function
        CostType lse;
//
//    solver.signal_model_ = &t1_sr;
//    solver.cf_ = &lse;
//
//    solver.max_iter_ = 150;
//    solver.max_fun_eval_ = 1000;
//    solver.thres_fun_ = 1e-4;
//
//    // set measured points
//    solver.x_.resize(11, 545); // echo time, in ms
//    solver.x_[10] = 10000;

        std::vector<double> y = {178, 185, 182, 189, 178, 180, 187, 179, 177, 177, 471};
        auto x = std::vector<double>(11, 545);
        x[10] = 10000;

        auto cost_function = [&y, &x, &t1_sr,&lse](const dlib::matrix<double, 2, 1> b) {
            auto b2 = std::vector<double>{b(0), b(1)};
            auto result = std::vector<double>(2);
            t1_sr.magnitude(x, b2, result);

            double tmp = lse.eval(y,result);
            return tmp;

        };

        b(0) = *std::max_element(y.begin(), y.end());
        b(1) = x[5];

//        dlib::find_min_bobyqa(cost_function, b, 4, dlib::uniform_matrix<double>(2, 1, 0),
//                              dlib::uniform_matrix<double>(2, 1, 1e5), 100, 1e-4, 150);
        dlib::find_min_using_approximate_derivatives(dlib::bfgs_search_strategy(),dlib::objective_delta_stop_strategy(1e-4),cost_function,b,-1);
//        dlib::find_min_
//        auto result = dlib::find_min_global(cost_function,{0,0},{1e5,1e5},dlib::max_function_calls(10000));
        best_cost = cost_function(b);

    }
    auto end = std::chrono::high_resolution_clock::now();

    std::cout << "Fitting tookz " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count() << std::endl;

    std::cout << "Best cost " << best_cost << " " << b << std::endl;
}
void time_gadgetron(){

    double best_cost;
    std::vector<float> b(2);
    auto start = std::chrono::system_clock::now();
    for (auto i = 0; i < ITERATIONS; i++) {
        typedef Gadgetron::twoParaExpRecoveryOperator<std::vector<float> > SignalType;
        typedef Gadgetron::leastSquareErrorCostFunction<std::vector<float> > CostType;

        // define solver
        Gadgetron::simplexLagariaSolver<std::vector<float>, SignalType, CostType> solver;

        // define signal model
        SignalType t1_sr;

        // define cost function
        CostType lse;

        solver.signal_model_ = &t1_sr;
        solver.cf_ = &lse;

        solver.max_iter_ = 1500;
        solver.max_fun_eval_ = 10000;
        solver.thres_fun_ = 1e-6;

        // set measured points
        solver.x_.resize(11, 545); // echo time, in ms
        solver.x_[10] = 10000;

        solver.y_.resize(11); // intensity
        solver.y_[0] = 178;
        solver.y_[1] = 185;
        solver.y_[2] = 182;
        solver.y_[3] = 189;
        solver.y_[4] = 178;
        solver.y_[5] = 180;
        solver.y_[6] = 187;
        solver.y_[7] = 179;
        solver.y_[8] = 177;
        solver.y_[9] = 177;
        solver.y_[10] = 471;

        std::vector<float> guess(2, 0);
        b[0] = 0;
        b[1] = 0;

        guess[0] = *std::max_element(solver.y_.begin(), solver.y_.end());
        guess[1] = solver.x_[solver.x_.size() / 2];

        solver.solve(b, guess);
        best_cost = solver.best_cost_;
    }
    auto end = std::chrono::system_clock::now();

    std::cout << "Fitting tookz " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count() << std::endl;
    std::cout << "Best cost " << best_cost << " " << b[0] << " " << b[1] <<  std::endl;
}
using namespace Gadgetron;
int main(){
    time_gadgetron();
    time_dlib();
    time_ceres();
}