/*
 [auto_generated]
 boost/numeric/odeint/stepper/adams_bashforth.hpp

 [begin_description]
 Implementaton of the Adam-Bashforth method a multistep method used for the predictor step in the
 Adams-Bashforth-Moulton method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED

#include <boost/static_assert.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>

#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>

#include <boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_bashforth_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
size_t Steps ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer ,
class InitializingStepper = runge_kutta4< State , Value , Deriv , Time , Algebra , Operations, Resizer >
>
class adams_bashforth : public algebra_stepper_base< Algebra , Operations >
{

#ifndef DOXYGEN_SKIP
    BOOST_STATIC_ASSERT(( Steps > 0 ));
    BOOST_STATIC_ASSERT(( Steps < 9 ));
#endif

public :

    typedef State state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef Time time_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    typedef InitializingStepper initializing_stepper_type;

    typedef typename algebra_stepper_base< Algebra , Operations >::algebra_type algebra_type;
    typedef typename algebra_stepper_base< Algebra , Operations >::operations_type operations_type;
#ifndef DOXYGEN_SKIP
    typedef adams_bashforth< Steps , State , Value , Deriv , Time , Algebra , Operations , Resizer , InitializingStepper > stepper_type;
#endif
    static const size_t steps = Steps;

    typedef unsigned short order_type;
    static const order_type order_value = steps;

    typedef detail::rotating_buffer< wrapped_deriv_type , steps > step_storage_type;

    order_type order( void ) const { return order_value; }

    adams_bashforth( const algebra_type &algebra = algebra_type() )
    : m_step_storage() , m_resizer() , m_coefficients() ,
      m_steps_initialized( 0 ) , m_initializing_stepper() ,
      m_algebra( algebra )
    { }

    adams_bashforth( const adams_bashforth &stepper )
    : m_step_storage( stepper.m_step_storage ) , m_resizer( stepper.m_resizer ) , m_coefficients() ,
      m_steps_initialized( stepper.m_steps_initialized ) , m_initializing_stepper( stepper.m_initializing_stepper ) ,
      m_algebra( stepper.m_algebra )
    { }

    adams_bashforth& operator=( const adams_bashforth &stepper )
    {
        m_resizer = stepper.m_resizer;
        m_step_storage = stepper.m_step_storage;
        m_algebra = stepper.m_algebra;
        return *this;
    }

    /*
     * Version 1 : do_step( system , x , t , dt );
     *
     * solves the forwarding problem
     */
    template< class System , class StateInOut >
    void do_step( System system , StateInOut &x , time_type t , time_type dt )
    {
        do_step( system , x , t , x , dt );
    }

    /**
     * \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
     */
    template< class System , class StateInOut >
    void do_step( System system , const StateInOut &x , time_type t , time_type dt )
    {
        do_step( system , x , t , x , dt );
    }

    /*
     * Version 2 : do_step( system , in , t , out , dt );
     *
     * solves the forwarding problem
     */

    template< class System , class StateIn , class StateOut >
    void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
    {
        do_step_impl( system , in , t , out , dt );
    }

    /**
     * \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateOut.
     */
    template< class System , class StateIn , class StateOut >
    void do_step( System system , const StateIn &in , time_type t , const StateOut &out , time_type dt )
    {
        do_step_impl( system , in , t , out , dt );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

    const step_storage_type& step_storage( void ) const
    {
        return m_step_storage;
    }

    step_storage_type& step_storage( void )
    {
        return m_step_storage;
    }

    template< class ExplicitStepper , class System , class StateIn >
    void initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
    {
        typename odeint::unwrap_reference< ExplicitStepper >::type &stepper = explicit_stepper;
        typename odeint::unwrap_reference< System >::type &sys = system;

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );

        for( size_t i=0 ; i<steps-1 ; ++i )
        {
            if( i != 0 ) m_step_storage.rotate();
            sys( x , m_step_storage[0].m_v , t );
            stepper.do_step( system , x , m_step_storage[0].m_v , t , dt );
            t += dt;
        }
        m_steps_initialized = steps;
    }

    template< class System , class StateIn >
    void initialize( System system , StateIn &x , time_type &t , time_type dt )
    {
        initialize( detail::ref( m_initializing_stepper ) , system , x , t , dt );
    }

    void reset( void )
    {
        m_steps_initialized = 0;
    }

    bool is_initialized( void ) const
    {
        return m_steps_initialized >= steps;
    }

    const initializing_stepper_type& initializing_stepper( void ) const { return m_initializing_stepper; }

    initializing_stepper_type& initializing_stepper( void ) { return m_initializing_stepper; }

private:

    template< class System , class StateIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        if( m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) ) )
        {
            m_steps_initialized = 0;
        }

        if( m_steps_initialized < steps - 1 )
        {
            if( m_steps_initialized != 0 ) m_step_storage.rotate();
            sys( in , m_step_storage[0].m_v , t );
            m_initializing_stepper.do_step( system , in , m_step_storage[0].m_v , t , out , dt );
            m_steps_initialized++;
        }
        else
        {
            m_step_storage.rotate();
            sys( in , m_step_storage[0].m_v , t );
            detail::adams_bashforth_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_step_storage , m_coefficients , dt );
        }
    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        for( size_t i=0 ; i<steps ; ++i )
        {
            resized |= adjust_size_by_resizeability( m_step_storage[i] , x , typename is_resizeable<deriv_type>::type() );
        }
        return resized;
    }

    step_storage_type m_step_storage;
    resizer_type m_resizer;
    const detail::adams_bashforth_coefficients< value_type , steps > m_coefficients;
    size_t m_steps_initialized;
    initializing_stepper_type m_initializing_stepper;

protected:

    algebra_type m_algebra;
};

/***** DOXYGEN *****/

/**
 * \class adams_bashforth
 * \brief The Adams-Bashforth multistep algorithm.
 *
 * The Adams-Bashforth method is a multi-step algorithm with configurable step
 * number. The step number is specified as template parameter Steps and it
 * then uses the result from the previous Steps steps. See also
 * en.wikipedia.org/wiki/Linear_multistep_method.
 * Currently, a maximum of Steps=8 is supported.
 * The method is explicit and fulfills the Stepper concept. Step size control
 * or continuous output are not provided.
 *
 * This class derives from algebra_base and inherits its interface via
 * CRTP (current recurring template pattern). For more details see
 * algebra_stepper_base.
 *
 * \tparam Steps The number of steps (maximal 8).
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 * \tparam InitializingStepper The stepper for the first two steps.
 */

    /**
     * \fn adams_bashforth::adams_bashforth( const algebra_type &algebra )
     * \brief Constructs the adams_bashforth class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored.
     */

    /**
     * \fn order_type adams_bashforth::order( void ) const
     * \brief Returns the order of the algorithm, which is equal to the number of steps.
     * \return order of the method.
     */

    /**
     * \fn void adams_bashforth::do_step( System system , StateInOut &x , time_type t , time_type dt )
     * \brief This method performs one step. It transforms the result in-place.
     *
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn void adams_bashforth::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
     * \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn void adams_bashforth::adjust_size( const StateType &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

    /**
     * \fn const step_storage_type& adams_bashforth::step_storage( void ) const
     * \brief Returns the storage of intermediate results.
     * \return The storage of intermediate results.
     */

    /**
     * \fn step_storage_type& adams_bashforth::step_storage( void )
     * \brief Returns the storage of intermediate results.
     * \return The storage of intermediate results.
     */

    /**
     * \fn void adams_bashforth::initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
     * \brief Initialized the stepper. Does Steps-1 steps with the explicit_stepper to fill the buffer.
     * \param explicit_stepper the stepper used to fill the buffer of previous step results
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn void adams_bashforth::initialize( System system , StateIn &x , time_type &t , time_type dt )
     * \brief Initialized the stepper. Does Steps-1 steps with an internal instance of InitializingStepper to fill the buffer.
     * \note The state x and time t are updated to the values after Steps-1 initial steps.
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Simple System concept.
     * \param x The initial state of the ODE which should be solved, updated in this method.
     * \param t The initial value of the time, updated in this method.
     * \param dt The step size.
     */

    /**
     * \fn void adams_bashforth::reset( void )
     * \brief Resets the internal buffer of the stepper.
     */

    /**
     * \fn bool adams_bashforth::is_initialized( void ) const
     * \brief Returns true if the stepper has been initialized.
     * \return bool true if stepper is initialized, false otherwise
     */

    /**
     * \fn const initializing_stepper_type& adams_bashforth::initializing_stepper( void ) const
     * \brief Returns the internal initializing stepper instance.
     * \return initializing_stepper
     */

    /**
     * \fn const initializing_stepper_type& adams_bashforth::initializing_stepper( void ) const
     * \brief Returns the internal initializing stepper instance.
     * \return initializing_stepper
     */

    /**
     * \fn initializing_stepper_type& adams_bashforth::initializing_stepper( void )
     * \brief Returns the internal initializing stepper instance.
     * \return initializing_stepper
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED


/* Author: Wolfgang Bangerth, University of Heidelberg, 2001 */

/*    $Id: step-11.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2001-2004, 2006, 2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// As usual, the program starts with a rather long list of include files which
// you are probably already used to by now:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/table_handler.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

// Just this one is new: it declares a class
// CompressedSparsityPattern, which we will use and explain
// further down below.
#include <deal.II/lac/compressed_sparsity_pattern.h>

// We will make use of the std::find algorithm of the C++ standard library, so
// we have to include the following file for its declaration:
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cmath>

// The last step is as in all previous programs:
namespace Step11
{
  using namespace dealii;

  // Then we declare a class which represents the solution of a Laplace
  // problem. As this example program is based on step-5, the class looks
  // rather the same, with the sole structural difference that the functions
  // assemble_system now calls solve itself, and is
  // thus called assemble_and_solve, and that the output function
  // was dropped since the solution function is so boring that it is not worth
  // being viewed.
  //
  // The only other noteworthy change is that the constructor takes a value
  // representing the polynomial degree of the mapping to be used later on,
  // and that it has another member variable representing exactly this
  // mapping. In general, this variable will occur in real applications at the
  // same places where the finite element is declared or used.
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem (const unsigned int mapping_degree);
    void run ();

  private:
    void setup_system ();
    void assemble_and_solve ();
    void solve ();

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

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    ConstraintMatrix     mean_value_constraints;

    Vector<double>       solution;
    Vector<double>       system_rhs;

    TableHandler         output_table;
  };

  // Construct such an object, by initializing the variables. Here, we use
  // linear finite elements (the argument to the fe variable
  // denotes the polynomial degree), and mappings of given order. Print to
  // screen what we are about to do.
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem (const unsigned int mapping_degree) :
    fe (1),
    dof_handler (triangulation),
    mapping (mapping_degree)
  {
    std::cout << "Using mapping with degree " << mapping_degree << ":"
              << std::endl
              << "============================"
              << std::endl;
  }

  // The first task is to set up the variables for this problem. This includes
  // generating a valid DoFHandler object, as well as the
  // sparsity patterns for the matrix, and the object representing the
  // constraints that the mean value of the degrees of freedom on the boundary
  // be zero.
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    // The first task is trivial: generate an enumeration of the degrees of
    // freedom, and initialize solution and right hand side vector to their
    // correct sizes:
    dof_handler.distribute_dofs (fe);
    solution.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());

    // Next task is to construct the object representing the constraint that
    // the mean value of the degrees of freedom on the boundary shall be
    // zero. For this, we first want a list of those nodes which are actually
    // at the boundary. The DoFTools class has a function that
    // returns an array of boolean values where true indicates
    // that the node is at the boundary. The second argument denotes a mask
    // selecting which components of vector valued finite elements we want to
    // be considered. This sort of information is encoded using the
    // ComponentMask class (see also @ref GlossComponentMask). Since we have a
    // scalar finite element anyway, this mask in reality should have only one
    // entry with a true value. However, the ComponentMask class
    // has semantics that allow it to represents a mask of indefinite size
    // whose every element equals true when one just default
    // constructs such an object, so this is what we'll do here.
    std::vector<bool> boundary_dofs (dof_handler.n_dofs(), false);
    DoFTools::extract_boundary_dofs (dof_handler,
                                     ComponentMask(),
                                     boundary_dofs);

    // Now first for the generation of the constraints: as mentioned in the
    // introduction, we constrain one of the nodes on the boundary by the
    // values of all other DoFs on the boundary. So, let us first pick out the
    // first boundary node from this list. We do that by searching for the
    // first true value in the array (note that
    // std::find returns an iterator to this element), and
    // computing its distance to the overall first element in the array to get
    // its index:
    const unsigned int first_boundary_dof
      = std::distance (boundary_dofs.begin(),
                       std::find (boundary_dofs.begin(),
                                  boundary_dofs.end(),
                                  true));

    // Then generate a constraints object with just this one constraint. First
    // clear all previous content (which might reside there from the previous
    // computation on a once coarser grid), then add this one line
    // constraining the first_boundary_dof to the sum of other
    // boundary DoFs each with weight -1. Finally, close the constraints
    // object, i.e. do some internal bookkeeping on it for faster processing
    // of what is to come later:
    mean_value_constraints.clear ();
    mean_value_constraints.add_line (first_boundary_dof);
    for (unsigned int i=first_boundary_dof+1; i<dof_handler.n_dofs(); ++i)
      if (boundary_dofs[i] == true)
        mean_value_constraints.add_entry (first_boundary_dof,
                                          i, -1);
    mean_value_constraints.close ();

    // Next task is to generate a sparsity pattern. This is indeed a tricky
    // task here. Usually, we just call
    // DoFTools::make_sparsity_pattern and condense the result
    // using the hanging node constraints. We have no hanging node constraints
    // here (since we only refine globally in this example), but we have this
    // global constraint on the boundary. This poses one severe problem in
    // this context: the SparsityPattern class wants us to state
    // beforehand the maximal number of entries per row, either for all rows
    // or for each row separately. There are functions in the library which
    // can tell you this number in case you just have hanging node constraints
    // (namely DoFHandler::max_coupling_between_dofs), but how is
    // this for the present case? The difficulty arises because the
    // elimination of the constrained degree of freedom requires a number of
    // additional entries in the matrix at places that are not so simple to
    // determine. We would therefore have a problem had we to give a maximal
    // number of entries per row here.
    //
    // Since this can be so difficult that no reasonable answer can be given
    // that allows allocation of only a reasonable amount of memory, there is
    // a class CompressedSparsityPattern, that can help us out
    // here. It does not require that we know in advance how many entries rows
    // could have, but allows just about any length. It is thus significantly
    // more flexible in case you do not have good estimates of row lengths,
    // however at the price that building up such a pattern is also
    // significantly more expensive than building up a pattern for which you
    // had information in advance. Nevertheless, as we have no other choice
    // here, we'll just build such an object by initializing it with the
    // dimensions of the matrix and calling another function
    // DoFTools::make_sparsity_pattern to get the sparsity
    // pattern due to the differential operator, then condense it with the
    // constraints object which adds those positions in the sparsity pattern
    // that are required for the elimination of the constraint.
    CompressedSparsityPattern csp (dof_handler.n_dofs(),
                                   dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, csp);
    mean_value_constraints.condense (csp);

    // Finally, once we have the full pattern, we can initialize an object of
    // type SparsityPattern from it and in turn initialize the
    // matrix with it. Note that this is actually necessary, since the
    // CompressedSparsityPattern is so inefficient compared to
    // the SparsityPattern class due to the more flexible data
    // structures it has to use, that we can impossibly base the sparse matrix
    // class on it, but rather need an object of type
    // SparsityPattern, which we generate by copying from the
    // intermediate object.
    //
    // As a further sidenote, you will notice that we do not explicitly have
    // to compress the sparsity pattern here. This, of course, is
    // due to the fact that the copy_from function generates a
    // compressed object right from the start, to which you cannot add new
    // entries anymore. The compress call is therefore implicit
    // in the copy_from call.
    sparsity_pattern.copy_from (csp);
    system_matrix.reinit (sparsity_pattern);
  }

  // The next function then assembles the linear system of equations, solves
  // it, and evaluates the solution. This then makes three actions, and we
  // will put them into eight true statements (excluding declaration of
  // variables, and handling of temporary vectors). Thus, this function is
  // something for the very lazy. Nevertheless, the functions called are
  // rather powerful, and through them this function uses a good deal of the
  // whole library. But let's look at each of the steps.
  template <int dim>
  void LaplaceProblem<dim>::assemble_and_solve ()
  {

    // First, we have to assemble the matrix and the right hand side. In all
    // previous examples, we have investigated various ways how to do this
    // manually. However, since the Laplace matrix and simple right hand sides
    // appear so frequently in applications, the library provides functions
    // for actually doing this for you, i.e. they perform the loop over all
    // cells, setting up the local matrices and vectors, and putting them
    // together for the end result.
    //
    // The following are the two most commonly used ones: creation of the
    // Laplace matrix and creation of a right hand side vector from body or
    // boundary forces. They take the mapping object, the
    // DoFHandler object representing the degrees of freedom and
    // the finite element in use, a quadrature formula to be used, and the
    // output object. The function that creates a right hand side vector also
    // has to take a function object describing the (continuous) right hand
    // side function.
    //
    // Let us look at the way the matrix and body forces are integrated:
    const unsigned int gauss_degree
      = std::max (static_cast<unsigned int>(std::ceil(1.*(mapping.get_degree()+1)/2)),
                  2U);
    MatrixTools::create_laplace_matrix (mapping, dof_handler,
                                        QGauss<dim>(gauss_degree),
                                        system_matrix);
    VectorTools::create_right_hand_side (mapping, dof_handler,
                                         QGauss<dim>(gauss_degree),
                                         ConstantFunction<dim>(-2),
                                         system_rhs);
    // That's quite simple, right?
    //
    // Two remarks are in order, though: First, these functions are used in a
    // lot of contexts. Maybe you want to create a Laplace or mass matrix for
    // a vector values finite element; or you want to use the default Q1
    // mapping; or you want to assembled the matrix with a coefficient in the
    // Laplace operator. For this reason, there are quite a large number of
    // variants of these functions in the MatrixCreator and
    // MatrixTools classes. Whenever you need a slightly
    // different version of these functions than the ones called above, it is
    // certainly worthwhile to take a look at the documentation and to check
    // whether something fits your needs.
    //
    // The second remark concerns the quadrature formula we use: we want to
    // integrate over bilinear shape functions, so we know that we have to use
    // at least a Gauss2 quadrature formula. On the other hand, we want to
    // have the quadrature rule to have at least the order of the boundary
    // approximation. Since the order of Gauss-r is 2r, and the order of the
    // boundary approximation using polynomials of degree p is p+1, we know
    // that 2r@>=p+1. Since r has to be an integer and (as mentioned above)
    // has to be at least 2, this makes up for the formula above computing
    // gauss_degree.
    //
    // Since the generation of the body force contributions to the right hand
    // side vector was so simple, we do that all over again for the boundary
    // forces as well: allocate a vector of the right size and call the right
    // function. The boundary function has constant values, so we can generate
    // an object from the library on the fly, and we use the same quadrature
    // formula as above, but this time of lower dimension since we integrate
    // over faces now instead of cells:
    Vector<double> tmp (system_rhs.size());
    VectorTools::create_boundary_right_hand_side (mapping, dof_handler,
                                                  QGauss<dim-1>(gauss_degree),
                                                  ConstantFunction<dim>(1),
                                                  tmp);
    // Then add the contributions from the boundary to those from the interior
    // of the domain:
    system_rhs += tmp;
    // For assembling the right hand side, we had to use two different vector
    // objects, and later add them together. The reason we had to do so is
    // that the VectorTools::create_right_hand_side and
    // VectorTools::create_boundary_right_hand_side functions
    // first clear the output vector, rather than adding up their results to
    // previous contents. This can reasonably be called a design flaw in the
    // library made in its infancy, but unfortunately things are as they are
    // for some time now and it is difficult to change such things that
    // silently break existing code, so we have to live with that.

    // Now, the linear system is set up, so we can eliminate the one degree of
    // freedom which we constrained to the other DoFs on the boundary for the
    // mean value constraint from matrix and right hand side vector, and solve
    // the system. After that, distribute the constraints again, which in this
    // case means setting the constrained degree of freedom to its proper
    // value
    mean_value_constraints.condense (system_matrix);
    mean_value_constraints.condense (system_rhs);

    solve ();
    mean_value_constraints.distribute (solution);

    // Finally, evaluate what we got as solution. As stated in the
    // introduction, we are interested in the H1 semi-norm of the
    // solution. Here, as well, we have a function in the library that does
    // this, although in a slightly non-obvious way: the
    // VectorTools::integrate_difference function integrates the
    // norm of the difference between a finite element function and a
    // continuous function. If we therefore want the norm of a finite element
    // field, we just put the continuous function to zero. Note that this
    // function, just as so many other ones in the library as well, has at
    // least two versions, one which takes a mapping as argument (which we
    // make us of here), and the one which we have used in previous examples
    // which implicitly uses MappingQ1.  Also note that we take a
    // quadrature formula of one degree higher, in order to avoid
    // superconvergence effects where the solution happens to be especially
    // close to the exact solution at certain points (we don't know whether
    // this might be the case here, but there are cases known of this, and we
    // just want to make sure):
    Vector<float> norm_per_cell (triangulation.n_active_cells());
    VectorTools::integrate_difference (mapping, dof_handler,
                                       solution,
                                       ZeroFunction<dim>(),
                                       norm_per_cell,
                                       QGauss<dim>(gauss_degree+1),
                                       VectorTools::H1_seminorm);
    // Then, the function just called returns its results as a vector of
    // values each of which denotes the norm on one cell. To get the global
    // norm, a simple computation shows that we have to take the l2 norm of
    // the vector:
    const double norm = norm_per_cell.l2_norm();

    // Last task -- generate output:
    output_table.add_value ("cells", triangulation.n_active_cells());
    output_table.add_value ("|u|_1", norm);
    output_table.add_value ("error", std::fabs(norm-std::sqrt(3.14159265358/2)));
  }

  // The following function solving the linear system of equations is copied
  // from step-5 and is explained there in some detail:
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {
    SolverControl           solver_control (1000, 1e-12);
    SolverCG<>              cg (solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);
  }

  // Finally the main function controlling the different steps to be
  // performed. Its content is rather straightforward, generating a
  // triangulation of a circle, associating a boundary to it, and then doing
  // several cycles on subsequently finer grids. Note again that we have put
  // mesh refinement into the loop header; this may be something for a test
  // program, but for real applications you should consider that this implies
  // that the mesh is refined after the loop is executed the last time since
  // the increment clause (the last part of the three-parted loop header) is
  // executed before the comparison part (the second one), which may be rather
  // costly if the mesh is already quite refined. In that case, you should
  // arrange code such that the mesh is not further refined after the last
  // loop run (or you should do it at the beginning of each run except for the
  // first one).
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    GridGenerator::hyper_ball (triangulation);
    static const HyperBallBoundary<dim> boundary;
    triangulation.set_boundary (0, boundary);

    for (unsigned int cycle=0; cycle<6; ++cycle, triangulation.refine_global(1))
      {
        setup_system ();
        assemble_and_solve ();
      };

    // After all the data is generated, write a table of results to the
    // screen:
    output_table.set_precision("|u|_1", 6);
    output_table.set_precision("error", 6);
    output_table.write_text (std::cout);
    std::cout << std::endl;
  }
}

// Finally the main function. It's structure is the same as that used in
// several of the previous examples, so probably needs no more explanation.
int main ()
{
  try
    {
      dealii::deallog.depth_console (0);
      std::cout.precision(5);

      // This is the main loop, doing the computations with mappings of linear
      // through cubic mappings. Note that since we need the object of type
      // LaplaceProblem@<2@> only once, we do not even name it,
      // but create an unnamed such object and call the run
      // function of it, subsequent to which it is immediately destroyed
      // again.
      for (unsigned int mapping_degree=1; mapping_degree<=3; ++mapping_degree)
        Step11::LaplaceProblem<2>(mapping_degree).run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    };

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//
// THIS SOFTWARE IS PROVIDED "AS-IS". THERE IS NO WARRANTY OF ANY KIND.
// NEITHER THE AUTHORS NOR THE OHIO STATE UNIVERSITY WILL BE LIABLE
// FOR ANY DAMAGES OF ANY KIND, EVEN IF ADVISED OF SUCH POSSIBILITY.
//
// Copyright (c) 2010 Jyamiti Research Group.
// CS&E Department of the Ohio State University, Columbus, OH.
// All rights reserved.
//
// Author: Sayan Mandal
//
///////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <ctime>
#include <map>
#include <list>
#include <vector>
#include <cmath>
#include <sstream>
#include <unordered_set>
#include <string>
#include <sys/stat.h>
#include <cstddef>

#ifndef INFINITY
#define INFINITY std::numeric_limits< double >::infinity()
#endif // INFINITY

#include <boost/config.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include <boost/geometry.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
//#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/graph_utility.hpp>

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/reader_utils.h>
#include <gudhi/Bitmap_cubical_complex.h>
#include <gudhi/Persistent_cohomology.h>
// standard stuff

#include<ParseCommand.h>
#include <Legal.h>
#include "Graph.h"

using namespace boost;
using namespace std;
namespace bg = boost::geometry;

//extern vector<int> complexSizes;
//extern vector<int> accumulativeSizes;
//extern int accumulativeSize;
//extern double fThreshold;
//extern vector<double> vecFiltrationScale;
//extern SimplicialTree<bool> domain_complex;

typedef Gudhi::cubical_complex::Bitmap_cubical_complex_base<double> Bitmap_cubical_complex_base;
typedef Gudhi::cubical_complex::Bitmap_cubical_complex<Bitmap_cubical_complex_base> Bitmap_cubical_complex;
typedef Gudhi::persistent_cohomology::Field_Zp Field_Zp;
typedef Gudhi::persistent_cohomology::Persistent_cohomology<Bitmap_cubical_complex, Field_Zp> Persistent_cohomology;

extern map <int,int> threesimplex_to_node; // maps index of tetrahedrons from simplex->first to dual-graph-nodes->second
extern map <int,int> twosimplex_to_tri; // maps index of traingles from simplex->first to dual-graph-edges->second
extern map <int,int> threesimplex_to_node_rev; // maps index of tetrahedrons from dual-graph-nodes->first to simplex->second
extern map <int,int> twosimplex_to_tri_rev; // maps index of traingles from dual-graph-edges->first to simplex->second
extern map<size_t, vector<int>>  sizescale;//0- edge, 1- vertex, for each deathindex, count of vertex and edges
extern vector<double> x;
extern vector<double> y;
extern vector<double> z;
extern map<unsigned long,unsigned long> vertind; // vertex index in cubical complex(first) to actual index in x,y,z, filter

struct cmp_intervals_by_dim_then_length {
    explicit cmp_intervals_by_dim_then_length(Bitmap_cubical_complex * sc)
    : sc_(sc) { }
    template<typename Persistent_interval>
    bool operator()(const Persistent_interval & p1, const Persistent_interval & p2) {
        if (sc_->dimension(get < 0 > (p1)) == sc_->dimension(get < 0 > (p2)))
            return (sc_->filtration(get < 1 > (p1)) - sc_->filtration(get < 0 > (p1))
                    > sc_->filtration(get < 1 > (p2)) - sc_->filtration(get < 0 > (p2)));
        else
            return (sc_->dimension(get < 0 > (p1)) > sc_->dimension(get < 0 > (p2)));
    }
    Bitmap_cubical_complex* sc_;
};

Bitmap_cubical_complex buildComplex(const char* file) {

    bool dbg = false;
    std::ifstream inFiltration;
    inFiltration.open(file);

    unsigned dimensionOfData;
    inFiltration >> dimensionOfData;

    if (dbg) {
        std::cerr << "dimensionOfData : " << dimensionOfData << std::endl;
    }

    std::vector<unsigned> sizes;
    sizes.reserve(dimensionOfData);
    // all dimensions multiplied
    std::size_t dimensions = 1;
    size_t top_dim = 1;
    for (std::size_t i = 0; i != dimensionOfData; ++i) {
        unsigned size_in_this_dimension;
        inFiltration >> size_in_this_dimension;
        sizes.push_back(size_in_this_dimension - 1);
        dimensions *= (size_in_this_dimension);
        top_dim *= (size_in_this_dimension - 1);
        if (dbg) {
            std::cerr << "size_in_this_dimension : " << size_in_this_dimension << std::endl;
        }
    }
    if (dbg) {
        std::cerr << "Top dimension names : " << top_dim << std::endl;
    }

    //Create a blank Cubical Complex
    vector<double> top_dim_cells(top_dim, 0);
    Bitmap_cubical_complex b(sizes, top_dim_cells);
    double filtrationLevel;

    vector<size_t> indices_to_consider;

    //Assign values to vertices
    //Bitmap_cubical_complex::Skeleton_simplex_iterator it(&b, 0);
    for (Bitmap_cubical_complex::Skeleton_simplex_iterator it = b.skeleton_simplex_range(0).begin();
         it != b.skeleton_simplex_range(0).end(); it++) {
        if (!(inFiltration >> filtrationLevel) || (inFiltration.eof())) {
            throw std::ios_base::failure("Bad Perseus file format.");
        }
        if (dbg) {
            std::cerr << "Cell of an index : " << (*it)
            << " and dimension: " << b.get_dimension_of_a_cell(*it)
            << " get the value : " << filtrationLevel << std::endl;
        }
        b.get_cell_data(*it) = filtrationLevel;
        size_t s = (*it);
        indices_to_consider.push_back(s);

    }
    vector<bool> is_this_cell_considered(b.num_simplices(), false);
    while (indices_to_consider.size()) {
        if (dbg) {
            std::cerr << "indices_to_consider in this iteration \n";
            for (std::size_t i = 0; i != indices_to_consider.size(); ++i) {
                std::cout << indices_to_consider[i] << "  ";
            }
        }
        std::vector<std::size_t> new_indices_to_consider;
        for (std::size_t i = 0; i != indices_to_consider.size(); ++i) {
            std::vector<std::size_t> bd = b.get_coboundary_of_a_cell(indices_to_consider[i]);
            for (std::size_t boundaryIt = 0; boundaryIt != bd.size(); ++boundaryIt) {
                if (dbg) {
                    std::cerr << "filtration of a cell : " << bd[boundaryIt] << " is : " << b.filtration(bd[boundaryIt])
                    << " while of a cell: " << indices_to_consider[i] << " is: " << b.filtration(indices_to_consider[i])
                    << std::endl;
                }
                if (b.filtration(bd[boundaryIt]) < b.filtration(indices_to_consider[i])) {
                    b.get_cell_data(bd[boundaryIt]) = b.filtration(indices_to_consider[i]);
                    if (dbg) {
                        std::cerr << "Setting the value of a cell : " << bd[boundaryIt]
                        << " to : " << b.filtration(indices_to_consider[i]) << std::endl;
                    }
                }
                if (is_this_cell_considered[bd[boundaryIt]] == false) {
                    new_indices_to_consider.push_back(bd[boundaryIt]);
//                    if (b.get_dimension_of_a_cell(bd[boundaryIt]) == dimensionOfData - 1) {
//                        edges->push_back(bd[boundaryIt]);
//                    }
                    is_this_cell_considered[bd[boundaryIt]] = true;
                }
            }
        }
        indices_to_consider.swap(new_indices_to_consider);
    }

    b.initialize_simplex_associated_to_key();
//cub = b;
    return b;
}

int main(int argc, char *argv[] )
{
    int dimensions, noPoints, currentEdgeSize = 0, barcode_count = 0, scalecount = 0;
    int filtration = 0, totinsCount = 0, numbars = 0, index = 0, gap = 0;

  string point_file, tr_file, to_file;
  std::vector<bg::model::point<double, 3, bg::cs::cartesian>> vPoint;
  bool verbose;
    ParseCommand(argc, argv,  point_file, numbars, verbose, index, gap);
//  Simplex_tree simplexTree;
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    cout<<"Point_file: "<<point_file; //getchar();
    // THIS LINE GENERATES THE CUBICAL COMPLEX
    Bitmap_cubical_complex cub(point_file.c_str());//buildComplex(point_file.c_str());//(//
//    c = ;
    // Compute the persistence diagram of the complex
    Persistent_cohomology pcoh(cub);

    int p = 2;
    double min_persistence = 0;
    vector<size_t> birthiv, deathiv;
    pcoh.init_coefficients(p);  // initializes the coefficient field for homology
    pcoh.compute_persistent_cohomology(min_persistence);
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ofstream spers(point_file+"_pers");
    ofstream fpers(point_file+"_filtpers");

    cmp_intervals_by_dim_then_length cmp(&cub);
    auto persistent_pairs = pcoh.get_persistent_pairs();
    cout<<" persistent_pairs: "<<persistent_pairs.size()<<"\n";
    if(verbose)
        getchar();
    sort(std::rbegin(persistent_pairs), std::rend(persistent_pairs), cmp);

    for (auto pair : persistent_pairs) {
        if(cub.dimension(get<0>(pair))!=2)
            continue;

        birthiv.push_back(get<0>(pair));
        deathiv.push_back(get<1>(pair));
    }

    if(birthiv.size()<numbars)
    {
        cout<<"Not enough 2-cycles: "<<birthiv.size()<<" "<<numbars<<endl;
        getchar();
        exit(0);
    }
//        birthiv.erase(birthiv.begin(),birthiv.end()-numbars);
//        deathiv.erase(deathiv.begin(),deathiv.end()-numbars);
    if(verbose){
        cout<<"birth size: "<<birthiv.size()<<" "<<deathiv.size()<<" numbers: "<<numbars<<" gaps: "<<gap;
    getchar();
    }
    birthiv.erase(birthiv.begin(),birthiv.end()-numbars-gap);
    deathiv.erase(deathiv.begin(),deathiv.end()-numbars-gap);
    birthiv.erase(birthiv.end()-gap,birthiv.end());
    deathiv.erase(deathiv.end()-gap,deathiv.end());
    buildFullCubicSetting(cub, point_file, deathiv);        // GRAPH BUILDING
    cout<<"Sizes: "<<birthiv.size()<<" "<<deathiv.size()<<" "<<numbars<<" "<<(birthiv.size()-numbars);
    if(verbose)
     getchar();

    for(int gotonumbars=birthiv.size()-1; gotonumbars >= 0; gotonumbars--){

        size_t birthi = birthiv[gotonumbars];
        size_t deathi = deathiv[gotonumbars];
//        cout<<"birthi: "<<birthi<<" deathi:"<<deathi<<endl;
//        cout.setf(ios::fixed);

        spers<<"2 "<<to_string(int(birthi))<<" "<<to_string(int(deathi))<<"\n";
        fpers<<"2 "<<to_string(float(cub.filtration(birthi)))<<" "<<to_string(float(cub.filtration(deathi)))<<"\n";
        if(verbose){
            cout<<"birthi: "<<birthi<<" deathi:"<<deathi<<endl;
            getchar();

        }

        vector<vector<int>> rGraph;
        vector<vector<int>> vectri
        = boostCut( cub, birthi, deathi, verbose);
//        = minCut(adjMatrix, threesimplex_to_node[death], adjMatrix.size()-1, rGraph );
        cout<<" Found Boykov Komolov cut.\n"; //getchar();
        string ind = point_file;

        string dir_path = ind+"loops/";

        boost::filesystem::path dir(dir_path.c_str());
        boost::filesystem::create_directory(dir_path.c_str());
        string ff = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".off";
        tr_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".txt";
        to_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".off";
        string cu_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+"cub.txt";
        cout<<"tr_file:"<<tr_file<<endl;
//        writeToFile( cub, vectri, birthi, deathi, tr_file);//
        writeToFileOFF( cub, vectri, birthi, deathi, to_file);
        cout<<"Written to TEXT: "<<gotonumbars<<" vec: "<<birthiv.size()<<" current index:"<<gotonumbars<<endl; //getchar();

    }

    threesimplex_to_node.clear();
    twosimplex_to_tri.clear();
    threesimplex_to_node_rev.clear();
    twosimplex_to_tri_rev.clear();
    sizescale.clear();
    x.clear();
    y.clear();
    z.clear();
    vertind.clear();

  return EXIT_SUCCESS;
}//end of main


#include "alpha_shapes.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>

int main(int argc, char* argv[]){
	if(argc != 2){
		std::cout << "usage: " << argv[0] << " <alpha>" << std::endl;
		return 1;
	}
	double alpha = boost::lexical_cast<double>(argv[1]);

	unsigned int num_edges, num_triangles, num_tetrahedra;
	double points[] = {
		.2,.2,.2,
		1,0,0,
		0,1,0,
		0,0,1,
		.7,.7,.7
	};
	double weights[] = {
		1, 1, 1, 1, 1
	};
	unsigned int* edges;
	unsigned int* triangles;
	unsigned int* tetrahedra;

	alpha_shapes(
		5, alpha, points, weights,
		&num_edges, &edges,
		&num_triangles, &triangles,
		&num_tetrahedra, &tetrahedra
	);

	std::cout << "Tetra " << num_tetrahedra << std::endl;
	for(unsigned int i = 0; i < num_tetrahedra*4; i+=4){
		for(unsigned int j = 0; j < 4; ++j){
			std::cout << "\t" << tetrahedra[i+j];
		}
		std::cout << std::endl;
	}

	std::cout << "Tri " << num_triangles << std::endl;
	for(unsigned int i = 0; i < num_triangles*3; i+=3){
		for(unsigned int j = 0; j < 3; ++j){
			std::cout << "\t" << triangles[i+j];
		}
		std::cout << std::endl;
	}

	std::cout << "Edge " << num_edges << std::endl;
	for(unsigned int i = 0; i < num_edges*2; i+=2){
		for(unsigned int j = 0; j < 2; ++j){
			std::cout << "\t" << edges[i+j];
		}
		std::cout << std::endl;
	}

	return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <arc_utilities/arc_helpers.hpp>
#include <Eigen/Geometry>

#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
    #include <omp.h>
#endif

#ifndef SIMPLE_HIERARCHICAL_CLUSTERING_HPP
#define SIMPLE_HIERARCHICAL_CLUSTERING_HPP

namespace simple_hierarchical_clustering
{
    enum CLUSTER_STRATEGY { SINGLE_LINK, COMPLETE_LINK };

    class SimpleHierarchicalClustering
    {
    private:

        SimpleHierarchicalClustering() {}

        static inline size_t GetNumOMPThreads()
        {
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            size_t num_threads = 0;
            #pragma omp parallel
            {
                num_threads = (size_t)omp_get_num_threads();
            }
            return num_threads;
#else
            return 1;
#endif
        }

        static std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> GetClosestPair(const std::vector<uint8_t>& datapoint_mask, const Eigen::MatrixXd& distance_matrix, const std::vector<std::vector<int64_t>>& clusters, const CLUSTER_STRATEGY strategy)
        {
            // Compute distances between unclustered points <-> unclustered points, unclustered_points <-> clusters, and clusters <-> clusters
            // Compute the minimum unclustered point <-> unclustered point / unclustered_point <-> cluster distance
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            const size_t num_threads = GetNumOMPThreads();
            std::vector<double> per_thread_min_distances(num_threads, INFINITY);
            std::vector<std::pair<int64_t, std::pair<bool, int64_t>>> per_thread_min_element_pairs(num_threads, std::make_pair(-1, std::make_pair(false, -1)));
            #pragma omp parallel for
#else
            double min_distance = INFINITY;
            std::pair<int64_t, std::pair<bool, int64_t>> min_element_pair(-1, std::pair<bool, int64_t>(false, -1));
#endif
            for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
            {
                // Make sure we aren't in a cluster already
                if (datapoint_mask[idx] == 0)
                {
                    // Compute the minimum unclustered point <-> unclustered point distance
                    double min_point_point_distance = INFINITY;
                    int64_t min_point_index = -1;
                    for (size_t jdx = 0; jdx < datapoint_mask.size(); jdx++)
                    {
                        // Make sure the other point isn't us, and isn't already in a cluster
                        if ((idx != jdx) && (datapoint_mask[jdx] == 0))
                        {
                            const double& current_distance = distance_matrix((ssize_t)idx, (ssize_t)jdx);
                            // Update the closest point
                            if (current_distance < min_point_point_distance)
                            {
                                min_point_point_distance = current_distance;
                                min_point_index = (int64_t)jdx;
                            }
                        }
                    }
                    // Compute the minimum unclustered point <-> cluster distance
                    double min_point_cluster_distance = INFINITY;
                    int64_t min_cluster_index = -1;
                    for (size_t cdx = 0; cdx < clusters.size(); cdx++)
                    {
                        // We only work with clusters that aren't empty
                        if (clusters[cdx].size() > 0)
                        {
                            // Compute the distance to the current cluster
                            double complete_link_distance = 0.0;
                            double single_link_distance = INFINITY;
                            for (size_t cpdx = 0; cpdx < clusters[cdx].size(); cpdx++)
                            {
                                const int64_t& current_cluster_point_index = clusters[cdx][cpdx];
                                const double& new_distance = distance_matrix((ssize_t)idx, (ssize_t)current_cluster_point_index);
                                complete_link_distance = std::max(complete_link_distance, new_distance);
                                single_link_distance = std::min(single_link_distance, new_distance);
                            }
                            const double current_distance = (strategy == COMPLETE_LINK) ? complete_link_distance : single_link_distance;
                            // Update the closest cluster
                            if (current_distance < min_point_cluster_distance)
                            {
                                min_point_cluster_distance = current_distance;
                                min_cluster_index = (int64_t)cdx;
                            }
                        }
                    }
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
                    const size_t thread_num = (size_t)omp_get_thread_num();
                    double& per_thread_min_distance = per_thread_min_distances[thread_num];
                    std::pair<int64_t, std::pair<bool, int64_t>>& per_thread_min_element_pair = per_thread_min_element_pairs[thread_num];
                    // Update the closest index
                    if (min_point_point_distance < per_thread_min_distance)
                    {
                        per_thread_min_distance = min_point_point_distance;
                        per_thread_min_element_pair.first = idx;
                        per_thread_min_element_pair.second.first = false;
                        per_thread_min_element_pair.second.second = min_point_index;
                    }
                    if (min_point_cluster_distance < per_thread_min_distance)
                    {
                        per_thread_min_distance = min_point_cluster_distance;
                        per_thread_min_element_pair.first = idx;
                        per_thread_min_element_pair.second.first = true;
                        per_thread_min_element_pair.second.second = min_cluster_index;
                    }
#else
                    // Update the closest index
                    if (min_point_point_distance < min_distance)
                    {
                        min_distance = min_point_point_distance;
                        min_element_pair.first = (int64_t)idx;
                        min_element_pair.second.first = false;
                        min_element_pair.second.second = min_point_index;
                    }
                    if (min_point_cluster_distance < min_distance)
                    {
                        min_distance = min_point_cluster_distance;
                        min_element_pair.first = (int64_t)idx;
                        min_element_pair.second.first = true;
                        min_element_pair.second.second = min_cluster_index;
                    }
#endif
                }
            }
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            double min_distance = INFINITY;
            std::pair<int64_t, std::pair<bool, int64_t>> min_element_pair(-1, std::pair<bool, int64_t>(false, -1));
            for (size_t idx = 0; idx < num_threads; idx++)
            {
                const double& current_min_distance = per_thread_min_distances[idx];
                const std::pair<int64_t, std::pair<bool, int64_t>>& current_min_element_pair = per_thread_min_element_pairs[idx];
                if (current_min_distance < min_distance)
                {
                    min_distance = current_min_distance;
                    min_element_pair = current_min_element_pair;
                }
            }
#endif
            // Compute the minimum cluster <-> cluster distance
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            std::vector<double> per_thread_min_cluster_cluster_distances(num_threads, INFINITY);
            std::vector<std::pair<int64_t, int64_t>> per_thread_min_cluster_pairs(num_threads, std::make_pair(-1, -1));
            #pragma omp parallel for
#else
            double min_cluster_cluster_distance = INFINITY;
            std::pair<int64_t, int64_t> min_cluster_pair(-1, -1);
#endif
            for (size_t fcdx = 0; fcdx < clusters.size(); fcdx++)
            {
                const std::vector<int64_t>& first_cluster = clusters[fcdx];
                // Don't evaluate empty clusters
                if (first_cluster.size() > 0)
                {
                    for (size_t scdx = 0; scdx < clusters.size(); scdx++)
                    {
                        // Don't compare against ourself
                        if (fcdx != scdx)
                        {
                            const std::vector<int64_t>& second_cluster = clusters[scdx];
                            // Don't evaluate empty clusters
                            if (second_cluster.size() > 0)
                            {
                                // Compute the cluster <-> cluster distance
                                double complete_link_distance = 0.0;
                                double single_link_distance = INFINITY;
                                // Find the maximum-pointwise distance between clusters
                                for (size_t fcpx = 0; fcpx < first_cluster.size(); fcpx++)
                                {
                                    const int64_t& fcp_index = first_cluster[fcpx];
                                    for (size_t scpx = 0; scpx < second_cluster.size(); scpx++)
                                    {
                                        const int64_t& scp_index = second_cluster[scpx];
                                        const double& new_distance = distance_matrix(fcp_index, scp_index);
                                        complete_link_distance = std::max(complete_link_distance, new_distance);
                                        single_link_distance = std::min(single_link_distance, new_distance);
                                    }
                                }
                                const double cluster_cluster_distance = (strategy == COMPLETE_LINK) ? complete_link_distance : single_link_distance;
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
                                const size_t thread_num = (size_t)omp_get_thread_num();
                                double& per_thread_min_cluster_cluster_distance = per_thread_min_cluster_cluster_distances[thread_num];
                                std::pair<int64_t, int64_t>& per_thread_min_cluster_pair = per_thread_min_cluster_pairs[thread_num];
                                if (cluster_cluster_distance < per_thread_min_cluster_cluster_distance)
                                {
                                    per_thread_min_cluster_cluster_distance = cluster_cluster_distance;
                                    per_thread_min_cluster_pair.first = fcdx;
                                    per_thread_min_cluster_pair.second = scdx;
                                }
#else
                                if (cluster_cluster_distance < min_cluster_cluster_distance)
                                {
                                    min_cluster_cluster_distance = cluster_cluster_distance;
                                    min_cluster_pair.first = (int64_t)fcdx;
                                    min_cluster_pair.second = (int64_t)scdx;
                                }
#endif
                            }
                        }
                    }
                }
            }
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            double min_cluster_cluster_distance = INFINITY;
            std::pair<int64_t, int64_t> min_cluster_pair(-1, -1);
            for (size_t idx = 0; idx < num_threads; idx++)
            {
                const double& current_min_cluster_cluster_distance = per_thread_min_cluster_cluster_distances[idx];
                const std::pair<int64_t, int64_t>& current_min_cluster_pair = per_thread_min_cluster_pairs[idx];
                if (current_min_cluster_cluster_distance < min_cluster_cluster_distance)
                {
                    min_cluster_cluster_distance = current_min_cluster_cluster_distance;
                    min_cluster_pair = current_min_cluster_pair;
                }
            }
#endif
            // Return the minimum-distance pair
            if (min_distance < min_cluster_cluster_distance)
            {
                // Set the indices
                const std::pair<bool, int64_t> first_index(false, min_element_pair.first);
                const std::pair<bool, int64_t> second_index = min_element_pair.second;
                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>> indices(first_index, second_index);
                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> minimum_pair(indices, min_distance);
                return minimum_pair;
            }
            // A cluster <-> cluster pair is closest
            else
            {
                // Set the indices
                const std::pair<bool, int64_t> first_index(true, min_cluster_pair.first);
                const std::pair<bool, int64_t> second_index(true, min_cluster_pair.second);
                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>> indices(first_index, second_index);
                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> minimum_pair(indices, min_cluster_cluster_distance);
                return minimum_pair;
            }
        }

    public:

        template<typename Datatype, typename Allocator=std::allocator<Datatype>>
        static std::pair<std::vector<std::vector<Datatype, Allocator>>, double> Cluster(const std::vector<Datatype, Allocator>& data, const std::function<double(const Datatype&, const Datatype&)>& distance_fn, const double max_cluster_distance, const CLUSTER_STRATEGY strategy)
        {
#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
            const Eigen::MatrixXd distance_matrix = arc_helpers::BuildDistanceMatrixParallel(data, distance_fn);
#else
            const Eigen::MatrixXd distance_matrix = arc_helpers::BuildDistanceMatrixSerial(data, distance_fn);
#endif
            return Cluster(data, distance_matrix, max_cluster_distance, strategy);
        }

        template<typename Datatype, typename Allocator=std::allocator<Datatype>>
        static std::pair<std::vector<std::vector<Datatype, Allocator>>, double> Cluster(const std::vector<Datatype, Allocator>& data, const Eigen::MatrixXd& distance_matrix, const double max_cluster_distance, const CLUSTER_STRATEGY strategy)
        {
            assert((size_t)distance_matrix.rows() == data.size());
            assert((size_t)distance_matrix.cols() == data.size());
            std::vector<uint8_t> datapoint_mask(data.size(), 0u);
            std::vector<std::vector<int64_t>> cluster_indices;
            double closest_distance = 0.0;
            bool complete = false;
            while (!complete)
            {
                // Get closest pair of elements (an element can be a cluster or single data value!)
                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> closest_element_pair = GetClosestPair(datapoint_mask, distance_matrix, cluster_indices, strategy);
                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>& closest_elements = closest_element_pair.first;
                closest_distance = closest_element_pair.second;
                //std::cout << "Element pair: " << PrettyPrint::PrettyPrint(closest_element_pair, true) << std::endl;
                if (closest_distance <= max_cluster_distance)
                {
                    const std::pair<bool, int64_t>& first_element = closest_elements.first;
                    const std::pair<bool, int64_t>& second_element = closest_elements.second;
                    // If both elements are points, create a new cluster
                    if ((first_element.first == false) && (second_element.first == false))
                    {
                        //std::cout << "New point-point cluster" << std::endl;
                        const int64_t first_element_index = first_element.second;
                        assert(first_element_index >= 0);
                        const int64_t second_element_index = second_element.second;
                        assert(second_element_index >= 0);
                        // Add a cluster
                        cluster_indices.push_back(std::vector<int64_t>{first_element_index, second_element_index});
                        // Mask out the indices
                        datapoint_mask[(size_t)first_element_index] = 1u;
                        datapoint_mask[(size_t)second_element_index] = 1u;
                    }
                    // If both elements are clusters, merge the clusters
                    else if ((first_element.first == true) && (second_element.first == true))
                    {
                        //std::cout << "Combining clusters" << std::endl;
                        // Get the cluster indices
                        const int64_t first_cluster_index = first_element.second;
                        assert(first_cluster_index >= 0);
                        const int64_t second_cluster_index = second_element.second;
                        assert(second_cluster_index >= 0);
                        // Merge the second cluster into the first
                        std::vector<int64_t>& first_cluster = cluster_indices[(size_t)first_cluster_index];
                        std::vector<int64_t>& second_cluster = cluster_indices[(size_t)second_cluster_index];
                        first_cluster.insert(first_cluster.end(), second_cluster.begin(), second_cluster.end());
                        // Empty the second cluster (we don't remove, because this triggers move)
                        second_cluster.clear();
                    }
                    // If one of the elements is a cluster and the other is a point, add the point to the existing cluster
                    else
                    {
                        //std::cout << "Adding to an existing cluster" << std::endl;
                        int64_t cluster_index = -1;
                        int64_t element_index = -1;
                        if (first_element.first)
                        {
                            cluster_index = first_element.second;
                            element_index = second_element.second;
                        }
                        else if (second_element.first)
                        {
                            cluster_index = second_element.second;
                            element_index = first_element.second;
                        }
                        else
                        {
                            assert(false);
                        }
                        assert(cluster_index >= 0);
                        assert(element_index >= 0);
                        // Add the element to the cluster
                        std::vector<int64_t>& cluster = cluster_indices[(size_t)cluster_index];
                        cluster.push_back(element_index);
                        // Mask out the element index
                        datapoint_mask[(size_t)element_index] = 1u;
                    }
                }
                else
                {
                    complete = true;
                }
            }
            // Extract the actual cluster data
            std::vector<std::vector<Datatype, Allocator>> clusters;
            for (size_t idx = 0; idx < cluster_indices.size(); idx++)
            {
                const std::vector<int64_t>& current_cluster = cluster_indices[idx];
                // Ignore empty clusters
                if (current_cluster.size() > 0)
                {
                    std::vector<Datatype, Allocator> new_cluster;
                    for (size_t cdx = 0; cdx < current_cluster.size(); cdx++)
                    {
                        const int64_t index = current_cluster[cdx];
                        new_cluster.push_back(data[(size_t)index]);
                    }
                    clusters.push_back(new_cluster);
                }
            }
            // Add any points that we haven't clustered into their own clusters
            for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
            {
                // If an element hasn't been clustered at all
                if (datapoint_mask[idx] == 0)
                {
                    clusters.push_back(std::vector<Datatype, Allocator>{data[idx]});
                }
            }
            return std::pair<std::vector<std::vector<Datatype, Allocator>>, double>(clusters, closest_distance);
        }
    };
}
#endif // SIMPLE_HIERARCHICAL_CLUSTERING_HPP


#include"main.hpp"
#include"Option.hpp"
#include"Pre.hpp"
#include"Post.hpp"

#include <boost/math/distributions/hypergeometric.hpp>

void Post::fdr(vector<map<double,fdrs,greater<double> > >& fdr,const int up)
{
    fdr.resize(1?1:d_pr.nl());
    for (unsigned l=0;l<d_pr.nl();l++) {
        map<double,fdrs,greater<double> >& fdr_l(fdr.at(fdr.size()==1?0:l));
        for (unsigned p=0;p<d_pr.np();p++) {
            fdr_l[up==-1 or d_up.at(l).at(p)==up?pr().cp().at(p).at(l):0].t++;
        }
    }
    for (unsigned l=0;l<fdr.size();l++) {
        map<double,fdrs,greater<double> >& fdr_l(fdr.at(l));
        int denom=fdr_l.begin()->second.t;
        double numer=(1-fdr_l.begin()->first)*denom;
        map<double,fdrs>::iterator it=fdr_l.begin();
        for (it++;it!=fdr_l.end();it++) {
            it->second.fdr=numer/denom;
            numer += (1-it->first)*it->second.t;
            denom += it->second.t;
        }
    }
}

void Post::selection(vector<vector<int> >& sel,const vector<map<double,fdrs,greater<double> > >& fdr,const int up)
{
    double minCPS=1;
    for (unsigned l=0;l<d_pr.nl();l++) {
        const map<double,fdrs,greater<double> >& fdr_l(fdr.at(fdr.size()==1?0:l));
        sel.at(l).assign(d_pr.np(),0);
        for (unsigned p=0;p<d_pr.np();p++) {
            if (fdr_l.at(up==-1 or d_up.at(l).at(p)==up?pr().cp().at(p).at(l):0).fdr<d_op.FDRcut()) {
                sel.at(l).at(p)=1;
                if (pr().cp().at(p).at(l)<minCPS) minCPS=pr().cp().at(p).at(l);
            }
        }
    }
}

void Post::test(const vector<vector<int> >& sel,vector<vector<double> >& pval,const int up)
{
    for (unsigned l=0;l<d_pr.nl();l++) {
        const int r=accumulate(sel.at(l).begin(),sel.at(l).end(),0);
        for (unsigned g=0;g<d_pr.ng();g++) {
            int sum=0;
            for (unsigned m=0;m<d_pr.mem().at(g).size();m++) {
                sum+=sel.at(l).at(d_pr.mem().at(g).at(m));
            }
            boost::math::hypergeometric a(r, d_pr.mem().at(g).size(), d_pr.np());
            pval.at(l).at(g)=cdf(complement(a,sum))+pdf(a,sum);
        }
    }
}

Post::Post(const Pre& pr) :
    d_op(pr.op()),d_pr(pr),d_up(pr.nl()),d_selup(pr.nl()),d_seldown(pr.nl()),d_selsig(pr.nl()),d_pvup(pr.nl(),vector<double>(pr.ng())),d_pvdown(pr.nl(),vector<double>(pr.ng())),d_pvsig(pr.nl(),vector<double>(pr.ng()))
{
    if (op().selbool()) {
        d_selup=pr.selup();
    } else {
        for (unsigned l=0;l<d_pr.nl();l++) {
            d_up.at(l).assign(d_pr.np(),0);
            for (unsigned p=0;p<d_pr.np();p++) {
                if (d_pr.R().at(p).at(l)<d_pr.R().at(p).at(l+1)) d_up.at(l).at(p)=1;
            }
        }
        fdr(d_fdrup,1);
        selection(d_selup,d_fdrup,1);
    }
    test(d_selup,d_pvup,1);

    if (op().selbool()) {
        d_seldown=pr.seldown();
    } else {
        fdr(d_fdrdown,0);
        selection(d_seldown,d_fdrdown,0);
    }
    test(d_seldown,d_pvdown,0);

    if (op().selbool()) {
        d_selsig=pr.selsig();
    } else {
        fdr(d_fdrsig,-1);
        selection(d_selsig,d_fdrsig,-1);
    }
    test(d_selsig,d_pvsig,-1);
}


#include "newton_solver.h"

#include "store.h"
#include "acap_solve_energy_gradient.h"
#include "muscle_energy_gradient.h"
#include "stablenh_energy_gradient.h"
#include <igl/polar_dec.h>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <igl/Timer.h>

using Store = famu::Store;
using namespace Eigen;
using namespace std;

double famu::Energy(Store& store, VectorXd& dFvec){
	double EM = famu::muscle::energy(store, dFvec);
	double ENH = famu::stablenh::energy(store, dFvec);
	double EACAP = famu::acap::fastEnergy(store, dFvec);

	return EM + ENH + EACAP;
}

void famu::polar_dec(Store& store, VectorXd& dFvec){
	if(store.jinput["polar_dec"]){
		//project bones dF back to rotations
		if(store.jinput["reduced"]){
			for(int b =0; b < store.bone_tets.size(); b++){
				Eigen::Matrix3d _r, _t;
				Matrix3d dFb = Map<Matrix3d>(dFvec.segment<9>(9*b).data()).transpose();
				igl::polar_dec(dFb, _r, _t);

				dFvec[9*b+0] = _r(0,0);
	      		dFvec[9*b+1] = _r(0,1);
	      		dFvec[9*b+2] = _r(0,2);
	      		dFvec[9*b+3] = _r(1,0);
	      		dFvec[9*b+4] = _r(1,1);
	      		dFvec[9*b+5] = _r(1,2);
	      		dFvec[9*b+6] = _r(2,0);
	      		dFvec[9*b+7] = _r(2,1);
	      		dFvec[9*b+8] = _r(2,2);

			}

		}else{
			for(int t = 0; t < store.bone_tets.size(); t++){
				for(int i=0; i<store.bone_tets[t].size(); i++){
					int b =store.bone_tets[t][i];

					Eigen::Matrix3d _r, _t;
					Matrix3d dFb = Map<Matrix3d>(dFvec.segment<9>(9*b).data()).transpose();
					igl::polar_dec(dFb, _r, _t);

					dFvec[9*b+0] = _r(0,0);
		      		dFvec[9*b+1] = _r(0,1);
		      		dFvec[9*b+2] = _r(0,2);
		      		dFvec[9*b+3] = _r(1,0);
		      		dFvec[9*b+4] = _r(1,1);
		      		dFvec[9*b+5] = _r(1,2);
		      		dFvec[9*b+6] = _r(2,0);
		      		dFvec[9*b+7] = _r(2,1);
		      		dFvec[9*b+8] = _r(2,2);
				}
			}
		}
	}
}

double famu::line_search(int& tot_ls_its, Store& store, VectorXd& grad, VectorXd& drt){
	// Decreasing and increasing factors
	VectorXd x = store.dFvec;
	VectorXd xp = x;
	double step = 50;
    const double dec = 0.5;
    const double inc = 2.1;
    int pmax_linesearch = 100;
    int plinesearch = 1;//1 for armijo, 2 for wolfe
    double pftol = 1e-4;
    double pwolfe = 0.9;
    double pmax_step = 1e8;
    double pmin_step = 1e-20;

    // Check the value of step
    if(step <= double(0))
        std::invalid_argument("'step' must be positive");

    polar_dec(store, x);
    famu::acap::solve(store, x);
   	double fx = Energy(store, x);
    // Save the function value at the current x
    const double fx_init = fx;
    // Projection of gradient on the search direction
    const double dg_init = grad.dot(drt);
    // Make sure d points to a descent direction
    if(dg_init > 0)
        std::logic_error("the moving direction increases the objective function value");

    const double dg_test = pftol * dg_init;
    double width;

    int iter;
    for(iter = 0; iter < pmax_linesearch; iter++)
    {
        // x_{k+1} = x_k + step * d_k
        x.noalias() = xp + step * drt;
        polar_dec(store, x);

        // Evaluate this candidate
        famu::acap::solve(store, x);
       	fx = Energy(store, x);

        if(fx > fx_init + step * dg_test)
        {
            width = dec;
        } else {
            // Armijo condition is met
            if(plinesearch == 1)
                break;

            const double dg = grad.dot(drt);
            if(dg < pwolfe * dg_init)
            {
                width = inc;
            } else {
                // Regular Wolfe condition is met
                if(plinesearch == 2)
                    break;

                if(dg > -pwolfe * dg_init)
                {
                    width = dec;
                } else {
                    // Strong Wolfe condition is met
                    break;
                }
            }
        }

        if(iter >= pmax_linesearch)
            throw std::runtime_error("the line search routine reached the maximum number of iterations");

        if(step < pmin_step)
            throw std::runtime_error("the line search step became smaller than the minimum value allowed");

        if(step > pmax_step)
            throw std::runtime_error("the line search step became larger than the maximum value allowed");

        step *= width;
    }
    // cout<<"			ls iters: "<<iter<<endl;
    // cout<<"			step: "<<step<<endl;
    tot_ls_its += iter;
    return step;
}

void famu::sparse_to_dense(const Store& store, SparseMatrix<double, Eigen::RowMajor>& H, MatrixXd& denseHess){
	//Fill denseHess with 9x9 block diags from H
	//TODO: this should be done in the hessians code. coeffRef is expensive
	//FIX AFTER THE DEADLINE

	#pragma omp parallel for
	for(int i=0; i<store.dFvec.size()/9; i++){
		//loop through 9x9 block and fill denseH
		Matrix9d A;
		#pragma omp parallel for collapse(2)
		for(int j =0; j<9; j++){
			for(int k=0; k<9; k++){
				A(j, k) = H.coeffRef(9*i + j, 9*i +k);
			}
		}
		denseHess.block<9,9>(9*i, 0) = A;
	}
}

void famu::fastWoodbury(Store& store, const VectorXd& g, MatrixModesxModes X, VectorXd& BInvXDy, MatrixXd& denseHess, VectorXd& drt){
	//Woodbury parallel approach 1 (with reduction)

	Matrix<double, NUM_MODES, 1> DAg = Matrix<double, NUM_MODES, 1>::Zero();
	Matrix<double, NUM_MODES, 1> InvXDAg;
	FullPivLU<MatrixModesxModes> WoodburyDenseSolve;

	double aa = store.jinput["alpha_arap"];
	X = store.InvC*aa;
	#pragma omp parallel
	{
		MatrixModesxModes Xpriv = MatrixModesxModes::Zero();
		Matrix<double, NUM_MODES, 1> DAgpriv = Matrix<double, NUM_MODES, 1>::Zero();

		#pragma omp for
		for(int i=0; i<store.dFvec.size()/9; i++){
			Matrix9d A = denseHess.block<9,9>(9*i, 0);
			LDLT<Matrix9d> InvA;
			InvA.compute(A);
			store.vecInvA[i] = InvA;

			Vector9d invAg = InvA.solve(g.segment<9>(9*i));
			drt.segment<9>(9*i) = invAg;

			Matrix9xModes B = store.WoodB.block<9, NUM_MODES>(9*i, 0);
			Xpriv = Xpriv + -B.transpose()*InvA.solve(B);

			DAgpriv  = DAgpriv + -B.transpose()*invAg;
		}
		#pragma omp critical
		{
			X += Xpriv;
			DAg += DAgpriv;
		}
	}

	#pragma omp single
	{

		WoodburyDenseSolve.compute(X);
		InvXDAg = WoodburyDenseSolve.solve(DAg);

	}

	#pragma omp parallel for
	for(int i=0; i<store.dFvec.size()/9; i++){
		Matrix9xModes B = store.WoodB.block<9, NUM_MODES>(9*i, 0);

		Vector9d InvAtemp1 = store.vecInvA[i].solve(B*InvXDAg);
		drt.segment<9>(9*i) -=  InvAtemp1;
	}

	drt *= -1;
}

int famu::newton_static_solve(Store& store){
	int MAX_ITERS = store.jinput["NM_MAX_ITERS"];
	VectorXd muscle_grad, neo_grad, acap_grad;
	muscle_grad.resize(store.dFvec.size());
	neo_grad.resize(store.dFvec.size());
	acap_grad.resize(store.dFvec.size());

	SparseMatrix<double, Eigen::RowMajor> constHess(store.dFvec.size(), store.dFvec.size());
	constHess.setZero();

	constHess = store.neoHess + store.muscleHess + store.acapHess;// + store.ContactHess;
	constHess -= store.neoHess;

	MatrixXd denseHess = MatrixXd::Zero(store.dFvec.size(),  9);
	MatrixXd constDenseHess = MatrixXd::Zero(store.dFvec.size(),  9);
	sparse_to_dense(store, constHess, constDenseHess);

	VectorXd delta_dFvec = VectorXd::Zero(store.dFvec.size());
	VectorXd test_drt = delta_dFvec;
	VectorXd grad_dofs = VectorXd::Zero(store.dFvec.size());

	VectorXd BInvXDy = VectorXd::Zero(store.dFvec.size());
	MatrixModesxModes X;

	igl::Timer timer, timer1;
	double woodtimes =0;
	double linetimes =0;
	int tot_ls_its = 0;
	int iter =1;
	timer1.start();
	for(iter=1; iter<MAX_ITERS; iter++){
		grad_dofs.setZero();
		double prevfx = Energy(store, store.dFvec);

		famu::acap::solve(store, store.dFvec);
		famu::muscle::gradient(store, muscle_grad);
		famu::stablenh::gradient(store, neo_grad);
		famu::acap::fastGradient(store, acap_grad);
		grad_dofs = muscle_grad + neo_grad + acap_grad;

		// cout<<"		muscle grad: "<<muscle_grad.norm()<<endl;
		// cout<<"		neo grad: "<<neo_grad.norm()<<endl;
		// cout<<"		acap grad: "<<acap_grad.norm()<<endl;
		// cout<<"		total grad: "<<grad_dofs.norm()<<endl;

		if(grad_dofs != grad_dofs){
			cout<<"Error: nans in grad"<<endl;
			exit(0);
		}

		famu::stablenh::hessian(store, store.neoHess, store.denseNeoHess, store.jinput["woodbury"]);

		// if(!store.jinput["woodbury"]){

		// SparseMatrix<double, Eigen::RowMajor> hessFvec = store.neoHess + constHess;
		// store.NM_SPLU.factorize(hessFvec);
		// if(store.NM_SPLU.info()!=Success){
		// 	cout<<"SOLVER FAILED"<<endl;
		// 	cout<<store.NM_SPLU.info()<<endl;
		// }
		// delta_dFvec = -1*store.NM_SPLU.solve(grad_dofs);

		// }else{

			// //Sparse Woodbury code
			// hessFvec.setZero();
			// hessFvec = store.neoHess + constHess;
			// store.NM_SPLU.factorize(hessFvec);
			// if(store.NM_SPLU.info()!=Success){
			// 	cout<<"SOLVER FAILED"<<endl;
			// 	cout<<store.NM_SPLU.info()<<endl;
			// }
			// VectorXd InvAg = store.NM_SPLU.solve(grad_dofs);
			// MatrixXd CDAB = store.InvC + store.WoodD*store.NM_SPLU.solve(store.WoodB);
			// FullPivLU<MatrixXd>  WoodburyDenseSolve;
			// WoodburyDenseSolve.compute(CDAB);
			// VectorXd temp1 = store.WoodB*WoodburyDenseSolve.solve(store.WoodD*InvAg);;

			// VectorXd InvAtemp1 = store.NM_SPLU.solve(temp1);
			// test_drt =  -InvAg + InvAtemp1;

			//Dense Woodbury code
			denseHess = constDenseHess + store.denseNeoHess;
			timer.start();
			fastWoodbury(store, grad_dofs, X, BInvXDy, denseHess, delta_dFvec);
			timer.stop();
			woodtimes += timer.getElapsedTimeInMicroSec();
			// cout<<"		woodbury diff: "<<(delta_dFvec - test_drt).norm()<<endl;

		// }

		if(delta_dFvec != delta_dFvec){
			cout<<"Error: nans"<<endl;
			exit(0);
		}

		//line search
		double alpha = 0.1;
		timer.start();
		alpha = line_search(tot_ls_its, store, grad_dofs, delta_dFvec);
		timer.stop();
		linetimes += timer.getElapsedTimeInMicroSec();

		if(fabs(alpha)<1e-9 ){
			break;
		}

		store.dFvec += alpha*delta_dFvec;
		polar_dec(store, store.dFvec);
		double fx = Energy(store, store.dFvec);

		if(grad_dofs.squaredNorm()/grad_dofs.size()<1e-4 || fabs(fx - prevfx)<1e-3){
			break;
		}
	}
	timer1.stop();
	double nmtime = timer1.getElapsedTimeInMicroSec();

	// timer1.start();
	// double acap_energy = famu::acap::fastEnergy(store, store.dFvec);
	// timer1.stop();
	// double energy_time = timer1.getElapsedTimeInMicroSec();

	// timer1.start();
	// famu::acap::solve(store, store.dFvec);
	// timer1.stop();

	cout<<"-----------QS STEP INFO----------"<<endl;
	cout<<"V, T:"<<store.V.rows()<<", "<<store.T.rows()<<endl;
	cout<<"Threads: "<<Eigen::nbThreads()<<endl;
	cout<<"NM Iters: "<<iter<<endl;
	cout<<"Total NM time: "<<nmtime<<endl;
	// cout<<"Total Hess time: "<<woodtimes<<endl;
	// cout<<"Total LS time: "<<linetimes<<endl;
	// cout<<"LS iters: "<<tot_ls_its<<endl;
	// cout<<"Energy: "<<acap_energy<<endl;
	// cout<<"Energy Time: "<<energy_time<<endl;
	// cout<<"ACAP time: "<<timer1.getElapsedTimeInMicroSec()<<endl;
	// cout<<"dFvec: "<<store.dFvec.transpose()<<endl;
	cout<<"--------------------------------"<<endl;
    return iter;
}


/*====================================================================
   Simple pendulum example
   Copyright (c) 2017 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
   Licensed under the zlib license. See LICENSE for more details.
 *///=================================================================

#include <string>
#include <iostream>
#include <stdio.h>
#include <rbdl/rbdl.h>
#include "csvtools.h"

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
//using namespace std;
using namespace boost::numeric::odeint;

#ifndef RBDL_BUILD_ADDON_LUAMODEL
    #error "Error: RBDL addon LuaModel not enabled."
#endif

#include <rbdl/addons/luamodel/luamodel.h>

using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

//====================================================================
// Boost stuff
//====================================================================

typedef std::vector< double > state_type;

typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

class rbdlToBoost {

    public:
        rbdlToBoost(Model* model) : model(model) {
            q = VectorNd::Zero(model->dof_count);
            qd = VectorNd::Zero(model->dof_count);
            qdd = VectorNd::Zero(model->dof_count);
            tau = VectorNd::Zero(model->dof_count);

        }

        //3c. Boost uses this 'operator()' function to evaluate the state
        //    derivative of the pendulum.
        void operator() (const state_type &x,
                         state_type &dxdt,
                         const double t){

            //3d. Here we split out q (generalized positions) and qd
            //    (generalized velocities) from the x (state vector)
            //q
            int j = 0;
            for(int i=0; i<model->dof_count; i++){
                q[i] = (double)x[j];
                j++;
            }

            //qd
            for(int i=0; i<model->dof_count; i++){
                qd[i] = (double)x[j];
                j++;
            }

            //3e. Here we set the applied generalized forces to zero
            for(int i=0; i<model->dof_count; i++){
                tau[i] = 0;
            }

            //3f. RBDL's ForwardDynamics function is used to evaluate
            //    qdd (generalized accelerations)
            ForwardDynamics (*model,q,qd,tau,qdd);

            //3g. Here qd, and qdd are used to populate dxdt
            //(the state derivative)
            j = 0;
            for(int i = 0; i < model->dof_count; i++){
                dxdt[j] = (double)qd[i];
                j++;
            }
            for(int i = 0; i < model->dof_count; i++){
                dxdt[j] = (double)qdd[i];
                j++;
            }

        }

    private:
        Model* model;
        VectorNd q, qd, qdd, tau;
};

struct pushBackStateAndTime
{
    std::vector< state_type >& states;
    std::vector< double >& times;

    pushBackStateAndTime( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : states( states ) , times( times ) { }

    void operator()( const state_type &x , double t )
    {
        states.push_back( x );
        times.push_back( t );
    }
};

void f(const state_type &x, state_type &dxdt, const double t);

/* Problem Constants */
int main (int argc, char* argv[]) {
    rbdl_check_api_version (RBDL_API_VERSION);

    //problem specific constants
    int     nPts    = 100;
    double  t0      = 0;
    double  t1      = 3;

    //Integration settings
    double absTolVal   = 1e-10;
    double relTolVal   = 1e-6;

    VectorNd q, qd;

    Model* model  = NULL;
    model         = new Model();

    //3a. The Lua model is read in here, and turned into a series of
    //    vectors and matricies in model which RBDL uses to evaluate
    //    dynamics quantities
    if (!Addons::LuaModelReadFromFile ("./../model/pendulum.lua",
                                       model, false)             ){
        std::cerr     << "Error loading model ./model/pendulum.lua"
                    << std::endl;
        abort();
    }

    q       = VectorNd::Zero (model->dof_count);
    qd      = VectorNd::Zero (model->dof_count);

    double t        = 0;             //time
    double ts       = 0;            //scaled time
    double dtsdt    = M_PI/(t1-t0);    //dertivative scaled time
                                    //w.r.t. time

    printf("DoF: %i\n",model->dof_count);

    printf("Forward Dynamics \n");

    //3b. Here we instantiate a wrapper class which is needed so that
    //    Boost can evaluate the state derivative of the model.
    rbdlToBoost rbdlModel(model);
    state_type xState(2);
    int steps = 0;
    xState[0] = -M_PI/4.0;
    xState[1] = 0;

    double dt   = (t1-t0)/((double)nPts);

    double ke, pe = 0;

    std::vector<std::vector< double > > matrixData;
    std::vector<std::vector< double > > matrixErrorData;
    std::vector< double > rowData(model->dof_count+1);
    std::vector< double > rowErrorData(2);

    double a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type
    controlled_stepper(
        default_error_checker< double ,
                               range_algebra ,
                               default_operations >
        ( absTolVal , relTolVal , a_x , a_dxdt ) );

    double tp = 0;
    rowData[0] = 0;
    for(int z=0; z < model->dof_count; z++){
        rowData[z+1] = xState[model->dof_count + z];
    }
    matrixData.push_back(rowData);

    double kepe0 = 0;

    printf("Columns\n");
    printf("      t,         q,       qd,       ke,        pe,   ke+pe-(kepe0)\n");
    for(int i = 0; i <= nPts; i++){

        t = t0 + dt*i;

        //3h. Here we integrate forward in time between a series of nPts from
        //    t0 to t1
        integrate_adaptive(
            controlled_stepper ,
            rbdlModel , xState , tp , t , (t-tp)/10 );
        tp = t;

        //3i. At each point the state, kinetic (ke), and potential energy (pe)
        //    is evaluated. In this conservative system the sum of kinetic and
        //    potential energy should be constant. Any error that accumulates
        //    is due to the cumulation of integration error.

        q[0]  = xState[0];
        qd[0] = xState[1];

        pe = Utils::CalcPotentialEnergy(*model, q, true);
        ke = Utils::CalcKineticEnergy(*model, q, qd, true);

        printf("%f, %f, %f, %f, %f, %f\n",
                    t, q[0],qd[0],ke,
                    pe,(ke+pe-kepe0));

        rowData[0] = t;
        for(int z=0; z < model->dof_count; z++){
            rowData[z+1] = xState[z];
        }
        matrixData.push_back(rowData);

        if(i==0) kepe0 = (ke+pe);

        rowErrorData[0] = t;
        rowErrorData[1] = (ke + pe) - kepe0;
        matrixErrorData.push_back(rowErrorData);
    }
    printf("Columns\n");
    printf("      t,         q,       qd,       ke,        pe,      ke+pe-(kepe0)\n");

    //3j. Now the data we have accumulated is written to file
    std::string header = "";
    std::string fname   = "../output/meshup.csv";
    printMatrixToFile(matrixData, header, fname);
    printf("Wrote: ../output/meshup.csv (meshup animation file)\n");

    fname               = "../output/kepe.csv";
    header = "time,systemEnergy,";
    printMatrixToFile(matrixErrorData,header,fname);
    printf("Wrote: ../output/kepe.csv (simulation data)\n");
    delete model;

    return 0;

}


#include "gdrawer.hpp"

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#if BOOST_VERSION >= 106000
#include <boost/phoenix/object/new.hpp>
#else
#include <boost/spirit/home/phoenix/object/new.hpp>
#endif
#include <stdexcept>
#include <QDebug>

using namespace boost;
using namespace spirit;
using namespace ascii;
using namespace phoenix;

template<class Iterator>
struct ExprGrammar : qi::grammar<Iterator, expr_t*(), ascii::space_type>
{
	ExprGrammar(): ExprGrammar::base_type(expr, "Expression")
	{
		primitive.name("Primitive");
		factor.name("Factor");
		term.name("Term");
		expr.name("Expression");

		primitive
			= ('(' >> expr >> ')')[_val = _1]
			| ('|' >> expr >> '|')[_val = new_<unop_t>('|', _1)]
			| (char_('a', 'z') | char_('A', 'Z'))[_val = new_<var_t>(_1)]
			| real[_val = new_<const_t>(_1)];

		primitive2
			= primitive[_val = _1]
			| ('-' >> primitive)[_val = new_<unop_t>('-', _1)];

		factor
			= (primitive2 >> '^' >> factor)[_val = new_<binop_t>('^', _1, _2)]
			| primitive2[_val = _1];

		term
			= factor[_val = _1] >>
				*( (char_("/*") >> factor)[_val = new_<binop_t>(_1, _val, _2)]
				| !char_("-+") >> factor[_val = new_<binop_t>('*', _val, _1)] );

		expr
			= term[_val = _1] >> *(char_("+-") >> term)[_val = new_<binop_t>(_1, _val, _2)];

		qi::on_error<qi::fail>
		(
			expr,
			std::cout
                << val("Error! Expecting ")
                << _4                               // what failed?
                << val(" here: \"")
                << construct<std::string>(_3, _2)   // iterators to error-pos, end
                << val("\"")
                << std::endl
		);
		/* qi::debug(expr); qi::debug(term); qi::debug(factor); qi::debug(primitive); qi::debug(primitive2); */
	}

	qi::rule<Iterator, expr_t*(), ascii::space_type> primitive, primitive2, factor, term, expr;
	qi::real_parser<real_t> real;
};

Vm *MathVm::get(const QString& expr)
{
	expr_t* tree = NULL;
	std::string s = expr.toStdString();
	ExprGrammar<std::string::const_iterator> g;
	std::string::const_iterator begin = s.begin(), end = s.end();
	bool r = phrase_parse(begin, end, g, space, tree);
	if (!r || begin != end || !tree)
	{
//		qDebug() << "end - begin = " << int(end - begin) << " tree " << (long long)tree << std::endl;
		if (tree) delete tree;
		throw Exception("Syntax error");
	}

	MathVm *ret = new MathVm;
	tree->addInstr(ret);
	ret->requiredStackSize = tree->getDepth();
	delete tree;
	return ret;
}