/**
 * @date Sat Mar 19 22:14:10 2011 +0100
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/shared_array.hpp>

#include <bob.math/svd.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>

// Declaration of the external LAPACK function (Divide and conquer SVD)
extern "C" void dgesdd_( const char *jobz, const int *M, const int *N,
  double *A, const int *lda, double *S, double *U, const int* ldu, double *VT,
  const int *ldvt, double *work, const int *lwork, int *iwork, int *info);

// Declaration of the external LAPACK function ('Slow' but 'safe' SVD)
extern "C" void dgesvd_( const char *jobu, const char *jobvt, const int *M,
  const int *N, double *A, const int *lda, double *S, double *U,
  const int* ldu, double *VT, const int *ldvt, double *work, const int *lwork,
  int *info);

static void svd_lapack( const char jobz, const int M, const int N,
  double *A, const int lda, double *S, double *U, const int ldu, double *VT,
  const int ldvt, const bool safe)
{
  // Calls the LAPACK function:
  // We use dgesdd by default which is faster than its predecessor dgesvd,
  // when computing the singular vectors.
  //   (cf. http://www.netlib.org/lapack/lug/node71.html)
  // However, dgesdd is failing on some matrices:
  //   see #171: http://github.com/idiap/bob/issues/171
  // Please note that matlab is relying on dgesvd.
  int info = 0;
  if (safe) {
    // A/ Queries the optimal size of the working array
    const int lwork_query = -1;
    double work_query;
    dgesvd_( &jobz, &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, &work_query, &lwork_query, &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesvd function returned a non-zero value.");

    // B/ Computes
    const int lwork = static_cast<int>(work_query);
    boost::shared_array<double> work(new double[lwork]);
    dgesvd_( &jobz, &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, work.get(), &lwork, &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesvd function returned a non-zero value.");
  }
  else {
    // Integer (workspace) array, dimension (8*min(M,N))
    const int l_iwork = 8*std::min(M,N);
    boost::shared_array<int> iwork(new int[l_iwork]);

    // A/ Queries the optimal size of the working array
    const int lwork_query = -1;
    double work_query;
    dgesdd_( &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, &work_query, &lwork_query, iwork.get(), &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesdd function returned a non-zero value. You may consider using LAPACK dgsevd instead (see #171) by enabling the 'safe' option.");

    // B/ Computes
    const int lwork = static_cast<int>(work_query);
    boost::shared_array<double> work(new double[lwork]);
    dgesdd_( &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, work.get(), &lwork, iwork.get(), &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesdd function returned a non-zero value. You may consider using LAPACK dgsevd instead (see #171) by enabling the 'safe' option.");
  }

  // Defining the sign of the eigenvectors
  // Approch extracted from page 8 - http://prod.sandia.gov/techlib/access-control.cgi/2007/076422.pdf
  if(U[0] < 0){
    int ucol=0; ucol= (jobz=='A')? M : std::min(M,N);
    for (int i=0; i<ldu*ucol; i++){
      U[i] = -1*U[i];
    }

    for (int i=0; i<ldvt*N; i++){
      VT[i] = -1*VT[i];
    }
  }
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, blitz::Array<double,2>& Vt, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(U);
  bob::core::array::assertZeroBase(sigma);
  bob::core::array::assertZeroBase(Vt);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(U.extent(0), M);
  bob::core::array::assertSameDimensionLength(U.extent(1), M);
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);
  bob::core::array::assertSameDimensionLength(Vt.extent(0), N);
  bob::core::array::assertSameDimensionLength(Vt.extent(1), N);

  bob::math::svd_(A, U, sigma, Vt, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, blitz::Array<double,2>& Vt, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function:
  // We will decompose A^T rather than A to reduce the required number of copy
  // We recall that FORTRAN/LAPACK is column-major order whereas blitz arrays
  // are row-major order by default.
  // If A = U.S.V^T, then A^T = V.S.U^T

  // Initialises LAPACK variables
  const char jobz = 'A'; // Get All left singular vectors
  const int lda = N;
  const int ldu = N;
  const int ldvt = M;
  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(bob::core::array::ccopy(A));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use U, Vt and S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  // U_lapack = V^T
  blitz::Array<double,2> U_blitz_lapack;
  const bool U_direct_use = bob::core::array::isCZeroBaseContiguous(Vt);
  if (!U_direct_use) U_blitz_lapack.resize(N,N);
  else               U_blitz_lapack.reference(Vt);
  double *U_lapack = U_blitz_lapack.data();
  // V^T_lapack = U
  blitz::Array<double,2> VT_blitz_lapack;
  const bool VT_direct_use = bob::core::array::isCZeroBaseContiguous(U);
  if (!VT_direct_use) VT_blitz_lapack.resize(M,M);
  else                VT_blitz_lapack.reference(U);
  double *VT_lapack = VT_blitz_lapack.data();

  // Call the LAPACK function
  svd_lapack(jobz, N, M, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack, ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!U_direct_use)  Vt = U_blitz_lapack;
  if (!VT_direct_use) U = VT_blitz_lapack;
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(U);
  bob::core::array::assertZeroBase(sigma);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(U.extent(0), M);
  bob::core::array::assertSameDimensionLength(U.extent(1), nb_singular);
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);

  bob::math::svd_(A, U, sigma, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function

  // Initialises LAPACK variables
  const char jobz = 'S'; // Get first min(M,N) columns of U
  const int lda = M;
  const int ldu = M;
  const int ldvt = std::min(M,N);

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use U and S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  // U_lapack = U^T
  blitz::Array<double,2> U_blitz_lapack;
  blitz::Array<double,2> Ut = U.transpose(1,0);
  const bool U_direct_use = bob::core::array::isCZeroBaseContiguous(Ut);
  if (!U_direct_use) U_blitz_lapack.resize(nb_singular,M);
  else               U_blitz_lapack.reference(Ut);
  double *U_lapack = U_blitz_lapack.data();
  boost::shared_array<double> VT_lapack(new double[nb_singular*N]);

  // Call the LAPACK function
  svd_lapack(jobz, M, N, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack.get(), ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!U_direct_use) Ut = U_blitz_lapack;
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(sigma);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);

  bob::math::svd_(A, sigma, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function

  // Initialises LAPACK variables
  const char jobz = 'N'; // Get first min(M,N) columns of U
  const int lda = M;
  const int ldu = M;
  const int ldvt = std::min(M,N);

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  double *U_lapack = 0;
  double *VT_lapack = 0;

  // Call the LAPACK function
  svd_lapack(jobz, M, N, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack, ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}


// Copyright 2002 Rensselaer Polytechnic Institute

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

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{
namespace detail
{
    template < typename T, typename BinaryPredicate >
    T min_with_compare(const T& x, const T& y, const BinaryPredicate& compare)
    {
        if (compare(x, y))
            return x;
        else
            return y;
    }

    template < typename VertexListGraph, typename DistanceMatrix,
        typename BinaryPredicate, typename BinaryFunction, typename Infinity,
        typename Zero >
    bool floyd_warshall_dispatch(const VertexListGraph& g, DistanceMatrix& d,
        const BinaryPredicate& compare, const BinaryFunction& combine,
        const Infinity& inf, const Zero& zero)
    {
        typename graph_traits< VertexListGraph >::vertex_iterator i, lasti, j,
            lastj, k, lastk;

        for (boost::tie(k, lastk) = vertices(g); k != lastk; k++)
            for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
                if (d[*i][*k] != inf)
                    for (boost::tie(j, lastj) = vertices(g); j != lastj; j++)
                        if (d[*k][*j] != inf)
                            d[*i][*j] = detail::min_with_compare(d[*i][*j],
                                combine(d[*i][*k], d[*k][*j]), compare);

        for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
            if (compare(d[*i][*i], zero))
                return false;
        return true;
    }
}

template < typename VertexListGraph, typename DistanceMatrix,
    typename BinaryPredicate, typename BinaryFunction, typename Infinity,
    typename Zero >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d, const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< VertexListGraph >));

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
}

template < typename VertexAndEdgeListGraph, typename DistanceMatrix,
    typename WeightMap, typename BinaryPredicate, typename BinaryFunction,
    typename Infinity, typename Zero >
bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const WeightMap& w, const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< VertexAndEdgeListGraph >));
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< VertexAndEdgeListGraph >));
    BOOST_CONCEPT_ASSERT((IncidenceGraphConcept< VertexAndEdgeListGraph >));

    typename graph_traits< VertexAndEdgeListGraph >::vertex_iterator firstv,
        lastv, firstv2, lastv2;
    typename graph_traits< VertexAndEdgeListGraph >::edge_iterator first, last;

    for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        for (boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2;
             firstv2++)
            d[*firstv][*firstv2] = inf;

    for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        d[*firstv][*firstv] = zero;

    for (boost::tie(first, last) = edges(g); first != last; first++)
    {
        if (d[source(*first, g)][target(*first, g)] != inf)
        {
            d[source(*first, g)][target(*first, g)]
                = detail::min_with_compare(get(w, *first),
                    d[source(*first, g)][target(*first, g)], compare);
        }
        else
            d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<
        typename graph_traits< VertexAndEdgeListGraph >::directed_category,
        undirected_tag >::value;
    if (is_undirected)
    {
        for (boost::tie(first, last) = edges(g); first != last; first++)
        {
            if (d[target(*first, g)][source(*first, g)] != inf)
                d[target(*first, g)][source(*first, g)]
                    = detail::min_with_compare(get(w, *first),
                        d[target(*first, g)][source(*first, g)], compare);
            else
                d[target(*first, g)][source(*first, g)] = get(w, *first);
        }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
}

namespace detail
{
    template < class VertexListGraph, class DistanceMatrix, class WeightMap,
        class P, class T, class R >
    bool floyd_warshall_init_dispatch(const VertexListGraph& g,
        DistanceMatrix& d, WeightMap /*w*/,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< WeightMap >::value_type WM;
        WM inf = choose_param(get_param(params, distance_inf_t()),
            std::numeric_limits< WM >::max BOOST_PREVENT_MACRO_SUBSTITUTION());

        return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
            choose_param(
                get_param(params, distance_compare_t()), std::less< WM >()),
            choose_param(get_param(params, distance_combine_t()),
                closed_plus< WM >(inf)),
            inf, choose_param(get_param(params, distance_zero_t()), WM()));
    }

    template < class VertexAndEdgeListGraph, class DistanceMatrix,
        class WeightMap, class P, class T, class R >
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g,
        DistanceMatrix& d, WeightMap w,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< WeightMap >::value_type WM;

        WM inf = choose_param(get_param(params, distance_inf_t()),
            std::numeric_limits< WM >::max BOOST_PREVENT_MACRO_SUBSTITUTION());
        return floyd_warshall_all_pairs_shortest_paths(g, d, w,
            choose_param(
                get_param(params, distance_compare_t()), std::less< WM >()),
            choose_param(get_param(params, distance_combine_t()),
                closed_plus< WM >(inf)),
            inf, choose_param(get_param(params, distance_zero_t()), WM()));
    }

} // namespace detail

template < class VertexListGraph, class DistanceMatrix, class P, class T,
    class R >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params< P, T, R >& params)
{
    return detail::floyd_warshall_init_dispatch(g, d,
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        params);
}

template < class VertexListGraph, class DistanceMatrix >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d)
{
    bgl_named_params< int, int > params(0);
    return detail::floyd_warshall_init_dispatch(
        g, d, get(edge_weight, g), params);
}

template < class VertexAndEdgeListGraph, class DistanceMatrix, class P, class T,
    class R >
bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const bgl_named_params< P, T, R >& params)
{
    return detail::floyd_warshall_noninit_dispatch(g, d,
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        params);
}

template < class VertexAndEdgeListGraph, class DistanceMatrix >
bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d)
{
    bgl_named_params< int, int > params(0);
    return detail::floyd_warshall_noninit_dispatch(
        g, d, get(edge_weight, g), params);
}

} // namespace boost

#endif


#include <eve/function/ellint_3.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <eve/wide.hpp>
#include <iostream>

using wide_ft = eve::wide<float, eve::fixed<4>>;

int main()
{
  wide_ft pf = {0.1f, 0.0f, 0.2f, 3.0f};
  wide_ft qf = {0.1f, 1.5f, 0.2f, 0.5f};
  wide_ft rf = {2.0f, 1.0f, 0.1f, 1.0f};

  std::cout << "---- simd" << '\n'
            << "<- pf                    = " << pf << '\n'
            << "<- qf                    = " << qf << '\n'
            << "<- rf                    = " << rf << '\n'
            << "-> ellint_3(pf, qf, rf) = " << eve::ellint_3(pf, qf, rf) << '\n';

  float xf = 3.0f;
  float yf = 0.5f;
  float zf = 1.0f;

  std::cout << "---- scalar" << '\n'
            << "<- xf                    = " << xf << '\n'
            << "<- yf                    = " << yf << '\n'
            << "<- zf                    = " << zf << '\n'
            << "-> ellint_3(xf, yf, zf) = " << eve::ellint_3(xf, yf, zf) << '\n';

  return 0;
}


// Implement member functions for SimplicialComplexOperators class.
#include "simplicial-complex-operators.h"
#include "geometrycentral/numerical/linear_algebra_utilities.h"
#include <Eigen/Sparse>
#include <cassert>

using namespace geometrycentral;
using namespace geometrycentral::surface;

/*
 * Assign a unique index to each vertex, edge, and face of a mesh.
 * All elements are 0-indexed.
 *
 * Input: None. Access geometry via the member variable <geometry>, and pointer to the mesh via <mesh>.
 * Returns: None.
 */
void SimplicialComplexOperators::assignElementIndices() {

    // Needed to access geometry->vertexIndices, etc. as cached quantities.
    // Not needed if you're just using v->getIndex(), etc.
    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();

    // You can set the index field of a vertex via geometry->vertexIndices[v], where v is a Vertex object (or an
    // integer). Similarly you can do edges and faces via geometry->edgeIndices, geometry->faceIndices, like so:
    size_t idx = 0;
    for (Vertex v : mesh->vertices()) {
        idx = geometry->vertexIndices[v];
    }

    for (Edge e : mesh->edges()) {
        idx = geometry->edgeIndices[e];
    }

    for (Face f : mesh->faces()) {
        idx = geometry->faceIndices[f];
    }

    // You can more easily get the indices of mesh elements using the function getIndex(), albeit less efficiently and
    // technically less safe (although you don't need to worry about it), like so:
    //
    //      v.getIndex()
    //
    // where v can be a Vertex, Edge, Face, Halfedge, etc. For example:

    for (Vertex v : mesh->vertices()) {
        idx = v.getIndex(); // == geometry->vertexIndices[v])
    }

    // Geometry Central already sets the indices for us, though, so this function is just here for demonstration.
    // You don't have to do anything :)
}

/*
 * Construct the unsigned vertex-edge adjacency matrix A0.
 *
 * Input:
 * Returns: The sparse vertex-edge adjacency matrix which gets stored in the global variable A0.
 */
SparseMatrix<size_t> SimplicialComplexOperators::buildVertexEdgeAdjacencyMatrix() const {

    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();
    // Note: You can build an Eigen sparse matrix from triplets, then return it as a Geometry Central SparseMatrix.
    // See <https://eigen.tuxfamily.org/dox/group__TutorialSparse.html> for documentation.

    // initialize eigensparse matrix
    SparseMatrix<size_t> mat(mesh->nEdges(),mesh->nVertices());

    for (Edge e : mesh->edges()){
        // we compute by edge since we know there will only be 2 per row
        // enter sparse matrix values one by one:
        size_t i1 = geometry->vertexIndices[e.firstVertex()];
        size_t i2 = geometry->vertexIndices[e.secondVertex()];
        size_t e_i = geometry->edgeIndices[e];

        mat.insert(e_i,i1)=1;
        mat.insert(e_i,i2)=1;

    }

    return mat;
}

/*
 * Construct the unsigned face-edge adjacency matrix A1.
 *
 * Input:
 * Returns: The sparse face-edge adjacency matrix which gets stored in the global variable A1.
 */
SparseMatrix<size_t> SimplicialComplexOperators::buildFaceEdgeAdjacencyMatrix() const {

    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();

    // initialize eigensparse matrix
    SparseMatrix<size_t> mat(mesh->nFaces(),mesh->nEdges());

    for (Halfedge he : mesh->halfedges()){

        // only count interior edges
        if(he.isInterior()){
        // enter sparse matrix values one by one:
        size_t e = geometry->edgeIndices[he.edge()];

        size_t f = geometry->faceIndices[he.face()];

        mat.insert(f,e)=1;
        }

    }
    return mat;
}

/*
 * Construct a vector encoding the vertices in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |V|, where |V| = # of vertices in the mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildVertexVector(const MeshSubset& subset) const {

    Vector<size_t> vertices= Vector<size_t>::Zero(mesh->nVertices(),1);

    std::set<size_t>::iterator it;

    for (it = subset.vertices.begin(); it!= subset.vertices.end(); it++){
        vertices[*it]=1;
    }

    return vertices;
}

/*
 * Construct a vector encoding the edges in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |E|, where |E| = # of edges in mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildEdgeVector(const MeshSubset& subset) const {

    Vector<size_t> edges= Vector<size_t>::Zero(mesh->nEdges(),1);

    std::set<size_t>::iterator it;

    for (it = subset.edges.begin(); it!= subset.edges.end(); it++){
         edges[*it]=1;
    }

    return edges;
}

/*
 * Construct a vector encoding the faces in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |F|, where |F| = # of faces in mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildFaceVector(const MeshSubset& subset) const {

    Vector<size_t> faces= Vector<size_t>::Zero(mesh->nFaces(),1);

    std::set<size_t>::iterator it;

    for (it = subset.faces.begin(); it!= subset.faces.end(); it++){
         faces[*it]=1;
    }

    return faces;
}

/*
 * Compute the simplicial star St(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The star of the given subset.
 */
MeshSubset SimplicialComplexOperators::star(const MeshSubset& subset) const {

    MeshSubset star = subset;

    Vector<size_t> E0;
    Vector<size_t> F0;
    Vector<size_t> F1;

    std::set<size_t> set_E0;
    std::set<size_t> set_F0;
    std::set<size_t> set_F1;

    Vector<size_t> sub_verts = buildVertexVector(subset);
    Vector<size_t> sub_edges = buildEdgeVector(subset);
    Vector<size_t> sub_faces = buildFaceVector(subset);

    // Compute all connected vertices in the vectors
    E0 = A0 * sub_verts;
    F0 = A1 * A0 * sub_verts;
    F1 = A1 * sub_edges;

    // fill edge sets
    for(int i =0; i<E0.rows(); ++i){
        if(E0[i] != 0){
            set_E0.insert(i);
        }
    }
    // fill face sets
    for(int i =0; i<F0.rows(); ++i){
        if(F0[i] != 0){
            set_F0.insert(i);
        }
        if(F1[i] != 0){
            set_F1.insert(i);
        }
    }

    star.addEdges(set_E0);
    star.addFaces(set_F0);
    star.addFaces(set_F1);

    return star;
}

/*
 * Compute the closure Cl(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The closure of the given subset.
 */
MeshSubset SimplicialComplexOperators::closure(const MeshSubset& subset) const {

    // final returned subset
    MeshSubset closure = subset;

    // vectors for computation
    Vector<size_t> V0;
    Vector<size_t> E1;
    Vector<size_t> V1;

    // sets for storing indices
    std::set<size_t> set_V0;
    std::set<size_t> set_E1;
    std::set<size_t> set_V1;

    // Only need the subset edges and faces to compute the closure
    Vector<size_t> sub_edges = buildEdgeVector(subset);
    Vector<size_t> sub_faces = buildFaceVector(subset);

    // Add vertices that touch edges
    V0 = A0.transpose()*sub_edges;
    for(int i =0; i<V0.rows(); ++i){
        if(V0[i] != 0){
            set_V0.insert(i);
        }
    }
    closure.addVertices(set_V0);

    // Add edges that touch faces
    E1 = A1.transpose()*sub_faces;
    for(int i =0; i<E1.rows(); i++){
        if(E1[i] != 0){
            set_E1.insert(i);
        }
    }
    closure.addEdges(set_E1);

    // Add edges that touch faces
    V1 =A0.transpose()*A1.transpose()*sub_faces;
    for(int i =0; i<V1.rows(); i++){
        if(V1[i] != 0){
            set_V1.insert(i);
        }
    }
    closure.addVertices(set_V1);

    return closure;
    }

/*
 * Compute the link Lk(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The link of the given subset.
 */
MeshSubset SimplicialComplexOperators::link(const MeshSubset& subset) const {

    // define Cl(St(subset)) and St(Cl(subset))
    MeshSubset ClSt = closure(star(subset));
    MeshSubset StCl = star(closure(subset));

    // to remove StCl from ClSt we pick out the set of faces, edges and vertices of StCl
    std::set<size_t> v = StCl.vertices;
    std::set<size_t> e = StCl.edges;
    std::set<size_t> f = StCl.faces;

    // initialize link as Cl(St(subset))
    MeshSubset link = ClSt;

    link.deleteVertices(v);
    link.deleteEdges(e);
    link.deleteFaces(f);

    return link;
}

/*
 * Return true if the selected subset is a simplicial complex, false otherwise.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: True if given subset is a simplicial complex, false otherwise.
 */
bool SimplicialComplexOperators::isComplex(const MeshSubset& subset) const {

    MeshSubset Cl;
    Cl = closure(subset);

    // a potentially faster method that only compares sizes rather than each elt
    size_t v0 = subset.vertices.size();
    size_t e0 = subset.edges.size();
    size_t f0 = subset.faces.size();
    size_t v1 = Cl.vertices.size();
    size_t e1 = Cl.edges.size();
    size_t f1 = Cl.faces.size();

    return (v0==v1 && e0==e1 && f0==f1);
    //return closure==subset;

}

/*
 * Check if the given subset S is a pure simplicial complex. If so, return the degree of the complex. Otherwise, return
 * -1.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: int representing the degree of the given complex (-1 if not pure)
 */
int SimplicialComplexOperators::isPureComplex(const MeshSubset& subset) const {

    if(isComplex(subset)){
        Vector<size_t> sub_verts = buildVertexVector(subset);
        Vector<size_t> sub_edges = buildEdgeVector(subset);
        Vector<size_t> sub_faces = buildFaceVector(subset);

        if(subset.faces.size()!=0){
            // make sure every edge and vertex is part of a face
            Vector<size_t> new_edges = A1.transpose() * sub_faces;
            for(int i =0; i<new_edges.rows(); i++){
                if(new_edges[i] != 0){
                    new_edges[i]=1;
                }
            }
            Vector<size_t> new_verts = A0.transpose() * A1.transpose() * sub_faces;
            for(int i =0; i<new_verts.rows(); i++){
                if(new_verts[i] != 0){
                    new_verts[i]=1;
                }
            }

            if(new_edges == sub_edges && new_verts == sub_verts){
                return 2;
            }
            else{
                return -1;
            }
        }

        else if(subset.edges.size()!=0){
            // make sure every vertex is part of an edge
            Vector<size_t> new_verts = A0.transpose() * sub_edges;
            for(int i =0; i<new_verts.rows(); i++){
                if(new_verts[i] != 0){
                    new_verts[i]=1;
                }
            }

            if(new_verts == sub_verts){
                return 1;
            }
            else{
                return -1;
            }
        }

        // if only vertices we are done
        else{
            return 0;
        }
    }
    return -1;
}

/*
 * Compute the set of simplices contained in the boundary bd(S) of the selected subset S of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The boundary of the given subset.
 */
MeshSubset SimplicialComplexOperators::boundary(const MeshSubset& subset) const {

    MeshSubset boundary;

    // find simplicial complex degree
    int deg = isPureComplex(subset);

    // if not pure complex, return empty placeholder
    // (or error?)
    if(deg == -1){
        return boundary;
    }

    // if subset is points, return empty placeholder
    else if(deg == 0){
        return boundary;
    }

    // 1-simplicial complex, return points connected to one edge
    else if(deg ==1){
        // only vertices that touch 1 edge
        // first find vertex connection vector
        Vector<size_t> edges = buildEdgeVector(subset);
        Vector<size_t> vertices = A0.transpose()*edges;
        std::set<size_t> int_verts_set;

        // fill bd_edges_set and turn edges into boundary edges
        for(int i =0; i<vertices.rows(); i++){
            if(vertices[i] == 1){
                int_verts_set.insert(i);
            }
        }
        boundary.addVertices(int_verts_set);
    }

    // since boundaries are simplicial complexes of degree one lower:
    // we can only find the boundary edges, then take the closure at the end
    else if(deg ==2){
        // first find edges connected to one face
        // then take the closure
        Vector<size_t> faces = buildFaceVector(subset);
        Vector<size_t> edges = A1.transpose()*faces;
        std::set<size_t> int_edges_set;
        MeshSubset bdry_only_edges;

        for(int i =0; i<edges.rows(); i++){
            if(edges[i] == 1){
                int_edges_set.insert(i);
            }
        }
        bdry_only_edges.addEdges(int_edges_set);

        boundary = closure(bdry_only_edges);

    }

    return boundary;
    }


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int s, p; cin >> s >> p;
    for (cpp_int i = 1; i <= sqrt(p); i++) {
        if (p % i == 0) {
            if (i + p / i == s) {
                cout << "Yes" << endl;
                return 0;
            }
        }
    }
    cout << "No" << endl;
    return 0;
}


// Copyright John Maddock 2006, 2007.
// Copyright Paul A. Bristow 2007.

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

#ifndef BOOST_STATS_CAUCHY_HPP
#define BOOST_STATS_CAUCHY_HPP

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4127) // conditional expression is constant
#endif

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#include <utility>

namespace boost{ namespace math
{

template <class RealType, class Policy>
class cauchy_distribution;

namespace detail
{

template <class RealType, class Policy>
RealType cdf_imp(const cauchy_distribution<RealType, Policy>& dist, const RealType& x, bool complement)
{
   //
   // This calculates the cdf of the Cauchy distribution and/or its complement.
   //
   // The usual formula for the Cauchy cdf is:
   //
   // cdf = 0.5 + atan(x)/pi
   //
   // But that suffers from cancellation error as x -> -INF.
   //
   // Recall that for x < 0:
   //
   // atan(x) = -pi/2 - atan(1/x)
   //
   // Substituting into the above we get:
   //
   // CDF = -atan(1/x)  ; x < 0
   //
   // So the proceedure is to calculate the cdf for -fabs(x)
   // using the above formula, and then subtract from 1 when required
   // to get the result.
   //
   BOOST_MATH_STD_USING // for ADL of std functions
   static const char* function = "boost::math::cdf(cauchy<%1%>&, %1%)";
   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_location(function, location, &result, Policy()))
   {
     return result;
   }
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
   { // cdf +infinity is unity.
     return static_cast<RealType>((complement) ? 0 : 1);
   }
   if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
   { // cdf -infinity is zero.
     return static_cast<RealType>((complement) ? 1 : 0);
   }
   if(false == detail::check_x(function, x, &result, Policy()))
   { // Catches x == NaN
      return result;
   }
   RealType mx = -fabs((x - location) / scale); // scale is > 0
   if(mx > -tools::epsilon<RealType>() / 8)
   {  // special case first: x extremely close to location.
      return 0.5;
   }
   result = -atan(1 / mx) / constants::pi<RealType>();
   return (((x > location) != complement) ? 1 - result : result);
} // cdf

template <class RealType, class Policy>
RealType quantile_imp(
      const cauchy_distribution<RealType, Policy>& dist,
      const RealType& p,
      bool complement)
{
   // This routine implements the quantile for the Cauchy distribution,
   // the value p may be the probability, or its complement if complement=true.
   //
   // The procedure first performs argument reduction on p to avoid error
   // when calculating the tangent, then calulates the distance from the
   // mid-point of the distribution.  This is either added or subtracted
   // from the location parameter depending on whether `complement` is true.
   //
   static const char* function = "boost::math::quantile(cauchy<%1%>&, %1%)";
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_location(function, location, &result, Policy()))
   {
     return result;
   }
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_probability(function, p, &result, Policy()))
   {
      return result;
   }
   // Special cases:
   if(p == 1)
   {
      return (complement ? -1 : 1) * policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   if(p == 0)
   {
      return (complement ? 1 : -1) * policies::raise_overflow_error<RealType>(function, 0, Policy());
   }

   RealType P = p - floor(p);   // argument reduction of p:
   if(P > 0.5)
   {
      P = P - 1;
   }
   if(P == 0.5)   // special case:
   {
      return location;
   }
   result = -scale / tan(constants::pi<RealType>() * P);
   return complement ? RealType(location - result) : RealType(location + result);
} // quantile

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class cauchy_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   cauchy_distribution(RealType location = 0, RealType scale = 1)
      : m_a(location), m_hg(scale)
   {
    static const char* function = "boost::math::cauchy_distribution<%1%>::cauchy_distribution";
     RealType result;
     detail::check_location(function, location, &result, Policy());
     detail::check_scale(function, scale, &result, Policy());
   } // cauchy_distribution

   RealType location()const
   {
      return m_a;
   }
   RealType scale()const
   {
      return m_hg;
   }

private:
   RealType m_a;    // The location, this is the median of the distribution.
   RealType m_hg;   // The scale )or shape), this is the half width at half height.
};

typedef cauchy_distribution<double> cauchy;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const cauchy_distribution<RealType, Policy>&)
{ // Range of permissible values for random variable x.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>()); // - to + max.
  }
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const cauchy_distribution<RealType, Policy>& )
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
     using boost::math::tools::max_value;
     return std::pair<RealType, RealType>(-tools::max_value<RealType>(), max_value<RealType>()); // - to + max.
  }
}

template <class RealType, class Policy>
inline RealType pdf(const cauchy_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::pdf(cauchy<%1%>&, %1%)";
   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_scale("boost::math::pdf(cauchy<%1%>&, %1%)", scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location("boost::math::pdf(cauchy<%1%>&, %1%)", location, &result, Policy()))
   {
      return result;
   }
   if((boost::math::isinf)(x))
   {
     return 0; // pdf + and - infinity is zero.
   }
   // These produce MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && abs(x) == std::numeric_limits<RealType>::infinity())
   //{ // pdf + and - infinity is zero.
   //  return 0;
   //}

   if(false == detail::check_x(function, x, &result, Policy()))
   { // Catches x = NaN
      return result;
   }

   RealType xs = (x - location) / scale;
   result = 1 / (constants::pi<RealType>() * scale * (1 + xs * xs));
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const cauchy_distribution<RealType, Policy>& dist, const RealType& x)
{
   return detail::cdf_imp(dist, x, false);
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const cauchy_distribution<RealType, Policy>& dist, const RealType& p)
{
   return detail::quantile_imp(dist, p, false);
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<cauchy_distribution<RealType, Policy>, RealType>& c)
{
   return detail::cdf_imp(c.dist, c.param, true);
} //  cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<cauchy_distribution<RealType, Policy>, RealType>& c)
{
   return detail::quantile_imp(c.dist, c.param, true);
} // quantile complement

template <class RealType, class Policy>
inline RealType mean(const cauchy_distribution<RealType, Policy>&)
{  // There is no mean:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::mean(cauchy<%1%>&)",
      "The Cauchy distribution does not have a mean: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType variance(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no variance:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::variance(cauchy<%1%>&)",
      "The Cauchy distribution does not have a variance: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType mode(const cauchy_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType median(const cauchy_distribution<RealType, Policy>& dist)
{
   return dist.location();
}
template <class RealType, class Policy>
inline RealType skewness(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no skewness:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::skewness(cauchy<%1%>&)",
      "The Cauchy distribution does not have a skewness: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
}

template <class RealType, class Policy>
inline RealType kurtosis(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no kurtosis:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::kurtosis(cauchy<%1%>&)",
      "The Cauchy distribution does not have a kurtosis: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no kurtosis excess:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::kurtosis_excess(cauchy<%1%>&)",
      "The Cauchy distribution does not have a kurtosis: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

} // namespace math
} // namespace boost

#ifdef _MSC_VER
#pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_CAUCHY_HPP


#include <stdexcept>
#include <utility>
#include <boost/cstdint.hpp>
#include <boost/bind.hpp>
#include <boost/unordered_set.hpp>
#include <luabind/luabind.hpp>
#include <luabind/iterator_policy.hpp>
#include "a_star.h"
#include "bresenham_supercover.h"
#include "heightfield.h"

namespace math
{

template<class T>
heightfield<T>::heightfield(matrix<4,4> const &tf, size_t nrows, size_t ncols):
    nrows_(nrows),
    ncols_(ncols)
{
	set_local_to_world(tf);

	heights_.resize(nrows * ncols);

	local_aabb_.lo.set(0, -1, 0);
	local_aabb_.hi.set(math::scalar(ncols_ - 1), 1, math::scalar(nrows_ - 1));
}

template<class T>
heightfield<T>::~heightfield()
{
}

template<class T>
scalar heightfield<T>::y_under(vec<3> const &position, scalar default_value)
{
	vec<3> local_position = position * world_to_local_;

	int col = (int) local_position.x;
	int row = (int) local_position.z;

	if (col < 0 || row < 0 || col >= (int) ncols_ - 1 || row >= (int) nrows_ - 1) return default_value;

	scalar v1 = local_y_at(col + 0, row + 0);
	scalar v2 = local_y_at(col + 1, row + 0);
	scalar v3 = local_y_at(col + 0, row + 1);
	scalar v4 = local_y_at(col + 1, row + 1);

	scalar k1 = local_position.x - col;
	scalar k2 = local_position.z - col;

	scalar v12 = v1 * (1 - k1) + v2 * k1;
	scalar v34 = v3 * (1 - k1) + v4 * k1;

	return v12 * (1 - k2) + v34 * k2;
}

template<class T>
scalar heightfield<T>::max_y_in_cell(int col, int row, scalar default_value) const
{
	if (col < 0 || row < 0 || col >= (int) ncols_ - 1 || row >= (int) nrows_ - 1)
	{
		return default_value;
	}

	scalar result = (scalar) local_y_at(col + 0, row + 0);
	result = std::max(result, (scalar) local_y_at(col + 0, row + 1));
	result = std::max(result, (scalar) local_y_at(col + 1, row + 0));
	result = std::max(result, (scalar) local_y_at(col + 1, row + 1));

	return result;
}

template<class T>
scalar heightfield<T>::max_y_in_cell(vec<3> const &position, scalar default_value) const
{
	vec<3> local_position = position * world_to_local_;

	int col = (int) local_position.x;
	int row = (int) local_position.z;

	return max_y_in_cell(col, row, default_value);
}

template<class T>
void heightfield<T>::set_local_to_world(math::matrix<4, 4> const &tf)
{
	local_to_world_ = tf;
    local_to_world_.inverse(world_to_local_);
}

template<class T> typename heightfield<T>::cell_t
heightfield<T>::world_position_to_cell(vec<3> const &position) const
{
	vec<3> p = position * world_to_local_;
	return cell_t(int(p.x), int(p.z));
}

template<class T>
vec<3> heightfield<T>::cell_to_world_position(cell_t const &cell) const
{
	return vec<3>((scalar) cell.x + 0.5f, 0, (scalar) cell.y + 0.5f) * local_to_world_;
}

template<class T>
contact_info<3> heightfield<T>::trace(ray<3> const &r, scalar max_distance) const
{
	vec<3> local_p0 = r.r0 * world_to_local_;
	vec<3> local_p1 = (r.r0 + normalize(r.a) * max_distance) * world_to_local_;

	ray<3> local_ray(local_p0, local_p1 - local_p0);

	contact_info<3> result;
	result.happened = false;

	scalar local_ray_t0, local_ray_t1;

	if (!local_aabb_.trace(local_ray, &local_ray_t0, &local_ray_t1)) return result;

	vec<3> local_p0_adjusted = local_ray.apply(local_ray_t0);
	vec<3> local_p1_adjusted = local_ray.apply(local_ray_t1);

	bresenham_supercover<> cells_iterator(cell_t(int (local_p0_adjusted.x), int(local_p0_adjusted.z)),
		cell_t(int(local_p1_adjusted.x), int(local_p1_adjusted.z)));

	while (true)
	{
		cell_t cell;
		bool flag;

		boost::tie(cell, flag) = cells_iterator.get();
		if (flag) break;

		if (cell.x == ncols_ - 1) --cell.x;
		if (cell.y == nrows_ - 1) --cell.y;

		vec<3> v1 = local_vertex_at(cell.x + 0, cell.y + 0);
		vec<3> v2 = local_vertex_at(cell.x + 0, cell.y + 1);
		vec<3> v3 = local_vertex_at(cell.x + 1, cell.y + 1);
		vec<3> v4 = local_vertex_at(cell.x + 1, cell.y + 0);

		triangle<3> t1(v1, v2, v3);
		triangle<3> t2(v1, v3, v4);

		bool trace_h1, trace_h2;
        scalar trace_t1, trace_t2;
		vec<3> trace_p1, trace_p2;

		trace_h1 = t1.trace(local_ray, trace_t1, trace_p1);
		trace_h2 = t2.trace(local_ray, trace_t2, trace_p2);

		if (trace_h1 || trace_h2)
        {
            contact_info<3> ci;

            ci.happened = true;
            ci.penetrated = false;

            if (trace_h1 && ((trace_h2 && trace_t1 < trace_t2) || trace_h2 == false))
            {
                ci.position = trace_p1;
                ci.normal = t1.get_normal();
                ci.time = trace_t1;
            }
            else
            {
                ci.position = trace_p2;
                ci.normal = t2.get_normal();
                ci.time = trace_t2;
            }

			if (result.worse_than(ci))
			{
				result = ci;
			}
        }
    }

	result.position = result.position * local_to_world_;

	return result;
}

template<class T>
void heightfield<T>::load_from_raw_buffer(value_t const *heights)
{
    heights_.assign(heights, heights + nsamples());

	post_load();
}

template<class T>
void heightfield<T>::resize(size_t ncols, size_t nrows)
{
	std::vector<value_t> heights;
	heights.resize(nrows * ncols);

	for (size_t i = 0, ni = std::min(nrows, nrows_); i < ni; ++i)
	{
		for (size_t j = 0, nj = std::min(ncols, ncols_); j < nj; ++j)
		{
			heights[i * ncols + j] = heights_[i * ncols_ + j];
		}
	}

	heights_.swap(heights);
	ncols_ = ncols;
	nrows_ = nrows;

	post_load();
}

namespace
{
template<class T>
class scorer_closure {
public:
	typedef typename heightfield<T>::cell_t cell_t;

	scorer_closure(heightfield<T> const *hf, scalar min_value, cell_t goal, std::vector<cell_t> const &obstacles):
		hf_(hf),
		min_value_(min_value),
		goal_(goal)
	{
		obstacles_.insert(obstacles.begin(), obstacles.end());
	}

	scalar get_score(cell_t const &cell) const
	{
		if (cell.x < 0) return -1;
		if (cell.y < 0) return -1;
		if (cell.x >= (int) hf_->nrows()) return -1;
		if (cell.y >= (int) hf_->ncols()) return -1;

		typename obstacles_t::const_iterator it = obstacles_.find(cell);
		if (it != obstacles_.end()) return -1;

		scalar delta = (scalar) (cell - goal_).length_sq();

		if (hf_->max_y_in_cell(cell.x, cell.y) < min_value_)
		{
			return delta;
		}
		else
		{
			return -1;
		}
	}

private:
	struct cell_hash {
		std::size_t operator ()(cell_t const &cell) const {
			return (cell.x * 1011) ^ cell.y;
		}
	};

	struct cell_eq {
		std::size_t operator ()(cell_t const &lhs, cell_t const &rhs) const {
			return lhs == rhs;
		}
	};

	typedef boost::unordered_set<cell_t, cell_hash, cell_eq> obstacles_t;

	heightfield<T> const *hf_;
	scalar min_value_;
	cell_t goal_;
	obstacles_t obstacles_;
};
}

template<class T> std::vector<typename heightfield<T>::cell_t>
heightfield<T>::build_path(vec<3> const &from, vec<3> const &to, value_t min_value, bool allow_best_heuristic_point,
	std::vector<cell_t> const &obstacles) const
{
	cell_t start = world_position_to_cell(from);
	cell_t goal = world_position_to_cell(to);

	scorer_closure<T> scorer(this, min_value, goal, obstacles);
	a_star pathfinder(start, goal, boost::bind(&scorer_closure<T>::get_score, &scorer, _1));

	pathfinder.calculate_path();

	return pathfinder.build_path(allow_best_heuristic_point);
}

template<class T>
void heightfield<T>::post_load()
{
	local_aabb_.null();

	for (typename std::vector<value_t>::const_iterator it = heights_.begin();
		 it != heights_.end(); ++it)
	{
		if (local_aabb_.lo.y > *it) local_aabb_.lo.y = *it;
		if (local_aabb_.hi.y < *it) local_aabb_.hi.y = *it;
	}

	local_aabb_.lo.y -= 1;
	local_aabb_.hi.y += 1;

	local_aabb_.lo.x = 0;
	local_aabb_.lo.z = 0;

	local_aabb_.hi.x = scalar(ncols_ - 1);
	local_aabb_.hi.z = scalar(nrows_ - 1);
}

template<class T>
void heightfield<T>::bind(lua_State *L, char const *name)
{
	using namespace luabind;

	module(L, "math")
	[
		class_<heightfield>(name)
		.def(constructor<matrix<4,4> const &, size_t, size_t>())
		.def("y_under", &heightfield::y_under)
		.def("max_y_in_cell", (scalar (heightfield::*)(vec<3> const &, scalar) const) &heightfield::max_y_in_cell)
		.def("ncols", &heightfield::ncols)
		.def("nrows", &heightfield::nrows)
		.def("set_local_to_world", &heightfield::set_local_to_world)
		.def("get_local_aabb", &heightfield::get_local_aabb)
		.def("world_position_to_cell", &heightfield::world_position_to_cell)
		.def("cell_to_world_position", &heightfield::cell_to_world_position)
		.def("build_path", &heightfield::build_path)
		.def("trace", &heightfield::trace)
		.def("resize", &heightfield::resize)
	];
}

template class heightfield<boost::uint8_t>;
template class heightfield<boost::uint16_t>;
template class heightfield<scalar>;

void bind_heightfield(lua_State *L)
{
	heightfield<boost::uint8_t>::bind(L, "heightfield_u8");
	heightfield<boost::uint16_t>::bind(L, "heightfield_u16");
	heightfield<scalar>::bind(L, "heightfield_s");
}

}


#include "MP2.hpp"
#include "RHF.hpp"
#include <armadillo>

namespace willow {
namespace qcmol {

arma::mat MP2::cphf(const arma::mat &l_ai, const arma::vec &ao_tei,
                    const bool l_print) {

  const double epsilon = 1.0e-8;
  // arma::vec v_p_ai (nvir*nocc, arma::fill::zeros);
  // arma::mat pm_ai  (v_p_aj.memptr(), nvir, nocc, false);

  arma::mat Cvir = Cmat.submat(0, ivir1, nbf - 1, ivir2);
  arma::mat Cocc = Cmat.submat(0, 0, nbf - 1, iocc2);

  arma::vec v_Pm(nvir * nocc, arma::fill::zeros);
  arma::mat Pm(v_Pm.memptr(), nvir, nocc, false);

  for (auto moi = 0; moi < nocc; moi++) {
    for (auto am = 0; am < nvir; am++) {
      auto moa = ivir1 + am;
      Pm(am, moi) = l_ai(am, moi) / (eval(moa) - eval(moi));
    }
  }

  std::vector<arma::vec> Pm_list;
  Pm_list.push_back(v_Pm);

  arma::vec v_APm(nvir * nocc, arma::fill::zeros);
  arma::mat APm(v_APm.memptr(), nvir, nocc, false);
  std::vector<arma::vec> APm_list;

  // arma::mat Dm  (nbf,  nbf,  arma::fill::zeros);
  // arma::mat Gm  (nbf,  nbf,  arma::fill::zeros);

  arma::vec P_sum_old(nvir * nocc, arma::fill::zeros);
  arma::vec P_sum_new(nvir * nocc, arma::fill::zeros);

  const int maxiter = 30;

  for (auto iter = 0; iter < maxiter; ++iter) {

    const int ndim = iter + 1;

    // Compute A*P[i-1] (call AP matrix)
    v_Pm = Pm_list[iter];

    arma::mat Dm = Cvir * Pm * Cocc.t();
    arma::mat Gm = g_matrix(ao_tei, Dm);
    APm = 4.0 * Cvir.t() * Gm * Cocc;

    for (auto moi = 0; moi < nocc; moi++) {
      for (auto am = 0; am < nvir; am++) {
        auto moa = ivir1 + am;
        double tmpval = APm(am, moi) / (eval(moi) - eval(moa));
        APm(am, moi) = tmpval;
      }
    }

    APm_list.push_back(v_APm);

    // compute alpha
    // Solve the linear system of equations C*X = B
    // where C is matrix, X and B are vector
    // put result (x) into array alpha
    arma::vec alpha(ndim, arma::fill::zeros);
    {
      arma::vec norm(ndim, arma::fill::zeros);

      for (auto i = 0; i < ndim; i++) {
        norm(i) = arma::norm(Pm_list[i]);
      }

      // Construct matrix Cm
      arma::mat Cm(ndim, ndim, arma::fill::zeros);

      for (auto j = 0; j < ndim; j++) {
        for (auto i = 0; i < ndim; i++) {

          double tmp1 = -arma::dot(Pm_list[i], APm_list[j]);

          if (i == j) {
            tmp1 += arma::dot(Pm_list[i], Pm_list[i]);
          }

          Cm(i, j) = tmp1 / (norm(i) * norm(j));
        }
      }

      arma::vec Bv(ndim, arma::fill::zeros);
      Bv(0) = norm(0);

      arma::vec Xv = Cm.i() * Bv;

      for (auto i = 0; i < ndim; i++) {
        alpha(i) = Xv(i) / norm(i);
      }
    }

    // P_sum_new = alpha[0]*P[0] +
    P_sum_new.zeros();
    for (auto j = 0; j < ndim; j++) {
      P_sum_new += alpha(j) * Pm_list[j];
    }

    // Test for convergence
    // (based on RMS (P2aj_new - P2aj_old)
    // and max abs. val. of element

    double tmpval = 0.0;
    double maxabs = 0.0;
    for (auto j = 0; j < nocc * nvir; j++) {
      double tmpval1 = P_sum_new(j) - P_sum_old(j);
      double tmpval2 = tmpval1 * tmpval1;
      tmpval += tmpval2;
      if (tmpval2 > maxabs)
        maxabs = tmpval2;
    }

    if (std::sqrt(tmpval / (nocc * nvir)) < epsilon &&
        std::sqrt(maxabs) < epsilon)
      break;

    // Put P_sum_new into P_sum_old

    P_sum_old = P_sum_new;

    // Compute projection of A*P[i-1] on P[0], ..., P[i-1]

    arma::vec projctn(nocc * nvir, arma::fill::zeros);

    for (auto j = 0; j < iter + 1; j++) {
      double dot_prod = arma::dot(Pm_list[j], Pm_list[j]);
      double coef = arma::dot(Pm_list[j], APm) / dot_prod;

      projctn += coef * Pm_list[j];
    }

    Pm_list.push_back(v_APm - projctn);

    // Test for convergence (based on norm (Pm[i]))

    double rmsd = arma::norm(Pm_list[iter + 1]) / sqrt(nocc * nvir);
    // double rmsd = arma::norm (p_ai - p_ai_old);

    if (l_print)
      std::cout << "Iter CPHF " << iter << "   " << rmsd << std::endl;
    if (rmsd < epsilon)
      break;
  }

  // Converged vector is in P_sum_new

  arma::mat p_ai(P_sum_new.memptr(), nvir, nocc, true);

  return p_ai;
}

/*
arma:mat MP2::cphf_old ()
{
  double epsilon = 1.0e-8;

  for (auto moi = 0; moi < nocc; moi++) {
    for (auto am = 0; am < navir; am++) {
      auto moa = ivir1 + am;
      p_ai(am,moi) = l_ai(am,moi)/(eval(moa) - eval(moi));
    }
  }

  // Solve the CPHF equations (iteratively, with DIIS like method)
  int ii = 0;
  int niter = 0;
  const int maxiter = 30;
  for (auto iter = 0; iter != maxiter; ++iter) {

    arma::mat p_ai_old = p_ai;
    // Compute A*P[ii-1]
    // Dm(nbf,nbf) = (nbf,navir)*(navir,nocc)*(nbf,nocc)
    arma::mat Dm = Cvir*p_ai_old*Cocc.t();
    arma::mat Gm = g_matrix (ints.TEI, Dm);

    //  APm(navir,nocc) =
    arma::mat APm = 2.0*Cvir.t()*Gm*Cocc;

    for (auto moi = 0; moi < nocc; moi++) {
      for (auto am = 0; am < navir; am++) {
        auto moa = ivir1 + am;
        p_ai (am,moi) = (l_ai(am,moi) - APm(am,moi))/(eval(moa) - eval(moi));
      }
    }
    p_ai = 0.6*p_ai + 0.4*p_ai_old;

    double rmsd = arma::norm (p_ai - p_ai_old);

    std::cout << "Iter CPHF " << iter << "   " << rmsd << std::endl;
    if (rmsd < 1.0e-7) break;
  }

  std::cout << "p_ai \n";

  return p_ai;
}
*/

} // namespace qcmol
} // namespace willow


//
//  lung_model_discrete_branching.hpp
//
//
//  Created by Carl Whitfield on 09/03/2017.
//
//
// Checked and commented 16/05/2018
#if defined(_WIN32) || defined(_WIN64)
#define _USE_MATH_DEFINES
#include <windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <algorithm>
#include <unordered_map>
#include <string.h>
#include <math.h>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <chrono>
#include <ctime>
#include <Eigen/Eigen/Dense>
#include <Eigen/Eigen/LU>
#include <Eigen/Eigen/Sparse>
#include <Eigen/Eigen/SparseLU>
#include <Eigen/Eigen/IterativeLinearSolvers>

//Options for tree config
#define LINEARPERT 1       //option TREE l
#define NOPERT 2    //option TREE n

//options for BCs
#define BAG 101            //option BC b
#define NOFLUX 102         //option BC n
#define SINK 103

//options for Defects
#define BLOCKAGE 201
#define AREA 202
#define LENGTH 203
#define ELASTICITY 204
#define BAG_RESISTANCE 205

//options for upwind scheme
#define CENTRAL_UPWIND 301
#define FIRST_ORDER_UPWIND 302      //First order upwind interpolation

//options for flow type
#define POISEUILLE 401   //--Poiseuille Flow Assumption--//
#define PEDLEY 402       //--See Pedley 1970--//

//option for elastic response function
#define LINEAR_RESP 501
#define NONLINEAR_RESP 502

//options for pressure profile
#define STEP_FUNCTION 601
#define SIGMOIDAL 602
#define SINUSOIDAL 603
#define LINEAR 604

//options for parameters to use
#define GEOMETRIC    701
#define HOMOGENISED  702
#define WEIBEL  703
#define LOBE_GEOMETRIC 704
#define ALT_LOBE_GEOMETRIC 705

//options for linear solve
#define ITERATIVE    801
#define DIRECT       802

//options for initialisation
#define EMPTY 901
#define FULL 902

//options for perturbations
#define PERT_AREA 1001
#define PERT_LENGTH 1002
#define PERT_ELASTICITY 1003

//options for volume/pressure input
#define VOLUME 1101
#define PRESSURE 1102

//options for dispersion
#define TAYLOR 1201
#define SCHERER 1202
#define NONE 1203

//Options for concentration file printout
#define VTK 1301
#define CSV 1302
#define VTK_CSV 1303

//Options for leak types
#define INSPIRATORY_LEAK 1401
#define EXPIRATORY_LEAK 1402
#define INSPIRATORY_AND_EXPIRATORY_LEAK 1403

/********************************************/
//Unit conversions
#define cmH20_Pa 100    //pressure conv from cm^2H20 to Pa
#define L_m3 0.001      //volume from litres to m^3
#define cm_m 0.01       //cm to metres
#define PEDLEY_C 1.85   //pedley constant

//Default values of parameters
#define NGENTOT 23              //Number of generations total
#define NGENDEF 15              //Number of generations before model terminates
#define VISCDEF 1.93E-07            //Air Viscosity (cmH20 s) (actual, 1.93E-07 at 37C) -- engineering toolbox
#define DENSITYDEF 1.138        //Air Density (kg m^-3) 1.138 at 37C -- engineering toolbox
#define EDEF 5.0              //Elastance (cmH20 L^-1)
#define RBDEF 0.2              //bag resistance
#define RMDEF 0.6              //mouth resistance
#define DIFFUSIONDEF 0.105       //Diffusion constant (cm2/s) (Helium is approx 0.71, Oxygen 0.19)
#define MAXPECDEF 10            //Max element peclet number
#define K0DEF 0                 //Uptake term
#define LDDEF 3.0                //Length to diameter ratio
#define LD2DEF 2.3                //Length to diameter ratio
#define VFRCDEF 3.0            //Functional residual capacity in L
#define VDDEF 0.12             //Conducting airway dead space
#define VDMDEF 0.05            //Volume of mouth cavity
#define VDUCTDEF 0.20            //fraction of acinus consisting of duct at FRC
#define TINDEF 2.5              //Duration of first breath in (secs)
#define P0DEF 1.0           //Max pressure applied at distal end (cmH20)
#define VTDEF 1.0
#define RUNDEF 40               //Total Simulation Time (number of breathing cycles)
#define LAMBDADEF 0.794          //Geometric parameter
#define LAMBDA2DEF 0.92         //Geom parameter acinus
#define STDEF 0               //Time source is left at opening (in number of breaths)
#define DEFPERTSIZE 0.1       //perturbation size
#define DEFPRINTERVAL 10       //time between prints (in terms of length of first breath)
#define DEFMINGENSIZE 1          //minimum number of points in a generation
#define MINGENSIZE_ACIN 4
#define DEFSTARTDELAY 10         //number of breaths to simulate before starting transport simulation
#define DFACTOR 0.2   //increase in eff area in acinus (default)

//Tolerances precisions etc.
#define PTOL 1E-12  //precision for stree calc
#define TOL 1E-12   //precision for ltree calc
#define OUTPUT_PRECISION_CONC 12   //precision for conc file output
#define OUTPUT_PRECISION_FLUX 12   //precision for flux file output
#define DEF_TS 0.01               //defined timestep
#define DEF_DX 0.025               //defined max spacestep

//define stree numbers for lobar models -- trachea is 0
#define BR 1    //right branch
#define BL 2    //left branch
#define BRML 3   //right middle/lower
#define BRU 4    //right upper
#define BLL 5    //left lower
#define BLU 6    //left upper
#define BRM 7    //right middle
#define BRL 8    //right lower
#define BRL1 9   //right lower major
#define BRL2 10  //right lower minor
#define BLL1 11  //left lower major
#define BLL2 12  //left lower minor

using namespace std;

class Defect    //class for storing ariway/acinar defect information
{
public:
	unsigned jn;   //generation number
	unsigned long kstart, kend;   //branch number start and end
	unsigned type;       //defect type
	double mag;      //magnitude of defect
	Defect()
	{
		jn = 0;
		kstart = 0;
		kend = 0;
		type = 0;   //should return error if unchanged
		mag = 0;
	}
	inline bool operator==(Defect d)   //equality operator
	{
		if (type == BLOCKAGE) return(type == d.type);  //defects are equal if both blocked
		else return (type == d.type && ((int)(1000000 * mag)) == ((int)(1000000 * d.mag)));  //defects equal if same type and magnitude
	}
};

class Leak    //class for storing leak details - for simulating inspiratory and expiratory leaks at mouth
{
public:
	bool exists;
	double start, end, size;    //start time (breaths), end time (breaths), size (fraction)
	unsigned type;             //Insp only, exp only or both
	Leak()
	{
		exists = false;
		start = 0;
		end = 100000;
		size = 0;
		type = INSPIRATORY_AND_EXPIRATORY_LEAK;
	}
};

class Conversions    //class for storing unit conversions
{
public:
	double LL_to_cm;    //lung length (sim units) to cm
	double P_to_cmH20;   //pressure (sim units) to cmH20
	double t_to_s;      //time (sim units) to seconds
	double V_to_Litres;    //volume (sim units) to litres
};

class Options    //for storing input options
{
public:
	/***Determined by input file***/

	/**Overarching options**/
	unsigned TreeOp;         //Tree format
	unsigned BcOp;           //Boundary condition
	unsigned UpwindOp;       //Upwind interpolation scheme
	unsigned PressOp;        //Function for pressure profile
	unsigned RespFunc;       //Function for elastic resistance
	unsigned FlowType;       //Poiseuille or turbulent
	unsigned ShapeOp;        //Determines the shape and size of the tubes
	unsigned SolverOp;
	unsigned InitOp;        //Determines c initialisation (empty c=0, full c=1)
	unsigned InputOp;         //Determines whether volume or pressure is inputted
	unsigned TaylorDisp;    //True if Taylor Dispersion is considered
	unsigned OutputOp;   //output format option
	bool output_perts;  //whether to output pert files or not
	bool output_lung_volumes;
	Leak N2leak;

	/**Parameters**/
	unsigned Ngen;           //Number of generations before alveoli
	unsigned Ngen2;          //Total number of generations in tree
	unsigned MinGenSize, MinAcinGenSize;
	double dt, dxmax;                //timestep
	double Viscosity;        //Air viscosity
	double Density;          //Air density
	double Diffusion;      //Diffusion constant
	double k0;          //bare uptake rate
	double LDratio, LDratio2;          //L0 length of trachea
	double VFRC;        //total lung resting volume
	double Tin;         //period first inhalation
	double P0;          //Pressure on first inhalation (always negative)
	double VT, VD, VDM, Vductvol;          //Tidal volume
	double E, Rb, Rmouth, V0;           //Elastance of bag, (Total) Resistance of bag, volume of bag
	double RunTime;     //Total run time of simulation in breaths
	double PrerunTime;
	double lambda, lambda2;      //Geometric ratio of tree generations
	double StimTime;    //Time stimulation kept at inlet
	double printerval;  //Time between data outputs
	double MaxPeclet;   //Maximum element peclet number allowed
	double AcinAreaFactor;
	vector<vector<Defect>> def;     //Vector of large magnitude perturbations to symmetric tree
	//vector<Defect> pert;      //Vector of small magnitude perturbations to consider individually
	//vector<vector<char>> *tree_pert;
	unordered_map<unsigned long, vector<Defect>> def_map;   //map from position in tree to defects at that location
	unsigned long kmtot;   //total number of finite volumes

	/***Determined in code***/
	chrono::time_point<std::chrono::system_clock> tstart, tend;
	unsigned Ntrees;               //Number of subtrees - identifier for this run
	string SimID;
	double Peclet;        //Peclet number for branch 0
	/***Constructor - Default Values***/
	Options()
	{
		TreeOp = NOPERT;
		BcOp = NOFLUX;
		UpwindOp = FIRST_ORDER_UPWIND;
		PressOp = SINUSOIDAL;
		RespFunc = LINEAR_RESP;
		FlowType = POISEUILLE;
		ShapeOp = GEOMETRIC;
		SolverOp = ITERATIVE;
		InitOp = FULL;
		InputOp = VOLUME;
		TaylorDisp = SCHERER;
		output_perts = true;
		output_lung_volumes = true;
		OutputOp = CSV;
		Ngen = NGENDEF;
		Ngen2 = NGENTOT;
		MinGenSize = DEFMINGENSIZE;
		MinAcinGenSize = MINGENSIZE_ACIN;
		Viscosity = VISCDEF;
		Density = DENSITYDEF;
		E = EDEF;
		Rb = RBDEF;
		Rmouth = RMDEF;
		Diffusion = DIFFUSIONDEF;
		MaxPeclet = MAXPECDEF;
		k0 = K0DEF;
		LDratio = LDDEF;
		LDratio2 = LD2DEF;
		VFRC = VFRCDEF;
		VD = VDDEF;
		VDM = VDMDEF;
		Vductvol = VDUCTDEF;
		Tin = TINDEF;
		P0 = P0DEF;
		VT = VTDEF;
		RunTime = RUNDEF;
		PrerunTime = DEFSTARTDELAY;
		lambda = LAMBDADEF;
		lambda2 = LAMBDA2DEF;
		StimTime = STDEF;
		printerval = DEFPRINTERVAL;
		dt = DEF_TS;
		dxmax = DEF_DX;
		AcinAreaFactor = DFACTOR;
		def.resize(13);
	}
	//functions to return text for options
	string read_tree_option();
	string read_bc_option();
	string read_flow_option();
	string read_resp_option();
	string read_pressure_option();
	string read_shape_option();
	string read_upwind_option();
	string read_perttype();
	string read_taylor_option();
	string read_solver_option();
	string read_init_option();
	string read_input_option();
	string read_output_option();
	string read_leak_type();
};

struct node     //stores info at a single finite volume node
{
	double c, cold, Aold, Anew, DA, x, uc, y0;        //values of conc, previous conc, totarea, position, vel at element centre
	double ul, ur, al, ar, Dl, Dr;                             //velocity, diffusion and area on left and right of element
	unsigned long km;                                       //corresponding entrance in matrix
	vector<unsigned> iup[3], jup[3], kup[3];        //stores indices for neighbours n-1, n and n+1 respectively
	vector<double> dxup[3], dcfr[3], dcfl[3], ucfrpos[3], ucflpos[3], ucfrneg[3], ucflneg[3], sr, sul;   //vector of dx at n-1, n  and n+1 and cross-sections
	double sl, sdr;   //cross section left sl and right (for node to left) sdr
	node()   //constructor
	{
		c = 0;
		cold = 0;
		Aold = 0;
		Anew = 0;
		DA = 0;
		x = 0;
		uc = 0;
		ul = 0;
		ur = 0;
		al = 0;
		ar = 0;
		Dl = 0;
		Dr = 0;
		km = 0;
		sl = 0;
		sdr = 0;
	}
	void set_quantities_zero()   //rest relevant quantities to zeros
	{
		c = 0;
		cold = 0;
		Aold = 0;
		Anew = 0;
		DA = 0;
		x = 0;
		uc = 0;
		ul = 0;
		ur = 0;
		al = 0;
		ar = 0;
		Dl = 0;
		Dr = 0;
		sl = 0;
		sdr = 0;
		km = 0;
		for (unsigned counter = 0; counter < 3; counter++)
		{
			for (unsigned counter2 = 0; counter2 < dxup[counter].size(); counter2++)
			{
				dxup[counter][counter2] = 0;
				dcfr[counter][counter2] = 0;
				dcfl[counter][counter2] = 0;
				ucfrpos[counter][counter2] = 0;
				ucflpos[counter][counter2] = 0;
				ucfrneg[counter][counter2] = 0;
				ucflneg[counter][counter2] = 0;
			}

		}
		for (unsigned counter2 = 0; counter2 < sr.size(); counter2++) sr[counter2] = 0;
		for (unsigned counter2 = 0; counter2 < sul.size(); counter2++) sul[counter2] = 0;
	}
};

struct gen    //stores a generation of finite volume nodes
{
	double dx;              //element length
	unsigned long Nb;        //No of branches in this gen
	double x0;         //start position
	vector<node> p;    //vector of nodes
	double Ap, Lp;     //defects/perturbations
	gen()    //Constructor
	{
		dx = 0;
		Nb = 0;
		x0 = 0;
	}
	void set_quantities_zero()
	{
		dx = 0;
		x0 = 0;
		Ap = 0;
		Lp = 0;
	}
};

struct subtree    //stores a subtree consisting of generations connected in series
{
	unsigned StartGen;           //generation where subtree starts
	unsigned long StartBranch;        //branch no. of tree base
	unsigned EndGen;             //generation where subtree ends
	unsigned Ncond, Ntot;         //Gen at which conducting airways terminate and alv airways terminate
	unsigned imeanpath;         //number of mean path
	double A0, L0, A1, L1;
	double Vold;            //equivalent total volume of bags at end at time t
	double Vnew;            //equivalent total volume of bags at end at time t+dt
	double Valv0, sValv0, V0, Vacinduct;           //length of all pipes in alveoli region
	double qend;
	vector<gen> gn;                 //generation
	double fluxin, fluxout; //instantantaneous flux at inlet and outlet
	double masstot, totIGvol;         //total mass in system
	double totIGvolold;         //total mass in system
	int treein;             //number of the tree feeding this one (-1 if root)
	int treeout[2];         //numbers of tree fed by this one (both -1 if base)
	double E, Ep, Rb, Rbp;              //tree elastance and bag resistance
	double dR;              //effective resistance change at start gen of tree
	double Rj;                  //effective resistance of subtree
	double Rtree;                 //resistance of this tree only
	double dVacinus;           //stores the volume change of the associated acinus (or acini)
	double z0;   //coordinate for plotting
	vector<unsigned> EndSubtrees;              //list of terminating subtrees connected to this subtree
	vector<unsigned> isub;                     //list of all subtrees
	bool blocked;     //true if blocked off, false if not
	unsigned i_stree;   //stores index of corresponding subtree in stree
	subtree()   //constructor
	{
		StartGen = 0;
		EndGen = NGENTOT;
		Vold = 0;
		Vnew = 0;
		Valv0 = 0;
		sValv0 = 0;
		Vacinduct = 0;
		L0 = 0;
		A0 = 0;
		L1 = 0;
		A1 = 0;
		V0 = 0;
		qend = 0;
		fluxin = 0;
		fluxout = 0;
		masstot = 0;
		totIGvol = 0;
		totIGvolold = 0;
		treein = -1;
		treeout[0] = -1;
		treeout[1] = -1;
		E = EDEF;
		Ep = 0;
		Rb = RBDEF;
		Rbp = 0;
		dR = 0;
		Rj = 0;
		Rtree = 0;
		z0 = 0;
		blocked = false;
		i_stree = 0;
		dVacinus = 0;
	}

	~subtree()
	{
		gn.clear();
	}
	void set_quantities_zero() //set relevant quantities to zero for perturbative model
	{
		Vold = 0;
		Vnew = 0;
		Valv0 = 0;
		sValv0 = 0;
		V0 = 0;
		qend = 0;
		fluxin = 0;
		fluxout = 0;
		masstot = 0;
		totIGvol = 0;
		totIGvolold = 0;
		EndSubtrees.clear();
		Ep = 0;
		Rbp = 0;
		dR = 0;
		Rj = 0;
	}
	void allocate_gn(unsigned size)
	{
		gn.resize(size);
	}
	void deallocate_gn()
	{
		gn.clear();
	}
	double length_function(unsigned j, Options &o);    //returns gen j length
	double area_function(double dx, unsigned j, Options &o);       //returns dx along gen j cross section
	double alveolar_density(unsigned j, unsigned k, Options &o);    //returns alveolar density function
	void subtree_resistance(void);                               //computes subtree resistance
	void tot_area_function(double Vtot);                         //distributes sac volume to cross section A
	void tot_lp_area_function(subtree &sst, double DVtot);        //same as abouve for perturbed tree
	void velocity_calc(double qend, Options &o);                //calculates flow velocity from A
	void velocity_lp_calc(subtree &sst, double qend, Options &o);          //same as above for perturbed tree
};

class Tree                  //for storing whole airway tree data
{
public:
	vector<subtree> st;
	double masstot, masstotold, fluxin;            //total mass in system
	double Ppl, Pplold;                //pleural pressure
	unsigned pert_ij[2];          //stores st and generation of perturbation
	char pert_type;               //'K', 'A' or 'L' depending on which per
	//Eigen matrices and functions fro ventialtion and transport problems
	Eigen::HouseholderQR<Eigen::MatrixXd> *AmatQR;
	Eigen::PartialPivLU<Eigen::MatrixXd> *AmatLU;
	Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::IncompleteLUT<double>> *solver_iter;
	Eigen::SparseLU<Eigen::SparseMatrix<double>> *solver_dir;
	Eigen::MatrixXd Amat, Bmat, Kmat;
	Eigen::VectorXd KVs, Vs;
	double VDfront, VDfrontold, VDfrontoldold, cmouth, cmouthold, Vlung, Vairways;  //volume of mouth dead space filled this breath
	double Vlungold;
	vector<double> ctop, Vtot;
	vector<unsigned> EndSubtrees, meanpath;
	unordered_map<unsigned, unsigned> STno_to_ESTno;   //subtree number to end subtree number
	vector<vector<unsigned>> mpsubtrees;  //stores subtrees that are in a given mean path
	double R0;
	unsigned long kmtot;  //total number of points for transport equations
	Tree() //constructor
	{
		masstot = 0;
		fluxin = 0;
		Ppl = 0;
		Pplold = 0;
		pert_ij[0] = 0;
		pert_ij[1] = 0;
		pert_type = 'N';  //N for none
		cmouth = 1;
		cmouthold = 1;
		VDfront = 0;
		VDfrontold = 0;
		VDfrontoldold = 0;
		kmtot = 0;
		ctop.clear();
		Vtot.clear();
	}
	~Tree()
	{
		ctop.clear();
		Vtot.clear();
	}
	//output functions
	int printfunc_tree(string filename, Conversions cons);
	int printfunc_lp_tree(string filename, Tree &stree, Conversions cons);
	int printfunc_tree_csv(string filename, Conversions cons);
	int printfunc_lp_tree_csv(string filename, Tree &stree, Conversions cons);
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//read and set simulation options
int parse_options(string infile, Options &o);
void check_option_consistency(Options &o);
int convert_units(Tree &stree, Conversions &cons, double LL, Options &o);

//initialisation
int initialise_system(Tree &stree, Conversions &cons, Options &o);
int initialise_defects(Tree &stree, Options &o);
int initialise_tree(Tree &stree, Options &o);
int pre_run_breaths(Tree &stree, vector<Tree> &ltree, double &time, Options &o);
int initialise_lung_volumes(Tree &stree, vector<Tree> &ltree, Options &o);

//tree building
double build_alternative_lobe_branches(Tree &stree, Options &o);
double build_symmetric_branches(Tree &stree, Options &o);
string read_lobe_no(unsigned i, Options &o);

//build perturbed trees
void setup_pert_trees(vector<Tree> &ltree, Tree &stree, Options &o);
void setup_lp_tree(vector<Tree> &ltree, Tree &stree, Options &o);
void calc_perts(vector<Tree> &ltree, Tree &stree);
void apply_pert(Tree &ltree, Tree &stree);

//build and solve ventilation problem
int update_flux(Tree &stree, vector<Tree> &ltree, double time, Options &o);
int tree_flux(Tree &stree, double time, Options &o);
int tree_lp_flux(Tree &ltree, Tree &stree, Options &o);

//build and solve transport problem
int update_conc(Tree &stree, vector<Tree> &ltree, double time, Options &o);
int update_c_stree(Tree &stree, double time, Options &o);
int update_c_ltree(Tree &ltree, Tree &stree, double time, Options &o);
void calc_gas_concs(Tree &stree, Eigen::VectorXd &XC, Options &o);
void fill_Ab(Tree &stree, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &XC, Eigen::VectorXd &BC, unsigned long Ntot, double time, Options &o);
void calc_gas_concs_lp(Tree &ltree, Tree &stree, Eigen::VectorXd &XC, Options &o);
void fill_Ab_lp(Tree &ltree, Tree &stree, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &XC, Eigen::VectorXd &BC, unsigned long Ntot, Options &o);

//output functions
int printfunc(Tree &stree, vector<Tree> &ltree, Options &o, Conversions cons, double t);
int append_masterout(string filename, double time, Tree &tree, Options &o, Conversions cons);
int print_simops(ofstream &summary_file, Options &o);
int print_params(ofstream &summary_file, Options &o);

//various input functions determining parameters
double pressure_func(double t, Options &o);
double c_stim(double t, Options &o);

double total_run_time(Options &o);
unsigned long calc_ts_number(Options &o);
unsigned lobe_gens(unsigned nlobe, Options &o);  //returns gen number of final branch
unsigned long ijk_index(unsigned i, unsigned j, unsigned long k, Options &o);
double weibel_length(unsigned j);
double weibel_area(unsigned j);

//calculating resistance matrices for ventilation problem
void subtree_resistance(Tree &tree, unsigned i, Options &o);
void subtree_lp_symm_resistance(subtree &st, subtree &symm, Options &o);
int full_tree_fluxcalc(Tree &tree, double time, Options &o);
int full_lp_tree_fluxcalc(Tree &ltree, Tree &stree, Options &o);
int build_resistance_matrix(Tree &tree, Options &o);
bool isparent(Tree &tree, unsigned ip, unsigned id);

//finite difference functions and solving routines
void upwind_coeffs(vector<double> ucfl[3], vector<double> ucfr[3], vector<double> dxh[3], double u0, Options &o);
void lp_upwind_coeffs(vector<double> ducfl[3], vector<double> ducfr[3], vector<double> dx0[3], vector<double> dxh[3], double u0, Options &o);
void fd_coeffs(vector<double> dcfl[3], vector<double> dcfr[3], vector<double> dxh[3]);
void lp_fd_coeffs(vector<double> ddcfl[3], vector<double> ddcfr[3], vector<double> dx0[3], vector<double> dxh[3]);
void tree_point_pos(Tree &tree, unsigned i, unsigned j, unsigned k, int n, vector<unsigned> &in, vector<unsigned> &jn, vector<unsigned> &kn);
int iterative_solver(Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::IncompleteLUT<double>> *solver, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &x, Eigen::VectorXd &b);
int LU_solver(Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> *solver, Eigen::SparseMatrix<double> &A, Eigen::VectorXd &x, Eigen::VectorXd &b);

inline unsigned bitflip(unsigned m)
{
	if (m == 0) return 1;
	else return 0;
}

inline bool jcomp(Defect d1, Defect d2)  //compare defect by i, then j, then k, then type, then magntiude
{
	return ((d1.jn < d2.jn) || ((d1.jn == d2.jn) && (d1.kstart < d2.kstart)) || ((d1.jn == d2.jn) && (d1.kstart == d2.kstart) && (d1.type < d2.type)));
}


/*
 * Copyright [2015] [Ke Sun <sunke.polyu@gmail.com>]
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <ros/ros.h>
#include <Eigen/SVD>
#include <mocap_base/KalmanFilter.h>

using namespace std;
using namespace Eigen;

namespace mocap {

KalmanFilter::KalmanFilter():
  attitude         (Quaterniond::Identity()),
  position         (Vector3d::Zero()),
  angular_vel      (Vector3d::Zero()),
  linear_vel       (Vector3d::Zero()),
  state_cov        (Matrix12d::Identity()),
  input_cov        (Matrix12d::Identity()),
  measurement_cov  (Matrix6d::Identity()),
  filter_status    (INIT_POSE),
  last_time_stamp  (0.0),
  msg_interval     (0.01),
  proc_jacob       (Matrix12d::Zero()),
  meas_jacob       (Matrix6_12d::Zero()),
  proc_noise_jacob (Matrix12d::Zero()),
  meas_noise_jacob (Matrix6d::Zero()) {

  // Needs to be changed based on angular velocity
  // and time interval
  proc_jacob = Matrix12d::Identity();
  proc_noise_jacob = Matrix12d::Identity();

  // Do not need to be changed
  meas_jacob.leftCols<6>() = Matrix6d::Identity();
  meas_noise_jacob = Matrix6d::Identity();

  return;
}

bool KalmanFilter::init(const Matrix12d& u_cov,
    const Matrix6d& m_cov, const int& freq) {
  bool is_valid = true;
  JacobiSVD<Matrix12d> u_svd(u_cov);
  JacobiSVD<Matrix6d> m_svd(m_cov);

  Vector12d u_sigmas = u_svd.singularValues();
  Vector6d m_sigmas = m_svd.singularValues();

  if (u_sigmas(11) < 1e-10) {
    is_valid = false;
    ROS_ERROR("Input Cov is close to singular (least singlar value:%f < 1e-7)",
        u_sigmas(11));
  } else {
    input_cov = u_cov;
  }

  if (m_sigmas(5) < 1e-10) {
    is_valid = false;
    ROS_ERROR("Measurement Cov is close to singular (least singlar value:%f < 1e-7)",
        m_sigmas(5));
  } else {
    measurement_cov = m_cov;
  }

  if (freq < 0) {
    is_valid = false;
    ROS_ERROR("Invalid frequency for filter (%d < 0)", freq);
  } else {
    msg_interval = 1.0 / static_cast<double>(freq);
  }

  return is_valid;
}

bool KalmanFilter::prepareInitialCondition(
    const double& curr_time_stamp,
    const Eigen::Quaterniond& m_attitude,
    const Eigen::Vector3d& m_position) {

  switch (filter_status) {
    case INIT_POSE: {
      // Set the current pose
      last_time_stamp = curr_time_stamp;
      attitude = m_attitude;
      position = m_position;
      filter_status = INIT_TWIST;
      // Set the uncertainty of the state
      state_cov = Matrix12d::Identity();
      return true;
    }
    case INIT_TWIST: {
      // Compute the difference between the current
      // pose and last pose
      Quaterniond dq = m_attitude*attitude.inverse();
      AngleAxisd daa(dq);
      Vector3d dr = m_position-position;
      double dt = curr_time_stamp-last_time_stamp;
      //dt = dt > 0 ? dt : msg_interval;
      dt = dt*0.9 + msg_interval*0.1;
      // Set current pose and velocity
      last_time_stamp += dt;
      attitude = m_attitude;
      position = m_position;
      angular_vel = daa.axis()*daa.angle()/dt;
      linear_vel = dr/dt;
      // Set the uncertainty of the state
      state_cov = Matrix12d::Identity();
      filter_status = READY;
      return true;
    }
    case READY:
      return false;
    default:
      return false;
  }
  return false;
}

bool KalmanFilter::isReady() {
  if (filter_status != READY)
    return false;
  else
    return true;
}

void KalmanFilter::reset() {
  filter_status = INIT_POSE;
  return;
}

void KalmanFilter::prediction(const double& curr_time_stamp) {
  // Propogate the actual state
  double dt = curr_time_stamp - last_time_stamp;
  //dt = dt > 0 ? dt : msg_interval;
  dt = dt*0.9 + msg_interval*0.1;
  Vector3d dw = angular_vel * dt;
  Vector3d dr = linear_vel * dt;

  double dangle = dw.norm();
  Vector3d axis = dw / dangle;
  AngleAxisd daa(dangle, axis);
  Quaterniond dq(daa);
  // Velocities are modeled as constants
  last_time_stamp += dt;
  attitude = dq * attitude;
  position = dr + position;

  // Propogate the uncertainty of the estimation error
  // TODO: Optimize this part
  proc_jacob(0,  1) =  dw(2);
  proc_jacob(0,  2) = -dw(1);
  proc_jacob(1,  0) = -dw(2);
  proc_jacob(1,  2) =  dw(0);
  proc_jacob(2,  0) =  dw(1);
  proc_jacob(2,  1) = -dw(0);
  proc_jacob(0,  6) =  dt;
  proc_jacob(1,  7) =  dt;
  proc_jacob(2,  8) =  dt;
  proc_jacob(3,  9) =  dt;
  proc_jacob(4, 10) =  dt;
  proc_jacob(5, 11) =  dt;

  //state_cov = proc_jacob*state_cov*proc_jacob.transpose() +
  //  proc_noise_jacob*input_cov*proc_noise_jacob.transpose();
  state_cov = proc_jacob*state_cov*proc_jacob.transpose() + input_cov;

  //cout << "Process: " << endl;
  //cout << "att: " <<
  //  Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl;
  //cout << "pos: " << position.transpose() << endl;
  //cout << "ang: " << angular_vel.transpose() << endl;
  //cout << "lin: " << linear_vel.transpose() << endl;
  //cout << "input noise:\n" << input_cov << endl;
  //cout << "proc_jacob:\n" << proc_jacob << endl;
  //cout << "Pk+1|k:\n" << state_cov << endl;

  return;
}

void KalmanFilter::update(const Eigen::Quaterniond& m_attitude,
    const Eigen::Vector3d& m_position) {
  // TODO: Optimize the whole function
  // Compute the residual of the measurement
  Quaterniond re_q = m_attitude * attitude.inverse();
  AngleAxisd re_aa(re_q);
  if (std::abs(re_aa.angle()) > std::abs(2*M_PI-re_aa.angle())) {
    re_aa.angle() = 2*M_PI - re_aa.angle();
    re_aa.axis() = -re_aa.axis();
  }
  Quaterniond re_qs(re_aa);
  Vector3d re_th(re_qs.x()*2.0, re_qs.y()*2.0, re_qs.z()*2.0);
  Vector3d re_r = m_position - position;
  Vector6d re;
  re.head<3>() = re_th;
  re.tail<3>() = re_r;

  // Compute the covariance of the residual
  //Matrix6d S = measurement_cov +
  //  meas_jacob*state_cov*meas_jacob.transpose();
  Matrix6d S = measurement_cov + state_cov.topLeftCorner<6, 6>();

  // Compute the Kalman gain
  //Matrix12_6d K = state_cov*meas_jacob.transpose()*S.inverse();
  Matrix12_6d K = state_cov.leftCols<6>()*S.inverse();

  // Compute the correction of the state
  Vector12d dx = K * re;

  // Update the state based on the correction
  Vector3d dq_vec3 = dx.head<3>() / 2.0;
  Vector4d dq_vec4 = 1.0/sqrt(1.0+dq_vec3.squaredNorm())*
    Vector4d(dq_vec3(0), dq_vec3(1), dq_vec3(2), 1.0);
  Quaterniond dq(dq_vec4(3), dq_vec4(0), dq_vec4(1), dq_vec4(2));

  attitude = dq * attitude;
  position += dx.segment<3>(3);
  angular_vel += dx.segment<3>(6);
  linear_vel += dx.segment<3>(9);

  // Update the uncertainty of the state/error
  state_cov = (Matrix12d::Identity()-K*meas_jacob) * state_cov;

  //cout << "Error: " << endl;
  //cout << "dat: " << Vector4d(re_q.w(), re_q.x(), re_q.y(), re_q.z()).transpose() << endl;
  //cout << "dpo: " << re_r.transpose() << endl;

  //cout << "Innovation: " << endl;
  //cout << "dat: " << Vector4d(dq.w(), dq.x(), dq.y(), dq.z()).transpose() << endl;
  //cout << "dpo: " << dx.segment<3>(3).transpose() << endl;
  //cout << "dan: " << dx.segment<3>(6).transpose() << endl;
  //cout << "dli: " << dx.segment<3>(9).transpose() << endl;

  //cout << "Update: " << endl;
  //cout << "att: " <<
  //  Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl;
  //cout << "pos: " << position.transpose() << endl;
  //cout << "ang: " << angular_vel.transpose() << endl;
  //cout << "lin: " << linear_vel.transpose() << endl;
  //cout << "means_jacob:\n" << meas_jacob << endl;
  //cout << "Pk+1|k:\n" << state_cov << endl;
  //cout << "meas_noise:\n" << measurement_cov << endl;
  //cout << endl;

  return;
}
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP

#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"
#include "../computer_vision/camera_matrix.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

#define GL_PACKED_CAMERA_SIZE 11
#define GL_3D_POINT_SIZE 3

class NelderMeadOptGordonLowe: public NelderMeadOptBase<double>
{
public:
    std::vector< std::vector< Vec<2, float> > > m;

    /**
     * @brief NelderMeadOptGordonLowe
     * @param m0
     * @param m1
     */
    NelderMeadOptGordonLowe(std::vector< Vec<2, float> > m0, std::vector< Vec<2, float> > m1) : NelderMeadOptBase()
    {
        this->m.push_back(m0);
        this->m.push_back(m1);
    }

    static Eigen::Matrix34d parseCameraMatrix(float *x, unsigned int index)
    {
        Eigen::Matrix3d K, R;
        Eigen::Vector3d t;

        //offset of the matrix
        unsigned int c = index * 11;

        Eigen::Quaternion<double> reg;
        reg.x() = x[c]; c++;
        reg.y() = x[c]; c++;
        reg.z() = x[c]; c++;
        reg.w() = x[c]; c++;
        R = reg.toRotationMatrix();

        t[0] = x[c]; c++;
        t[1] = x[c]; c++;
        t[2] = x[c]; c++;

        K.setZero();
        K(0, 0) = x[c]; c++;
        K(1, 1) = x[c]; c++;
        K(0, 2) = x[c]; c++;
        K(1, 2) = x[c];
        K(2, 2) = 1.0;

        return getCameraMatrix(K, R, t);
    }

    /**
     * @brief ProjectionError
     * @param x
     * @param index
     * @return
     */
    double ProjectionError(float *x, unsigned int index) {

       double err = 0.0;

       Eigen::Matrix34d P = parseCameraMatrix(x, index);

       //offset of vertices
       int c = GL_PACKED_CAMERA_SIZE * int(m.size());

       Eigen::Vector4d point;
       for(int i = 0; i < m[index].size(); i++) {
           point = Eigen::Vector4d(x[c], x[c + 1], x[c + 2], 1.0);

           Eigen::Vector3d point_proj = P * point;
           point_proj /= point_proj[2];

           double dx = point_proj[0] - m[index][i][0];
           double dy = point_proj[1] - m[index][i][1];

           err += dx * dx + dy * dy;

           c += 3;
       }

       return err;
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    float function(float *x, unsigned int n)
    {
        int n2 = int(m.size() * m[0].size());
        double err = sqrt((ProjectionError(x, 0) + ProjectionError(x, 1)) / double(n2));

        return float(err);
    }

    /**
     * @brief init3DPoints
     * @param K
     * @param m
     * @param x
     * @param distance
     */
    static void init3DPoints(Eigen::Matrix3d K, std::vector< Vec<2, float> > &m, std::vector< Eigen::Vector3d > &x, float distance = 20.0f)
    {
        Eigen::Matrix3d K_inv = K.inverse();
    \
        printf("Points: %zd\n", m.size());

        for(unsigned int i = 0; i < m.size(); i++) {
            Eigen::Vector3d point = Eigen::Vector3d (m[i][0], m[i][1], 1.0);

            point = K_inv * point;

            point *= distance;

            x.push_back(point);
        }
    }

    /**
     * @brief prepareInputData
     * @param K
     * @param R
     * @param t
     * @param x
     * @param ret_size
     * @return
     */
    static double *prepareInputData(std::vector< Eigen::Matrix3d > &K, std::vector< Eigen::Matrix3d > &R, std::vector< Eigen::Vector3d > &t, std::vector< Eigen::Vector3d > &x, unsigned int &ret_size)
    {
        if(R.size() != t.size()) {
            return NULL;
        }

        if(x.empty()) {
            return NULL;
        }

        int n = int (R.size());
        ret_size = GL_PACKED_CAMERA_SIZE * n + GL_3D_POINT_SIZE * int(x.size());
        double *ret = new double[ret_size];

        int c = 0;
        for(int i = 0; i < n; i++) {

            Eigen::Quaternion<double> reg(R[i]);

            ret[c] = reg.x(); c++;
            ret[c] = reg.y(); c++;
            ret[c] = reg.z(); c++;
            ret[c] = reg.w(); c++;

            ret[c] = t[i][0]; c++;
            ret[c] = t[i][1]; c++;
            ret[c] = t[i][2]; c++;

            ret[c] = K[i](0, 0); c++;
            ret[c] = K[i](1, 1); c++;
            ret[c] = K[i](0, 2); c++;
            ret[c] = K[i](1, 2); c++;
        }

        for(size_t i = 0; i < x.size(); i++) {
            ret[c] = x[i][0]; c++;
            ret[c] = x[i][1]; c++;
            ret[c] = x[i][2]; c++;
        }

        return ret;
    }
};

#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP