#include <boost/interprocess/managed_shared_memory.hpp>
#include <fmt/core.h>

#include <random>

using namespace boost::interprocess;

int main()
{
    std::random_device dev;
    std::mt19937 rng(dev());
    auto dist100 = std::uniform_int_distribution<std::mt19937::result_type>(0, 100);
    int random_int = dist100(rng);

    shared_memory_object::remove("Boost");
    auto managed_shm = managed_shared_memory { open_or_create, "Boost", 1024 };

    int* i = managed_shm.construct<int>("Integer")(random_int);
    fmt::print("random integer in shm set to {}\n", *i);
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::common::factory.hpp                 	 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_COMMON_FACTORY2_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_COMMON_FACTORY2_HPP_ER_2009
#include <boost/statistics/detail/distribution/survival/response/common/meta_factory.hpp>
#include <boost/statistics/detail/distribution/model/tag/response.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/make_map.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace response {

	// Returns a fusion map with tag Tag and data type Y
	template<typename Y,typename Tag = distribution::model::tag::response>
	class factory2{
    	typedef typename meta::factory<Y>::type factory_;
        typedef	boost::fusion::pair<Tag,Y> pair_;

        public:
		typedef Tag tag;
        typedef boost::fusion::map<pair_> result_type;

		factory2(){}
        template<typename Arg>
        factory2(const Arg& arg):f(arg){}
        factory2(const factory2& that):f(that.f){}

        template<typename M>
        result_type operator()(const M& record)const{
        	return boost::fusion::make_map<Tag>(
    			f(record)
            );
        }

    	private:
    	mutable factory_ f;

    };

}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#ifndef SYSTEM_HPP
#define SYSTEM_HPP

#include <armadillo>

class System {
public:
    int nX;
    int nU;
    System(int _nX, int _nU) : nX(_nX), nU(_nU) {;}
    virtual ~System() {;}
    virtual arma::vec f(const arma::vec&, const arma::vec&) = 0;
    virtual arma::mat fdx(const arma::vec&, const arma::vec&) = 0;
    virtual arma::mat fdu(const arma::vec&, const arma::vec&) = 0;
};

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C)  2017 Cord Harms

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/termstructures/localcorrtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    LocalCorrTermStructure::LocalCorrTermStructure(const std::vector<boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>>& processes,
												   const boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>&								   processToCal)
    : CorrelationTermStructureStrike(processes[0]->blackVolatility()->referenceDate(),
		processes[0]->blackVolatility()->calendar(),
		processes[0]->blackVolatility()->businessDayConvention(),
		processes[0]->blackVolatility()->dayCounter()), processToCal_(processToCal){

		processes_ = std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>>(processes.size());

		for (size_t i = 0; i < processes_.size(); i++)
		{
			processes_[i] = boost::shared_ptr<QuantLib::HestonSLVProcess >( new QuantLib::HestonSLVProcess(
				boost::shared_ptr<QuantLib::HestonProcess >(new QuantLib::HestonProcess(
						processes[i]->riskFreeRate(), processes[i]->dividendYield(),
						Handle<Quote>(boost::make_shared<SimpleQuote>(processes[i]->x0())),1.0,
						0.0,0.0,0.0,0.0))
				, boost::shared_ptr<LocalVolTermStructure>(processes[i]->localVolatility().currentLink())));
		}

	}

	LocalCorrTermStructure::LocalCorrTermStructure(const std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>>& processes,
		const boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>&			   processToCal)
		: CorrelationTermStructureStrike(processes[0]->leverageFct()->referenceDate(),
			processes[0]->leverageFct()->calendar(),
			processes[0]->leverageFct()->businessDayConvention(),
			processes[0]->leverageFct()->dayCounter()), processToCal_(processToCal), processes_(processes) {}

    void LocalCorrTermStructure::localCorr(RealStochasticProcess::MatA& corrMatrix,
											   const Date& d,
											   const RealStochasticProcess::VecA& X0,
                                               bool extrapolate) {

		for (size_t i = 0; i < X0.size(); i++)
		{
			checkRange(d, extrapolate);
			checkStrike(X0[i], i, extrapolate);
		}
        Time t = timeFromReference(d);

		localCorrImpl(corrMatrix, t, X0,extrapolate);
    }

    void LocalCorrTermStructure::localCorr(RealStochasticProcess::MatA& corrMatrix,
											   Time t,
											   const RealStochasticProcess::VecA& X0,
                                               bool extrapolate) {
		checkRange(t, extrapolate);
		for (size_t i = 0; i < X0.size(); i++)
		{
			checkStrike(X0[i], i, extrapolate);
		}
        localCorrImpl(corrMatrix, t, X0, extrapolate);

		//cap to 1 and floor to -1:
		for (size_t i = 0; i < corrMatrix.size(); i++)
		{
			for (size_t j = i+1; j < corrMatrix.size(); j++)
			{
				if (corrMatrix[i][j] > 1 || corrMatrix[i][j] < -1) {
					QL_FAIL("Correlation values have to be checked and corrected by checkLambdaValue function.");
				}
			}
		}
    }

    void LocalCorrTermStructure::accept(AcyclicVisitor& v) {
        Visitor<LocalCorrTermStructure>* v1 =
            dynamic_cast<Visitor<LocalCorrTermStructure>*>(&v);
        if (v1 != 0)
            v1->visit(*this);
        else
            QL_FAIL("not a local-Correlation term structure visitor");
    }

	const Date& LocalCorrTermStructure::referenceDate() const {
		return processes_[0]->leverageFct()->referenceDate();
	}

	DayCounter LocalCorrTermStructure::dayCounter() const {
		return processes_[0]->leverageFct()->dayCounter();
	}

	Date LocalCorrTermStructure::maxDate() const {
		Date minMaxDate = processes_[0]->leverageFct()->maxDate();
		for (size_t i = 0; i < processes_.size(); i++)
		{
			if (minMaxDate < processes_[i]->leverageFct()->maxDate()) minMaxDate = processes_[i]->leverageFct()->maxDate();
		}
		return minMaxDate;
	}

	Real LocalCorrTermStructure::minStrike(Natural ulId) const {
		return processes_[ulId]->leverageFct()->minStrike();
	}

	Real LocalCorrTermStructure::maxStrike(Natural ulId) const {
		return processes_[ulId]->leverageFct()->maxStrike();
	}

}


//
// Copyright (c) 2010 Thomas Klimpel
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <iostream>
#include <vector>
#include <boost/numeric/bindings/detail/complex_utils.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

int test_inshuffle(std::size_t n) {
  std::vector<std::size_t> data(2*n);
  for (std::size_t i = 0; i < n; ++i) {
    data[i] = 2*i+1;
    data[i+n] = 2*i;
  }
  boost::numeric::bindings::detail::inshuffle(&data[0],n);
  for (std::size_t i = 1; i < 2*n; ++i)
    if (data[i-1]+1 != data[i]) {
      std::cout << "Test inshuffle for n=" << n << std::endl;
      for (std::size_t j = 0; j < 2*n; ++j)
        std::cout << " " << data[j];
      std::cout << std::endl;
      std::cout << "logic error" << std::endl;
      return 255;
    }
  return 0;
}

int test_interlace(std::size_t n) {
  std::vector<std::complex<double> > data(n);
  for (std::size_t i = 0; i < n; ++i)
    data[i] = std::complex<double>(2*i,2*i+1);
  boost::numeric::bindings::detail::interlace(data);
  for (std::size_t i = 0; i < n; ++i)
    if (data[i].real() != i || data[i].imag() != i+n) {
      std::cout << "Test interlace for n=" << n << std::endl;
      for (std::size_t j = 0; j < n; ++j)
        std::cout << " " << data[j];
      std::cout << std::endl;
      std::cout << "logic error" << std::endl;
      return 255;
    }
  return 0;
}

int main() {
  for (std::size_t n = 1; n <= 1000; ++n) {
    int success = test_inshuffle(n);
    if (success != 0)
      return success;
  }
  for (std::size_t n = 1; n <= 1000; ++n) {
    int success = test_interlace(n);
    if (success != 0)
      return success;
  }
  return 0;
}


// TestVariant101.cpp
//
// Show the essentials of the Variant Library. There is type-safe
// visitation of variants (outside current scope).
//
// (C) Datasim Education BV 2009-2011
//

#include <iostream>
#include <string>
using namespace std;

#include <boost/variant.hpp>
#include "Point.hpp"

int main()
{
     // Using declaration, for readability purposes
     using boost::variant;

	 // Equivalent of C union
	 typedef variant<int, float, double> NumericDataType;

	 NumericDataType myType = 3.142;
	 cout << myType << endl;
	 myType = 1.98f;
	 cout << myType << endl;
	 myType = static_cast<double>(3.14);
	 cout << myType << endl;

	 // Gives compiler error
	// myType = string("dddd");

	 // Some ways to construct a variant
     variant<long, string, Point> myVariant; // default-constructed long
	 variant<long, string, Point> myVariant2(myVariant);

	 Point pt(1.0, 2.0);
	 variant<long, string, Point> myVariant3(pt);

	 long val = 100;
	 variant<long, string, Point> myVariant4(val);

	 variant<long, string, Point> myVariant5 = myVariant4;

	 variant<long, string, Point> myVariant6 = val;

	 // Give some values
     myVariant = 24;

     myVariant = string("It's amazing");
     myVariant = Point(3.0, 4.0);

	 // Try to get the value out of the variant
	 Point ptA;
	 string stA;
	 try
	 {
		 ptA = boost::get<Point>(myVariant);
	//	 stA = boost::get<string>(myVariant); // won't work, wrong type
	 }
	 catch (boost::bad_get& err)
	 {
		cout << "Error: " << err.what() << endl;
	 }

	 cout << "Value got from Variant: " << ptA << endl;

     return 0;
}

Level_8/V. An Introduction to the Boost C++ Libraries/03b - Variant/TestVariant101.cpp


/* Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Ross Linscott (rossklin@gmail.com)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *     Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in
 *     the documentation and/or other materials provided with the
 *     distribution.
 *
 *     The names of its contributors may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#if defined(NDEBUG)
#undef NDEBUG
#endif

#include <cstdlib>
#include <cassert>
#include <cmath>
#include <cstring>

#include <vector>
#include <utility>
#include <exception>

#include <boost/numeric/odeint.hpp>

#include <Rcpp.h>

using std::vector;

using Rcpp::NumericMatrix;
using Rcpp::NumericVector;
using Rcpp::CharacterVector;
using Rcpp::XPtr;
using Rcpp::wrap;
using Rcpp::as;

using namespace boost::numeric::odeint;
using namespace boost::math;
using namespace std;

/*
  Thanks
  http://www.boost.org/doc/libs/1_55_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.implicit_solvers
  for example code on an sde
*/

template< class T > class stochastic_euler
{
public:

    typedef T state_type;
    typedef T deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;
    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};

struct r_compute{
  Rcpp::Function f;

  r_compute(Rcpp::Function g) : f(g){}

  void operator()(vector<double> &q, vector<double> &out){
    out = as<vector<double> >(f(q));
  }

};

//' Simulates an SDE explicitly (derivative free)
//'
//' For an Ito form SDE dx = f(x) dt + g(x) E sqrt(dt)
//'
//' @param d_det R function representing f(x)
//' @param d_stoch R function representing g(x) E
//' @param start numeric vector with initial state
//' @param from scalar with initial time
//' @param to scalar with final time
//' @param steps number of steps to take
//' @export
// [[Rcpp::export]]
NumericMatrix solve_general_sde(Rcpp::Function d_det
			   , Rcpp::Function d_stoch
			   , vector<double> start
			   , double from, double to, int steps ) {
  stochastic_euler< vector<double> > se;
  const double dt = (to - from)/steps;
  r_compute sd = r_compute(d_det);
  r_compute ss = r_compute(d_stoch);

  vector<double> state(start);

  NumericMatrix result(steps+1, start.size());

  for(int j = 0; j < start.size(); ++j)
    result(0, j) = state[j];

  for(int i = 1; i <= steps; ++i) {
    se.do_step(std::make_pair(sd, ss), state, i*dt, dt);
    for(int j = 0; j < start.size(); ++j)
      result(i, j) = state[j];
  }

  return result;
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SYSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SYSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include "boost/numeric/bindings/traits/ublas_symmetric.hpp"
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B with A symmetric matrix
    //
    /////////////////////////////////////////////////////////////////////

    namespace detail {

      inline
      integer_t sytrf_block (float, integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "SSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (double, integer_t const ispec, char const ul, integer_t const n) {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "DSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (traits::complex_f,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "CSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (traits::complex_d,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "ZSYTRF", ul2, n);
      }
    }

    template <typename SymmA>
    integer_t sytrf_block (char const q, char const ul, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::sytrf_block (val_t(), ispec, ul, n);
    }

    template <typename SymmA>
    integer_t sytrf_block (char const q, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::sytrf_block (val_t(), ispec, ul, n);
    }

    template <typename SymmA>
    integer_t sytrf_work (char const q, char const ul, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::sytrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename SymmA>
    integer_t sytrf_work (char const q, SymmA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::sytrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename SymmA>
    inline
    integer_t sysv_work (char const q, char const ul, SymmA const& a) {
      return sytrf_work (q, ul, a);
    }

    template <typename SymmA>
    inline
    integer_t sysv_work (char const q, SymmA const& a) { return sytrf_work (q, a); }

    /*
     * sysv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric matrix and X and B
     * are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^T,  if UPLO = 'U',
     *   A = L * D * L^T,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is symmetric and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks.
     * The factored form of A is then used to solve the system
     * of equations A * X = B.
     */

    namespace detail
    {

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 float* a, integer_t const lda, integer_t* ipiv,
                 float* b, integer_t const ldb,
                 float* w, integer_t const lw, integer_t* info)
      {
        LAPACK_SSYSV (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, w, &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 double* a, integer_t const lda, integer_t* ipiv,
                 double* b, integer_t const ldb,
                 double* w, integer_t const lw, integer_t* info)
      {
        LAPACK_DSYSV (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, w, &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_f* b, integer_t const ldb,
                 traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CSYSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_d* b, integer_t const ldb,
                 traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZSYSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      template <typename SymmA, typename MatrB, typename IVec, typename Work>
      int sysv (char const ul, SymmA& a, IVec& i, MatrB& b, Work& w)
      {
        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        sysv (ul, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              traits::vector_storage (w),
              traits::vector_size (w),
              &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec, typename Work>
    inline
    int sysv (char const ul, SymmA& a, IVec& i, MatrB& b, Work& w)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::sysv (ul, a, i, b, w);
    }

    template <typename SymmA, typename MatrB, typename IVec, typename Work>
    inline
    int sysv (SymmA& a, IVec& i, MatrB& b, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::sysv (uplo, a, i, b, w);
    }

    template <typename SymmA, typename MatrB>
    int sysv (char const ul, SymmA& a, MatrB& b)
    {
      // with 'internal' pivot and work vectors

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = sytrf_work ('O', ul, a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
        typedef typename SymmA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info =  detail::sysv (ul, a, i, b, w);
      }
      return info;
    }

    template <typename SymmA, typename MatrB>
    int sysv (SymmA& a, MatrB& b)
    {
      // with 'internal' pivot and work vectors

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = sytrf_work ('O', a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
        typedef typename SymmA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info =  detail::sysv (uplo, a, i, b, w);
      }
      return info;
    }

    /*
     * sytrf() computes the factorization of a symmetric matrix A using
     * the  Bunch-Kaufman diagonal pivoting method. The form of the
     * factorization is
     *    A = U * D * U^T  or  A = L * D * L^T
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is symmetric and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail
    {
      inline
      void sytrf (char const uplo, integer_t const n,
                  float* a, integer_t const lda, integer_t* ipiv,
                  float* w, integer_t const lw, integer_t* info)
      {
        LAPACK_SSYTRF (&uplo, &n, a, &lda, ipiv, w, &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  double* a, integer_t const lda, integer_t* ipiv,
                  double* w, integer_t const lw, integer_t* info)
      {
        LAPACK_DSYTRF (&uplo, &n, a, &lda, ipiv, w, &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CSYTRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZSYTRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      template <typename SymmA, typename IVec, typename Work>
      int sytrf (char const ul, SymmA& a, IVec& i, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (i));

        integer_t info;
        sytrf (ul, n, traits::matrix_storage (a),
               traits::leading_dimension (a),
               traits::vector_storage (i),
               traits::vector_storage (w),
               traits::vector_size (w),
               &info);
        return info;
      }

    }

    template <typename SymmA, typename IVec, typename Work>
    inline
    int sytrf (char const ul, SymmA& a, IVec& i, Work& w)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::sytrf (ul, a, i, w);
    }

    template <typename SymmA, typename IVec, typename Work>
    inline
    int sytrf (SymmA& a, IVec& i, Work& w)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytrf (uplo, a, i, w);
    }

    template <typename SymmA, typename Ivec>
    int sytrf (char const ul, SymmA& a, Ivec& i)
    {
      // with 'internal' work vector

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t info = -101;
      integer_t lw = sytrf_work ('O', ul, a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info =  detail::sytrf (ul, a, i, w);
      return info;
    }

    template <typename SymmA, typename Ivec>
    int sytrf (SymmA& a, Ivec& i)
    {
      // with 'internal' work vector

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      integer_t lw = sytrf_work ('O', a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info =  detail::sytrf (uplo, a, i, w);
      return info;
    }

    /*
     * sytrs() solves a system of linear equations A*X = B with
     * a symmetric matrix A using the factorization
     *    A = U * D * U^T   or  A = L * D * L^T
     * computed by sytrf().
     */

    namespace detail {

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  float const* a, integer_t const lda, integer_t const* ipiv,
                  float* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_SSYTRS (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  double const* a, integer_t const lda, integer_t const* ipiv,
                  double* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_DSYTRS (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_f const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_f* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_CSYTRS (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_d const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_d* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_ZSYTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmA, typename MatrB, typename IVec>
      int sytrs (char const ul, SymmA const& a, IVec const& i, MatrB& b) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        sytrs (ul, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::vector_storage (i),
#else
               traits::vector_storage_const (i),
#endif
               traits::matrix_storage (b),
               traits::leading_dimension (b), &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sytrs (char const ul, SymmA const& a, IVec const& i, MatrB& b) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::sytrs (ul, a, i, b);
    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sytrs (SymmA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytrs (uplo, a, i, b);
    }

    namespace detail
    {
      inline
      void sytri (char const uplo, integer_t const n, float* a, integer_t const lda,
          integer_t const* ipiv, float* work, integer_t* info)
      {
        LAPACK_SSYTRI (&uplo, &n, a, &lda, ipiv, work, info);
      }

      inline
      void sytri (char const uplo, integer_t const n, double* a, integer_t const lda,
          integer_t const* ipiv, double* work, integer_t* info)
      {
        LAPACK_DSYTRI (&uplo, &n, a, &lda, ipiv, work, info);
      }

      inline
      void sytri (char const uplo, integer_t const n, traits::complex_f* a,
          integer_t const lda, integer_t const* ipiv, traits::complex_f* work, integer_t* info)
      {
        LAPACK_CSYTRI (&uplo, &n, traits::complex_ptr (a), &lda, ipiv,
            traits::complex_ptr (work), info);
      }

      inline
      void sytri (char const uplo, integer_t const n, traits::complex_d* a,
          integer_t const lda, integer_t const* ipiv, traits::complex_d* work, integer_t* info)
      {
        LAPACK_ZSYTRI (&uplo, &n, traits::complex_ptr (a), &lda, ipiv,
            traits::complex_ptr (work), info);
      }

      template <typename SymmA, typename IVec, typename Work>
      int sytri (char const ul, SymmA& a, IVec const& ipiv, Work& work)
      {
        assert (ul == 'U' || ul == 'L');

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (ipiv));
        assert (n == traits::vector_size (work));

        integer_t info;
        //const double* dummy = traits::matrix_storage (a);
        detail::sytri (ul, n, traits::matrix_storage (a),
            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
            traits::vector_storage (ipiv),
#else
            traits::vector_storage_const (ipiv),
#endif
            traits::vector_storage (work),
            &info);
        return info;
      }

    } // namespace detail

    //Internal allocation of workspace, general matrix with up/low tag
    template <typename SymmA, typename IVec>
    int sytri (char const ul, SymmA& a, IVec const& ipiv)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<SymmA>::matrix_structure,
            traits::general_t
            >::value));
#endif

      typedef typename SymmA::value_type value_type;
      std::ptrdiff_t n = traits::matrix_size1(a);
      traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1,n));

      return detail::sytri (ul, a, ipiv, work);
    }

    //Internal allocation of workspace, symmetric matrix

    /*Warning: the function will work only if SymmA is a
      symmetric_adaptor. With SymmA = symmetric_matrix a
      boost::STATIC_ASSERTION_FAILURE will be thrown at compile
      time, because symmetric_matrix has a symmetric_packed_t
      structure instead of symmetric_t. Use sptri() for
      symmetric packed matrices.
      */
    template <typename SymmA, typename IVec>
    int sytri (SymmA& a, IVec const& ipiv)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      typedef typename SymmA::value_type value_type;
      std::ptrdiff_t n = traits::matrix_size1(a);
      traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1,n));

      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytri (uplo, a, ipiv, work);
    }

  } // namespace lapack

}}} // namespace boost::numeric::bindings

#endif


#include "dbcmetaparser.h"

#include <cstdint>
#include <string>
#include <stdexcept>

#include <boost/xpressive/xpressive_static.hpp>

namespace xpr = boost::xpressive;

namespace {

using namespace xpr;
mark_tag factor{1}, offset{2}, minimum{3}, maximum{4}, fractional{1}, exponent{2};

// Fields in DBC signal definition using double type
sregex decimal_values = '(' >> (factor = -+_) >> ',' >> (offset = -+_) >> ')' >> blank
    >> '[' >> (minimum = -+_) >> '|' >> (maximum = -+_) >> ']';

// Decimal number with optional fractional part and scientific notation
sregex decimal = +_d >> !('.' >> (fractional = +_d)) >> !((set='e','E')
    >> (exponent = !(set='-','+') >> +_d));

}  // namespace

dbc::Signal_meta_data dbc::meta::parse_signal(const std::string& line)
{
  // Calculate required decimal places for correct reproduction (not significant digits)
  auto fractional_precision = [](auto&& match) -> std::int8_t {
    xpr::smatch number;
    if (xpr::regex_match(match.first, match.second, number, decimal)) {
      auto exp = number[exponent].length() > 0 ? std::stoi(number[exponent].str()) : 0;
      auto prec = number[fractional].length() - exp;
      return prec < 0 ? 0 : prec;  // Positive exponent may lead to negative precision, e.g 1.0e9
    }
    return 0;
  };

  xpr::smatch values;
  if (xpr::regex_search(line, values, decimal_values)) {
    try {
      Signal_meta_data meta_data;
      meta_data.factor_precision = fractional_precision(values[factor]);
      meta_data.offset_precision = fractional_precision(values[offset]);
      meta_data.minimum_precision = fractional_precision(values[minimum]);
      meta_data.maximum_precision = fractional_precision(values[maximum]);
      return meta_data;
    }
    catch (const std::logic_error& e) {
      return {};
    }
  }

  return {};
}


// copyright (c) 2014 the dzcoin core developers
// distributed under the mit software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "main.h"

#include "test/test_dzcoin.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

boost_fixture_test_suite(main_tests, testingsetup)

static void testblocksubsidyhalvings(const consensus::params& consensusparams)
{
    int maxhalvings = 64;
    camount ninitialsubsidy = 50 * coin;

    camount nprevioussubsidy = ninitialsubsidy * 2; // for height == 0
    boost_check_equal(nprevioussubsidy, ninitialsubsidy * 2);
    for (int nhalvings = 0; nhalvings < maxhalvings; nhalvings++) {
        int nheight = nhalvings * consensusparams.nsubsidyhalvinginterval;
        camount nsubsidy = getblocksubsidy(nheight, consensusparams);
        boost_check(nsubsidy <= ninitialsubsidy);
        boost_check_equal(nsubsidy, nprevioussubsidy / 2);
        nprevioussubsidy = nsubsidy;
    }
    boost_check_equal(getblocksubsidy(maxhalvings * consensusparams.nsubsidyhalvinginterval, consensusparams), 0);
}

static void testblocksubsidyhalvings(int nsubsidyhalvinginterval)
{
    consensus::params consensusparams;
    consensusparams.nsubsidyhalvinginterval = nsubsidyhalvinginterval;
    testblocksubsidyhalvings(consensusparams);
}

boost_auto_test_case(block_subsidy_test)
{
    testblocksubsidyhalvings(params(cbasechainparams::main).getconsensus()); // as in main
    testblocksubsidyhalvings(150); // as in regtest
    testblocksubsidyhalvings(1000); // just another interval
}

boost_auto_test_case(subsidy_limit_test)
{
    const consensus::params& consensusparams = params(cbasechainparams::main).getconsensus();
    camount nsum = 0;
    for (int nheight = 0; nheight < 14000000; nheight += 1000) {
        camount nsubsidy = getblocksubsidy(nheight, consensusparams);
        boost_check(nsubsidy <= 50 * coin);
        nsum += nsubsidy * 1000;
        boost_check(moneyrange(nsum));
    }
    boost_check_equal(nsum, 2099999997690000ull);
}

bool returnfalse() { return false; }
bool returntrue() { return true; }

boost_auto_test_case(test_combiner_all)
{
    boost::signals2::signal<bool (), combinerall> test;
    boost_check(test());
    test.connect(&returnfalse);
    boost_check(!test());
    test.connect(&returntrue);
    boost_check(!test());
    test.disconnect(&returnfalse);
    boost_check(test());
    test.disconnect(&returntrue);
    boost_check(test());
}

boost_auto_test_suite_end()


#include "ModalWarpingTrainingModel.h"
#include "Integrator/ImplicitNewMatkSparseIntegrator.h"
#include <random>
#include <fstream>
#include <iostream>
#include <time.h>
#include "Simulator/DeepWarp/ModalRotationMatrix.h"
#include "Integrator/ImplicitModalWarpingIntegrator.h"
#include "Functions/GeoMatrix.h"
#include <Eigen/QR>
#include "Functions/findElementInVector.h"
#include "LoboVolumetricMesh/LoboVolumetriceMeshCore.h"
#include "Simulator/ForceField/RotateForceField.h"

#include "Simulator/ReducedSTVK/ReducedSTVKModel.h"
#include "Simulator/ReducedForceModel/ReducedForceModel.h"
#include "Simulator/ReducedForceModel/ReducedSTVKForceModel.h"
#include "Integrator/ImpicitNewMarkDenseIntegrator.h"

ModalWarpingTrainingModel::ModalWarpingTrainingModel(LoboVolumetricMesh* volumtrciMesh_, VectorXd gravity_, SparseMatrix<double>* modalRotationSparseMatrix_, ModalRotationMatrix* modalrotationMatrix_, MatrixXd* subspaceModes_, int r, double timestep, SparseMatrix<double>* massMatrix_, LoboForceModel* forcemodel_, LoboForceModel* nonlinearforceModel_, int numConstrainedDOFs_, int* constrainedDOFs_, double dampingMassCoef, double dampingStiffnessCoef, bool useStaticSolver) :WarpingMapTrainingModel(subspaceModes_, r, timestep, massMatrix_, forcemodel_, nonlinearforceModel_, numConstrainedDOFs_, constrainedDOFs_, dampingMassCoef, dampingStiffnessCoef, useStaticSolver)
{
	this->volumtrciMesh = volumtrciMesh_;
	this->modalRotationSparseMatrix = modalRotationSparseMatrix_;
	this->modalrotationMatrix = modalrotationMatrix_;
	localOrientationMatrixR = new SparseMatrix<double>();
	matrixR_ = new SparseMatrix<double>();

	this->gravity = gravity_;

	//0.001
	modalwarpingintegrator = new ImplicitModalWarpingIntegrator(modalrotationMatrix, modalRotationSparseMatrix,
		r, 0.001, massMatrix_, loboforceModel, 1, numConstrainedDOFs_, constrainedDOFs_, dampingMassCoef, dampingStiffnessCoef, 1, 1e-7, 0.25, 0.5, false
		);

	if (subspaceModes->size() > 0)
	{
		std::cout << "read modes please" << std::endl;
		reducedMassMatrix = new MatrixXd();
		*reducedMassMatrix = subspaceModes->transpose()*(*massMatrix)**subspaceModes;
		int r = subspaceModes->cols();
		VectorXd reducedgraivty(r);
		int R = volumtrciMesh->getNumVertices() * 3;

		reducedSTVKmodel = new ReducedSTVKModel(this->volumtrciMesh, this->massMatrix, subspaceModes);

		reducedSTVKmodel->computeGravity(&reducedgraivty, massMatrix, R);
		reducedSTVKmodel->setGravityForce(reducedgraivty.data());
		reducedSTVKmodel->setGravity(false);
		reducedSTVKmodel->computeReducedModesCoefficients();

		reducedforcemodel = new ReducedSTVKForceModel(reducedSTVKmodel);

		reducedIntergrator = new ImpicitNewMarkDenseIntegrator(r, timestep, reducedMassMatrix, reducedforcemodel, 0, 0, dampingMassCoef, dampingStiffnessCoef, 1);
	}

	modalwarpingintegrator->setDampingMassCoef(0.1);
	modalwarpingintegrator->setDampingStiffnessCoef(0.1); //0.1

	nonLinearIntegrator->setDampingMassCoef(0.3);
	nonLinearIntegrator->setDampingStiffnessCoef(0.3);
	nonLinearIntegrator->setTimeStep(0.005);

	integrator->setDampingMassCoef(0.8);
	integrator->setDampingStiffnessCoef(0.8);

	nonLinearIntegrator->setMaxInteration(50);
}

ModalWarpingTrainingModel::~ModalWarpingTrainingModel()
{
	delete localOrientationMatrixR;
	delete matrixR_;
	delete reducedIntergrator;
	delete modalwarpingintegrator;
	delete reducedforcemodel;
	delete reducedSTVKmodel;
}

void ModalWarpingTrainingModel::excute()
{
	if (getForcefieldType() == 1)
	{
		modalwarpingintegrator->setTimeStep(0.002);
		modalwarpingintegrator->setDampingMassCoef(0.2);
		modalwarpingintegrator->setDampingStiffnessCoef(0.2); //0.1
		method12Twist();
	}
	else
	if (getForcefieldType() == 0)
	{
		modalwarpingintegrator->setTimeStep(0.002);
		modalwarpingintegrator->setDampingMassCoef(0.5);
		modalwarpingintegrator->setDampingStiffnessCoef(0.5); //0.1
		method10();
	}
	//methodFortwist();
	//method9();
	return;
}

void ModalWarpingTrainingModel::getTraingLinearForce(int index, VectorXd &force)
{
	//force = modalwarpingintegrator->getInteranlForce(trainingNonLinearDis[index]);

	//force = gravity;
	Vector3d nodeacc = forcefieldDirection[index];
	VectorXd fullforce = this->createGravity(nodeacc);
	force = fullforce;
	//force = gravity;
}

void ModalWarpingTrainingModel::getTraingLinearForce(VectorXd& lq, VectorXd &force)
{
	force = modalwarpingintegrator->getInteranlForce(lq);
	//force = gravity;
}

void ModalWarpingTrainingModel::getTraingLinearForce(VectorXd& lq, VectorXd &force, double poisson)
{
	LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
	LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
	enmateria->setNu(poisson);
	modalwarpingintegrator->updateMaterial();
	force = modalwarpingintegrator->getInteranlForce(lq);

}

Eigen::VectorXd ModalWarpingTrainingModel::getNonlinearInternalforce(int disid)
{
	return nonLinearIntegrator->getInteranlForce(trainingNonLinearDis[disid]);
}

void ModalWarpingTrainingModel::subexcute3()
{
	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	trainingLinearDis.clear();
	trainingNonLinearDis.clear();
	int maxIteration = 1000;
	trainingLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	trainingNonLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	VectorXd reducedForce(subspaceModes->cols());
	int dataCount = 0;

	while (dataCount < getNumTrainingSet())
	{
		std::cout << dataCount << std::endl;
		srand(time(NULL) + dataCount);
		std::default_random_engine generator;
		generator.seed(time(NULL) + dataCount);
		std::uniform_real_distribution<double> latitude_distribution(-forceScale, forceScale);
		double scale = (this->forceScale / getNumTrainingSet()) *(getNumTrainingSet() - dataCount) + 0.01;
		std::cout << "scale " << scale << std::endl;
		double angle = (PI_/2 / getNumTrainingSet())*dataCount + 0.01;

		Vector3d axis(0, 0, 1);

		angle *= -1;

		//axis.setRandom();
		//axis.normalize();
		//test

		VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		//fullforce = gravity*scale;

		nonLinearIntegrator->setExternalForces(fullforce.data());
		nonLinearIntegrator->resetToRest();
		nonLinearIntegrator->setSaveStepResidual(true);
		VectorXd q_dis;

		//store training linear dis
		//trainingLinearDis.push_back(q_dis);
		double preNorm = DBL_MAX;
		VectorXd preDis = gravity;
		preDis.setConstant(1000);

		double residual;

		for (int j = 0; j < maxIteration; j++)
		{
			nonLinearIntegrator->doTimeStep();
			double norm_ = nonLinearIntegrator->getVectorq().norm();
			VectorXd cur_dis = nonLinearIntegrator->getVectorq();

			residual = std::abs((preNorm - norm_) / preNorm);
			//We only pick the displacement which has larget difference
			if (residual > 0.001)
			{
				if (norm_ > 0.01)
				{
					trainingNonLinearDis.push_back(cur_dis);

					VectorXd curExternalForce = nonLinearIntegrator->getStep_residual();
					modalwarpingintegrator->setSaveStepResidual(true);
					convergeIntegrationLocal(modalwarpingintegrator, curExternalForce, 1000);
					q_dis = modalwarpingintegrator->getVectorq();

					trainingLinearDis.push_back(q_dis);

					dataCount++;
				}
			}
			else
			{
				break;
			}

			preNorm = norm_;
			if (residual < 1e-5)
			{
				break;
			}

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		nonLinearIntegrator->setSaveStepResidual(false);
	}
}

void ModalWarpingTrainingModel::subexcute()
{
	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	trainingLinearDis.clear();
	trainingNonLinearDis.clear();
	int maxIteration = 1000;
	trainingLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	trainingNonLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	VectorXd reducedForce(subspaceModes->cols());
	int dataCount = 0;
	while (dataCount < getNumTrainingSet())
	{
		std::cout << dataCount << std::endl;
		srand(time(NULL) + dataCount);
		std::default_random_engine generator;
		generator.seed(time(NULL) + dataCount);
		std::uniform_real_distribution<double> latitude_distribution(-forceScale, forceScale);
		double scale = (this->forceScale / getNumTrainingSet()) *(getNumTrainingSet()-dataCount) + 0.01;
		std::cout <<"scale "<< scale << std::endl;
		double angle = (PI_ / getNumTrainingSet())*dataCount - PI_ / 2 + 0.1;

		Vector3d axis(0, 0, 1);
		//axis.setRandom();
		//axis.normalize();
		//test
		angle = PI_ / 2;

		VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		fullforce = gravity*scale;

		modalwarpingintegrator->resetToRest();
		modalwarpingintegrator->setSaveStepResidual(true);
		double preNorm = DBL_MAX;
		double residual = DBL_MAX;
		VectorXd preq = fullforce;
		preq.setConstant(10000);

		for (int j = 0; j < maxIteration; j++)
		{
			VectorXd fullq = modalwarpingintegrator->getVectorq();
			VectorXd w = (*modalRotationSparseMatrix)*fullq;

			//set the external force
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w);
			//VectorXd mergedExternalForce = (*localOrientationMatrixR)*fullforce;
			modalwarpingintegrator->setExternalForces(fullforce.data());
			modalwarpingintegrator->doTimeStep();

			double norm_ = modalwarpingintegrator->getVectorq().norm();

			residual = std::abs((fullq - preq).norm() / preq.norm());

			if (residual > 0.01)
			{
				preNorm = norm_;
				preq = fullq;

				if (norm_ > 0.01)
				{
					bool reset = false;
					if (j == 0)
					{
						reset = true;
					}

					VectorXd q_dis;
					q_dis = modalwarpingintegrator->getVectorq();
					w = (*modalRotationSparseMatrix)*q_dis;
					//store training linear dis
					trainingLinearDis.push_back(q_dis);
					VectorXd curExternalForce = modalwarpingintegrator->getStep_residual();
					modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w,false);
					curExternalForce = (*localOrientationMatrixR)*curExternalForce;

					convergeIntegrationNonLinear(nonLinearIntegrator, curExternalForce, maxIteration, reset);
					q_dis = nonLinearIntegrator->getVectorq();
					trainingNonLinearDis.push_back(q_dis);
					dataCount++;
					std::cout << dataCount << std::endl;
				}
			}

			if (residual < 1e-6)
			{
				break;
			}

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		modalwarpingintegrator->setSaveStepResidual(false);
	}
	std::cout << "excute finished" << std::endl;
}

void ModalWarpingTrainingModel::subexcute2()
{
	std::vector<Vector3d> nodedisplacement;

	generateRotationVectorSample(M_PI * 2, 0, 3,
		M_PI / 2, -M_PI / 2, 1,
		0, M_PI/3, 2, nodedisplacement);

	/*Vector3d firstNode = nodedisplacement[10];
	nodedisplacement.clear();
	nodedisplacement.push_back(firstNode);*/

	int r = this->gravity.rows();

	double maxNodedisdance = -DBL_MAX;
	int maxNodeid = -1;

	for (int j = 0; j < r / 3; j++)
	{
		if (node_distance[j] > maxNodedisdance)
		{
			maxNodedisdance = node_distance[j];
			maxNodeid = j;
		}
	}

	LoboNodeBase* nodep = volumtrciMesh->getNodeRef(maxNodeid);
	int correspondingSize = 3 + nodep->neighbor.size() * 3;
	int Phi_1_Size = nodep->neighbor.size() * 3;
	MatrixXd phi1(3, Phi_1_Size);
	phi1.setZero();
	MatrixXd phi2(3, 3);
	phi2.setZero();
	MatrixXd phitotal(3, Phi_1_Size + 3);

	for (int k = 0; k < modalRotationSparseMatrix->outerSize(); ++k)
		for (SparseMatrix<double>::InnerIterator it(*modalRotationSparseMatrix, k); it; ++it)
		{
			if (it.row() / 3 == maxNodeid)
			{
				int neighborid = it.col() / 3;
				int insideIndex = findElementIndex(nodep->neighbor, neighborid);
				if (insideIndex != -1)
				{
					phi1.data()[(insideIndex * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();
					phitotal.data()[(insideIndex * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();

				}
				if (it.col() / 3 == maxNodeid)
				{
					phi2.data()[(it.col() % 3) * 3 + it.row() % 3] = it.value();
					phitotal.data()[(nodep->neighbor.size() * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();
				}
			}
		}

	MatrixXd invPhi1 = pseudoinverse(phi1);
	MatrixXd invPhitotal = pseudoinverse(phitotal);

	if (subspaceModes == NULL)
	{
		std::cout << "subspaceModes is NULL" << std::endl;
	}

	MatrixXd subModes(Phi_1_Size + 3, subspaceModes->cols());
	subModes.setZero();
	for (int i = 0; i < nodep->neighbor.size() * 3; i++)
	{
		int neighborid = nodep->neighbor[i / 3];
		int row = neighborid * 3 + i % 3;
		for (int j = 0; j < subspaceModes->cols(); j++)
		{
			subModes.data()[j*subModes.rows() + i] = subspaceModes->data()[j*subspaceModes->rows() + row];
		}
	}

	for (int i = 0; i < 3; i++)
	{
		int row = maxNodeid * 3 + i;
		for (int j = 0; j < subspaceModes->cols(); j++)
		{
			subModes.data()[j*subModes.rows() + i + Phi_1_Size] = subspaceModes->data()[j*subspaceModes->rows() + row];
		}
	}

	MatrixXd phiSubModes = phitotal*subModes;
	MatrixXd invPhiSubModes = pseudoinverse(phiSubModes);
	MatrixXd invSubModes = pseudoinverse(subModes);

	modalwarpingintegrator->setStoreLagrangeMultipliers(true);

	int numscale = 1;

	std::vector<VectorXd> exforceList;

	for (int i = 0; i < subspaceModes->cols(); i++)
	{
		VectorXd lq = subspaceModes->col(i);
		VectorXd w = (*modalRotationSparseMatrix)*lq;
		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
		VectorXd nq = *localOrientationMatrixR*lq;
		VectorXd constrainForce = nonLinearIntegrator->getInteranlForce(nq);
		exforceList.push_back(constrainForce);
	}

	if (0)
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		VectorXd w(r);
		w.setZero();

		Vector3d w_j = nodedisplacement[i];

		for (int j = 0; j < numscale; j++)
		{
			Vector3d uscale = Vector3d(0, -j*0.06, 0);

			VectorXd targetq;

			VectorXd T(nodep->neighbor.size() * 3 + 3);
			T.setZero();

			for (int k = 0; k < T.rows() / 3; k++)
			{
				T.data()[k * 3 + 0] = uscale.data()[0];
				T.data()[k * 3 + 1] = uscale.data()[1];
				T.data()[k * 3 + 2] = uscale.data()[2];
			}

			targetq = invPhitotal*(w_j - phitotal*T);
			targetq += T;

			VectorXd linearq;
			VectorXd reducedq = invPhiSubModes*(w_j);
			linearq = subModes*reducedq;
			targetq = linearq + T;

			SparseMatrix<double> constrainMatrix;
			VectorXd constrainTarget;

			VectorXd lq(r);
			lq.setZero();
			lq = *subspaceModes*invSubModes*targetq;
			w = (*modalRotationSparseMatrix)*lq;
			modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
			VectorXd nq = *localOrientationMatrixR*lq;

			VectorXd constrainForce = nonLinearIntegrator->getInteranlForce(nq);
			/*constrainForce = modalwarpingintegrator->getInteranlForce(lq);
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
			constrainForce = *localOrientationMatrixR*constrainForce;*/
			exforceList.push_back(constrainForce);

			//VectorXd nq;
			////nq = *localOrientationMatrixR*lq;
			//nq = nonLinearIntegrator->getVectorq();

			//trainingLinearDis.push_back(lq);

			//trainingNonLinearDis.push_back(nq);
		}
	}

	int maxIteration = 1000;
	int dataCount = 0;
	/*VectorXd temp = exforceList[0];
	exforceList.clear();
	exforceList.push_back(temp);*/

	this->setNumTrainingSet(exforceList.size());
	this->setNumTrainingHighFreq(0);

	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	for (int i = 0; i < exforceList.size(); i++)
	{
		std::cout <<"--------"<< i <<"----------"<< std::endl;
		VectorXd fullforce = exforceList[i];

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		nonLinearIntegrator->setExternalForces(fullforce.data());
		nonLinearIntegrator->resetToRest();
		nonLinearIntegrator->setSaveStepResidual(true);
		VectorXd q_dis;

		//store training linear dis
		//trainingLinearDis.push_back(q_dis);
		double preNorm = DBL_MAX;
		VectorXd preDis = gravity;
		preDis.setConstant(1000);

		double residual;
		VectorXd qn = subspaceModes->col(i);
		bool converged = convergeIntegrationLocal(modalwarpingintegrator, fullforce, 1000, true);
		VectorXd ql = modalwarpingintegrator->getVectorq();
		trainingNonLinearDis.push_back(qn);
		trainingLinearDis.push_back(ql);

		if (0)
		for (int j = 0; j < maxIteration; j++)
		{
			nonLinearIntegrator->doTimeStep();

			double norm_ = nonLinearIntegrator->getVectorq().norm();
			VectorXd cur_dis = nonLinearIntegrator->getVectorq();

			residual = std::abs((preNorm - norm_) / preNorm);
			//We only pick the displacement which has larget difference
			if (residual < 1e-5)
			{
				if (norm_ > 0.01)
				{
					VectorXd curExternalForce = nonLinearIntegrator->getStep_residual();
					modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, cur_dis, true);
					curExternalForce = (*localOrientationMatrixR)*curExternalForce;

					bool ifreset = false;
					if (j == 0)
					{
						ifreset = true;
					}
					bool converged = convergeIntegrationLocal(modalwarpingintegrator, curExternalForce, 1000, ifreset);

					if (!converged)
					{
						continue;
					}

					q_dis = modalwarpingintegrator->getVectorq();
					trainingNonLinearDis.push_back(cur_dis);
					trainingLinearDis.push_back(q_dis);

					dataCount++;
				}
				break;
			}

			preNorm = norm_;

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		nonLinearIntegrator->setSaveStepResidual(false);
	}

	std::cout << "excute finished" << std::endl;

}

void ModalWarpingTrainingModel::subexcute4()
{
	int numScale = 100;
	this->setNumTrainingSet(numScale);
	this->setNumTrainingHighFreq(0);

	MatrixXd forceMatrix(subspaceModes->rows(), numScale);

	for (int i = 0; i < numScale; i++)
	{
		VectorXd lq = subspaceModes->col(2) * (i*0.1+0.1);

		VectorXd w = *modalRotationSparseMatrix*lq;

		VectorXd extforce = modalwarpingintegrator->getInteranlForce(lq);

		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);

		extforce = *localOrientationMatrixR*extforce;

		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);

		VectorXd guessq = *localOrientationMatrixR*lq;
		VectorXd modalwarpingforce = nonLinearIntegrator->getInteranlForce(guessq);
		std::cout <<"diff force = > "<< (modalwarpingforce - extforce).norm() / extforce.norm() << std::endl;
		forceMatrix.col(i) = extforce;

		//convergeIntegrationNonLinear(nonLinearIntegrator, extforce, 1000, false);

		//VectorXd nq = nonLinearIntegrator->getVectorq();

		trainingNonLinearDis.push_back(guessq);
		trainingLinearDis.push_back(lq);

	}

}

void ModalWarpingTrainingModel::subexcute5()
{
	int numScale = 600;
	int numData = 400;
	this->setNumTrainingSet(numData-1);
	this->setNumTrainingHighFreq(0);

	for (int i = 0; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd lq = ((subspaceModes->col(3)*10)/numScale)*(i+1);
		VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
		VectorXd w = *modalRotationSparseMatrix*lq;
		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
		VectorXd nf = *localOrientationMatrixR*lf;
		modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
		VectorXd wq = *matrixR_*lq;

		if (i / numScale < 0.1)
		{
			nonLinearIntegrator->setState(wq.data());
		}

		nonLinearIntegrator->setMaxInteration(80);
		convergeIntegrationNonLinear(nonLinearIntegrator, nf, 100, false);

		VectorXd nq = nonLinearIntegrator->getVectorq();

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
		}
	}

}

void ModalWarpingTrainingModel::method6()
{

	int numScale = 100;
	int numData = 100;
	this->setNumTrainingSet(numData*2);
	this->setNumTrainingHighFreq(0);

	int numVertex = volumtrciMesh->getNumVertices();
	std::ofstream test("test2.txt");
	VectorXd nodeYaxis(numVertex * 3);
	nodeYaxis.setZero();
	for (int i = 0; i < numVertex; i++)
	{
		nodeYaxis.data()[i * 3 + 1] = 1;
	}

	VectorXd lq = (subspaceModes->col(3) * 10);

	VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
	VectorXd w = *modalRotationSparseMatrix*lq;
	modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
	VectorXd lqorientation = *localOrientationMatrixR*nodeYaxis;

	VectorXd nf = *localOrientationMatrixR*lf;
	modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
	VectorXd wq = *matrixR_*lq;
	modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, wq, false);
	VectorXd nqorientation = *localOrientationMatrixR*nodeYaxis;
	test << lqorientation.transpose() << std::endl;
	test << nqorientation.transpose() << std::endl;
	test.close();
	std::cout << (lqorientation - nqorientation).norm() / nqorientation.norm() << std::endl;

	VectorXd ori_p(numVertex * 3);
	for (int i = 0; i < numVertex; i++)
	{
		Vector3d nodeori = volumtrciMesh->getNodeRestPosition(i);
		ori_p.data()[i * 3 + 0] = nodeori.data()[0];
		ori_p.data()[i * 3 + 1] = nodeori.data()[1];
		ori_p.data()[i * 3 + 2] = nodeori.data()[2];
	}

	for (int i = 0; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd nq = ((*matrixR_*lq) / numScale)*(i + 1);
		VectorXd originlq = lq / numScale*(i + 1);

		VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
		modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, nq, true);
		//modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, true);
		VectorXd warp_nf = *localOrientationMatrixR*real_nf;

		convergeIntegrationLocal(modalwarpingintegrator, warp_nf, 200, true);

		VectorXd real_lq = modalwarpingintegrator->getVectorq();
		//VectorXd real_lq;
		//real_lq = *localOrientationMatrixR*nq;

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(originlq);
		}
	}
}

void ModalWarpingTrainingModel::method7()
{
	int numScale = 1;
	int numData = 1;
	this->setNumTrainingSet(numData * 2);
	this->setNumTrainingHighFreq(0);

	Vector3d axis(0, 0, 1);
	double angle = PI_/2;
	VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

	convergeIntegrationNonLinear(nonLinearIntegrator, fullforce, 1000, true);
	VectorXd targetnq = nonLinearIntegrator->getVectorq();

	for (int i = numScale-1; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd nq = ((targetnq) / numScale)*(i + 1);
		VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
		modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, nq, true);
		VectorXd warp_nf = *localOrientationMatrixR*real_nf;
		convergeIntegrationLocal(modalwarpingintegrator, warp_nf, 200, true);
		VectorXd real_lq = modalwarpingintegrator->getVectorq();

		//warp real_lq
		VectorXd w = *modalRotationSparseMatrix*real_lq;
		modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
		VectorXd wq = *matrixR_*real_lq;

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(wq);
		}
	}

}

void ModalWarpingTrainingModel::method8()
{
	std::vector<Vector3d> nodedisplacement;
	nodedisplacement.clear();
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	std::vector<bool> ifreset;

	//if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI/3.0, -M_PI / 2.0, 10,
			9.8, 4.9, 2, nodedisplacement, ifreset, potentialDirection);
		samplePoissonRatio(0.3, 0.3, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	/*nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);*/

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	std::vector<VectorXd> ql_list;

	int dataCount = 0;
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		VectorXd nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		convergeIntegrationNonLinearBuffer(nonLinearIntegrator, fullforce, 2000, qn_list, true);

		int range = qn_list.size() / numDataPerSample;
		range = 1;
		for (int j = 0; j <qn_list.size(); j++)
		{
			VectorXd nq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((nq - trainingNonLinearDis.back()).norm() / trainingNonLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, nq, true);
			VectorXd warp_nf = *localOrientationMatrixR*real_nf;

			bool reset = false;

			if (j == 0 && ifreset[i] == true)
			{
				reset = true;
			}

			convergeIntegrationLocal(modalwarpingintegrator, real_nf, 2000, reset);

			VectorXd real_lq = modalwarpingintegrator->getVectorq();

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			forcefieldDirection.push_back(potentialDirection[i]);
			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());

	std::cout << "finished" << std::endl;
}

void ModalWarpingTrainingModel::method9()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	nodedisplacement.clear();
	ifreset.clear();

	//if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI / 2.0, -M_PI / 2.0, 10,
			4.0, 2.0, 2, nodedisplacement, ifreset, potentialDirection);

		samplePoissonRatio(0.2, 0.2, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	/*nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);*/

	//nodedisplacement.push_back(Vector3d(-4.9, 1.83721e-032, 3.00038e-016));
	//ifreset.push_back(true);
	//potentialDirection.push_back(Vector3d(-1, 0, 0));
	//poissonPerdirection.push_back(0.2);

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout <<"poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	int dataCount = 0;
	nonLinearIntegrator->resetToRest();
	//VectorXd q_test(volumtrciMesh->getNumVertices() * 3);
	//q_test.setRandom();
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		Vector3d nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		//reset = false;
		bool reset = ifreset[i];

		std::cout <<"ifrest"<< reset << std::endl;
		convergeIntegrationLocalBuffer(modalwarpingintegrator, fullforce, 1000, qn_list, reset);

		int range = qn_list.size() / numDataPerSample;
		if (range <= 1)
		{
			range = 2;
		}
		range = 1;

		std::cout <<"range => "<< range << std::endl;
		for (int j = 0; j < qn_list.size(); j += range)
		{
			std::cout << "force " << i << "/" << nodedisplacement.size() - 1 << std::endl;
			std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
			VectorXd lq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			//use ku = f instead of ku = Rf
			VectorXd nq;
			if (getLinearDisOnly())
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
				nq = *localOrientationMatrixR*lq;
			}
			else
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
				VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
				VectorXd nf = *localOrientationMatrixR*lf;

				bool reset = false;

				if (j == 0 && ifreset[i] == true)
				{
					reset = true;
				}

				bool converged = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

				nq = nonLinearIntegrator->getVectorq();

				if (converged == false)
				{
					std::cout << "break" << std::endl;
					break;
				}
			}

			if (dataCount % 2 == 1)
			{
				dataCount++;
				continue;
			}

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
			forcefieldDirection.push_back(potentialDirection[i]);

			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());
}

void ModalWarpingTrainingModel::method10()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	nodedisplacement.clear();
	ifreset.clear();

	if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI / 2.0, -M_PI / 2.0, 10,
			4.0, 2.0, 2, nodedisplacement, ifreset, potentialDirection);

		samplePoissonRatio(0.2, 0.2, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);

	//nodedisplacement.push_back(Vector3d(-4.9, 1.83721e-032, 3.00038e-016));
	//ifreset.push_back(true);
	//potentialDirection.push_back(Vector3d(-1, 0, 0));
	//poissonPerdirection.push_back(0.2);

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	int dataCount = 0;
	nonLinearIntegrator->resetToRest();
	//VectorXd q_test(volumtrciMesh->getNumVertices() * 3);
	//q_test.setRandom();
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		Vector3d nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		//nonLinearIntegrator->updateMaterial();

		//reset = false;
		bool reset = ifreset[i];

		std::cout << "ifrest" << reset << std::endl;
		convergeIntegrationLocalBuffer(modalwarpingintegrator, fullforce, 1000, qn_list, reset);

		int range = qn_list.size() / numDataPerSample;
		if (range <= 1)
		{
			range = 2;
		}
		range = 1;

		std::cout << "range => " << range << std::endl;
		for (int j = 0; j < qn_list.size(); j += range)
		{
			std::cout << "force " << i << "/" << nodedisplacement.size() - 1 << std::endl;
			std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
			VectorXd lq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			//use ku = f instead of ku = Rf
			VectorXd nq;
			if (getLinearDisOnly())
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
				nq = *localOrientationMatrixR*lq;
			}
			else
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
				VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
				VectorXd nf = *localOrientationMatrixR*lf;

				bool reset = false;

				if (j == 0 && ifreset[i] == true)
				{
					reset = true;
				}

				VectorXd reducednf = subspaceModes->transpose()*nf;

				bool converged = this->convergeIntegrationNonlinearReduced(reducedIntergrator, nf, 1000, reset);

				nq = nonLinearIntegrator->getVectorq();

				if (converged == false)
				{
					std::cout << "break" << std::endl;
					break;
				}
			}

			if (dataCount % 2 == 1)
			{
				dataCount++;
				continue;
			}

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
			forcefieldDirection.push_back(potentialDirection[i]);

			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());
}

void ModalWarpingTrainingModel::method10Twist()
{
	std::vector<VectorXd> qn_list;
	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.0005);

	convergeIntegrationNonLinearBuffer(nonLinearIntegrator, forcefield, 300, qn_list, true);

	delete forcefield;

	int range = 1;
	std::cout << "range => " << range << std::endl;
	for (int j = 0; j < qn_list.size(); j += range)
	{
		std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
		VectorXd nq = qn_list[j];

		if (trainingNonLinearDis.size() > 0)
			if ((nq - trainingNonLinearDis.back()).norm() / trainingNonLinearDis.back().norm() < 1e-3)
			{
				//the diff is too small
				continue;
			}

		VectorXd nf = nonLinearIntegrator->getInteranlForce(nq);

		bool reset = false;
		if (j == 0)
			reset = true;

		convergeIntegrationLocal(modalwarpingintegrator, nf, 1000, reset);
		VectorXd lq = modalwarpingintegrator->getVectorq();

		trainingNonLinearDis.push_back(nq);
		trainingLinearDis.push_back(lq);
		std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
		dataCount++;
	}
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::method11Twist()
{
	std::vector<VectorXd> qn_list;
	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.002);
	convergeIntegrationLocalBuffer(modalwarpingintegrator, forcefield, 200, qn_list, true);

	delete forcefield;

	//qn_list.clear();
	//
	//double maxscale = 8;
	//for (int i = 0; i < 4000; i++)
	//{
	//	double scale = maxscale / 4000 * i;
	//	qn_list.push_back(subspaceModes->col(2)*scale);
	//}

	int range = 1;
	std::cout << "range => " << range << std::endl;
	for (int j = 0; j < qn_list.size(); j += range)
	{
		std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
		VectorXd lq = qn_list[j];

		if (trainingLinearDis.size() > 0)
			if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-4)
			{
				continue;
			}

		VectorXd w = *modalRotationSparseMatrix*lq;
		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
		VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);

		VectorXd nf = *localOrientationMatrixR*lf;

		bool reset = false;
		if (j == 0)
			reset = true;
		reset = false;

		bool converge = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

		VectorXd nq = nonLinearIntegrator->getVectorq();

		if (converge == false)
		{
			break;
		}

		/*if (j % 2 != 0)
		{
			continue;
		}*/

		trainingNonLinearDis.push_back(nq);
		trainingLinearDis.push_back(lq);

		std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
		dataCount++;
	}
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::method12Twist()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	/*generateRotationVectorSample(M_PI/2 , 0, 3,
		M_PI / 2, 0, 3,
		1, 1, 1, nodedisplacement);*/

	generateRotationVectorSample(M_PI, M_PI, 1,
		-M_PI / 2, -M_PI / 2, 1,
		1, 1, 1, nodedisplacement, ifreset, potentialDirection);

	samplePoissonRatio(0.20, 0.20, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);

	//nodedisplacement.clear();
	//nodedisplacement.push_back(Vector3d(1, 0, 0));

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.001);
	forcefield->centeraxis_position = this->getPotentialCenter();

	std::cout << "nodedisplacement.size()" << nodedisplacement.size() << std::endl;

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		forcefield->centeraxis = nodedisplacement[i].normalized();
		std::cout << forcefield->centeraxis.transpose() << std::endl;

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		double preNorm = DBL_MAX;
		double residual = DBL_MAX;

		int r = volumtrciMesh->getNumVertices() * 3;
		int maxIteration = 200;

		VectorXd preq(r);
		VectorXd extForce(r);
		VectorXd nq(r);
		nq.setZero();
		extForce.setZero();
		preq.setConstant(1000);

		bool converged = false;
		std::cout << std::endl;

		modalwarpingintegrator->resetToRest();
		for (int j = 0; j < maxIteration; j++)
		{
			bool reset = false;
			if (j == 0)
				reset = true;

			std::cout << '\r';
			std::cout << j;

			volumtrciMesh->setDisplacement(nq.data());
			forcefield->computeCurExternalForce(extForce);

			VectorXd mergedExternalForce = extForce;
			modalwarpingintegrator->setExternalForces(mergedExternalForce.data());

			modalwarpingintegrator->doTimeStep();

			VectorXd curq = modalwarpingintegrator->getVectorq();

			residual = std::abs((preq - curq).norm() / preq.norm());
			preq = curq;

			VectorXd w = *modalRotationSparseMatrix*curq;

			double maxangle = getMaxRotatedAngleFromW(w);
			std::cout << "  maxangle => " << maxangle << "  ";

			/*if (maxangle > M_PI)
			{
				break;
			}*/

			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
			VectorXd lf = modalwarpingintegrator->getInteranlForce(curq);
			VectorXd nf = *localOrientationMatrixR*lf;

			//bool converge = true;

			bool converge = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

			nq = nonLinearIntegrator->getVectorq();

			if (converge == false)
			{
				std::cout << "not coverged" << " " << std::endl;
				break;
			}

			/*if (j % 2 != 0)
			{
			continue;
			}*/

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(curq);
			forcefieldDirection.push_back(forcefield->centeraxis);
			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << "         " << trainingNonLinearDis.size() << " ";

			if (residual < 1e-5)
			{
				converged = true;
				std::cout << "converged" << std::endl;
				break;
			}
		}
	}

	std::cout << std::endl;
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::methodFortwist()
{
	VectorXd baselq = subspaceModes->col(2);
	for (int i = -50; i < 50; i++)
	{
		VectorXd lq = baselq*i*0.1;
		trainingNonLinearDis.push_back(lq);
		trainingLinearDis.push_back(lq);
	}

	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);

}

bool ModalWarpingTrainingModel::convergeIntegrationLocal(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset)
{
	if (reset)
	integrator_loc->resetToRest();

	/*modalwarpingintegrator->computeStaticDisplacement(extForce, false);

	return true;*/

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);

	bool converged = false;

	for (int j = 0; j < maxIteration; j++)
	{
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-6)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationLocalBuffer(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);

	bool converged = false;
	qn_list.clear();
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationLocalBuffer(LoboIntegrator* integrator_loc, RotateForceField* forcefiled, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;

	int r = volumtrciMesh->getNumVertices() * 3;

	VectorXd preq(r);
	VectorXd extForce(r);
	extForce.setZero();
	preq.setConstant(1000);

	bool converged = false;
	qn_list.clear();
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd w = *modalRotationSparseMatrix*fullq;
		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);

		VectorXd nq = (*localOrientationMatrixR)*fullq;
		volumtrciMesh->setDisplacement(nq.data());
		forcefiled->computeCurExternalForce(extForce);

		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();

		if (residual < 1e-5)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationNonLinear(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset)
{
	if (reset)
		integrator_loc->resetToRest();

	bool converge = ((ImplicitNewMatkSparseIntegrator*)integrator_loc)->computeStaticDisplacement(extForce, false);

	return converge;

	std::cout << "convergeIntegrationNonLinear === >" << std::endl;

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);
	std::cout << std::endl;

	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;

	return true;
}

bool ModalWarpingTrainingModel::convergeIntegrationNonlinearReduced(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	bool converge = ((ImpicitNewMarkDenseIntegrator*)integrator_loc)->computeStaticDisplacement(extForce, false);

	return converge;
}

void ModalWarpingTrainingModel::convergeIntegrationNonLinearBuffer(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	qn_list.clear();

	if (reset)
		integrator_loc->resetToRest();

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;
	std::cout << residual << std::endl;
}

void ModalWarpingTrainingModel::convergeIntegrationNonLinearBuffer(LoboIntegrator* integrator_loc, RotateForceField* forcefiled, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	qn_list.clear();

	if (reset)
		integrator_loc->resetToRest();

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;

	int r = volumtrciMesh->getNumVertices() * 3;

	VectorXd preq(r);
	VectorXd extForce(r);

	preq.setConstant(1000);
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;

		VectorXd iq = integrator_loc->getVectorq();
		volumtrciMesh->setDisplacement(iq.data());
		forcefiled->computeCurExternalForce(extForce);

		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();

		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = curq;
		if (residual < 1e-4)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;
	std::cout << residual << std::endl;
}

typedef Eigen::Triplet<double> EIGEN_TRI;

void ModalWarpingTrainingModel::generateConstrainMatrixAndVector(SparseMatrix<double>* constrainMatrix, VectorXd &constrainTarget, LoboNodeBase* nodep, int nodeid, VectorXd target_)
{
	int numVertex = this->volumtrciMesh->getNumVertices();
	int totalConstrain = numConstrainedDOFs + nodep->neighbor.size() * 3 +3;

	constrainMatrix->resize(totalConstrain, numVertex * 3);
	constrainTarget.resize(totalConstrain, 1);
	constrainTarget.setZero();
	std::vector<EIGEN_TRI> entrys;

	for (int i = 0; i < numConstrainedDOFs; i++)
	{
		int row = i;
		int col = constrainedDOFs[i];
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = 0;
	}

	for (int i = 0; i < nodep->neighbor.size()*3; i++)
	{
		int row = i + numConstrainedDOFs;
		int col = nodep->neighbor[i/3]*3 + i % 3;
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = target_.data()[i];
	}

	for (int i = 0; i < 3; i++)
	{
		int row = i + numConstrainedDOFs + nodep->neighbor.size() * 3;
		int col = nodeid * 3 + i;
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = target_.data()[nodep->neighbor.size() * 3+i];
	}
	constrainMatrix->setFromTriplets(entrys.begin(), entrys.end());

}

double ModalWarpingTrainingModel::getMaxRotatedAngleFromW(VectorXd &w)
{
	double maxAngle = -DBL_MAX;
	int numVertex = w.rows() / 3;
	for (int i = 0; i < numVertex; i++)
	{
		Vector3d wi;
		wi.data()[0] = w.data()[i * 3 + 0];
		wi.data()[1] = w.data()[i * 3 + 1];
		wi.data()[2] = w.data()[i * 3 + 2];
		double angle = wi.norm();
		if (angle > maxAngle)
		{
			maxAngle = angle;
		}
	}
	return maxAngle;
}

void ModalWarpingTrainingModel::samplePoissonRatio(double maxPoisson, double minPoisson, int maxNumPoisson, int minNumPoisson, std::vector<Vector3d> &nodeforce, std::vector<Vector3d> &forcedirection, std::vector<bool> &ifreset, std::vector<double> &poissonPerdirection)
{
	int num_direction = nodeforce.size();
	poissonPerdirection.clear();

	Vector3d baseline(-1, 0, 0);
	double maxpoisson = maxPoisson;
	double minpoisson = minPoisson;
	int maxnumpoisson = maxNumPoisson;
	int minnumpoisson = minNumPoisson;
	this->poissonPerDis.clear();

	std::vector<Vector3d> finalnodeforce;
	std::vector<Vector3d> finalforcedirection;
	std::vector<bool> finalifreset;

	for (int i = 0; i < num_direction; i++)
	{
		double angle = forcedirection[i].dot(baseline);
		angle = std::acos(angle);
		int  numpoisson = angle / (M_PI / 2.0)*(minnumpoisson - maxnumpoisson) + maxnumpoisson;
		for (int j = 0; j < numpoisson; j++)
		{
			finalnodeforce.push_back(nodeforce[i]);
			finalforcedirection.push_back(forcedirection[i]);
			finalifreset.push_back(ifreset[i]);

			double interval = (maxpoisson - minpoisson) / (numpoisson - 1);
			if (numpoisson == 1)
			{
				interval = (maxpoisson - minpoisson);
			}
			double poisson = interval*j + minpoisson;
			poissonPerdirection.push_back(poisson);
		}
	}

	nodeforce = finalnodeforce;
	forcedirection = finalforcedirection;
	ifreset = finalifreset;
}