/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.

	 Author: 							Jaume Coll-Font
	 Last Modification:		September 6 2017
*/

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>

// Tikhonov specific headers
#include <Core/Algorithms/Legacy/Inverse/TikhonovAlgoAbstractBase.h>
#include <Core/Algorithms/Legacy/Inverse/TikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithStandardTikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTikhonovSVD_impl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTSVD_impl.h>

// Datatypes
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Math/MiscMath.h>
#include <unsupported/Eigen/Splines>

// SCIRun structural
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Core/Logging/LoggerInterface.h>
#include <Core/Logging/Log.h>
#include <Core/Utils/Exception.h>

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::Inverse;

// shared inputs
const AlgorithmInputName TikhonovAlgoAbstractBase::ForwardMatrix("ForwardMatrix");
const AlgorithmInputName TikhonovAlgoAbstractBase::MeasuredPotentials("MeasuredPotentials");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSourceSpace("WeightingInSourceSpace");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSensorSpace("WeightingInSensorSpace");

// Inputs specific from the Tikhonov SVD module
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixU("matrixU");
const AlgorithmInputName TikhonovAlgoAbstractBase::singularValues("singularValues");
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixV("matrixV");

// outputs
const AlgorithmOutputName TikhonovAlgoAbstractBase::InverseSolution("InverseSolution");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegularizationParameter("RegularizationParameter");
const AlgorithmOutputName TikhonovAlgoAbstractBase::LambdaArray("LambdaArray");
const AlgorithmOutputName TikhonovAlgoAbstractBase::Lambda_Index("Lambda_Index");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegInverse("RegInverse");

ALGORITHM_PARAMETER_DEF( Inverse, TikhonovImplementation);
ALGORITHM_PARAMETER_DEF( Inverse, RegularizationMethod);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationChoice);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaFromDirectEntry);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMin);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMax);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaNum);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaResolution);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaSliderValue);
//ALGORITHM_PARAMETER_DEF( Inverse, LambdaCorner);
//ALGORITHM_PARAMETER_DEF( Inverse, LCurveText);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationSolutionSubcase);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationResidualSubcase);

TikhonovAlgoAbstractBase::TikhonovAlgoAbstractBase()
{
	addParameter(Parameters::TikhonovImplementation, std::string("NoMethodSelected") );
	addOption(Parameters::RegularizationMethod, "lcurve", "single|slider|lcurve");
	addParameter(Parameters::regularizationChoice, 0);
	addParameter(Parameters::LambdaFromDirectEntry,1e-6);
	addParameter(Parameters::LambdaMin,1e-6);
	addParameter(Parameters::LambdaMax,1);
	addParameter(Parameters::LambdaNum,200);
	addParameter(Parameters::LambdaResolution,1e-6);
	addParameter(Parameters::LambdaSliderValue,0);
	addParameter(Parameters::regularizationSolutionSubcase,solution_constrained);
	addParameter(Parameters::regularizationResidualSubcase,residual_constrained);
}

////// CHECK IF INPUT MATRICES HAVE THE CORRECT SIZE
bool TikhonovAlgoAbstractBase::checkInputMatrixSizes( const AlgorithmInput & input) const
{
	auto forwardMatrix_ = input.get<Matrix>(ForwardMatrix);
	auto measuredData_ = input.get<Matrix>(MeasuredPotentials);
	auto sourceWeighting_ = input.get<Matrix>(WeightingInSourceSpace);
	auto sensorWeighting_ = input.get<Matrix>(WeightingInSensorSpace);

  const int M = forwardMatrix_->nrows();
  const int N = forwardMatrix_->ncols();

  // check that rows of fwd matrix equal number of measurements
  if ( M != measuredData_->nrows() )
  {
  	THROW_ALGORITHM_INPUT_ERROR("Input matrix dimensions must agree.");
  	return false;
  }

  // check source regularization matrix sizes
  if (sourceWeighting_)
  {
    if( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained )
    {
      // check that the matrix is of appropriate size (equal number of rows as columns in fwd matrix)
      if ( N != sourceWeighting_->ncols() )
      {
    		THROW_ALGORITHM_INPUT_ERROR("Solution Regularization Matrix must have the same number of rows as columns in the Forward Matrix !");
    		return false;
      }
    }
    // otherwise, if the source regularization is provided as the squared version (RR^T)
    else if ( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as columns in fwd matrix)
      if ( ( N != sourceWeighting_->nrows() ) || ( N != sourceWeighting_->ncols() ) )
      {
        THROW_ALGORITHM_INPUT_ERROR("The squared solution Regularization Matrix must have the same number of rows and columns and must be equal to the number of columns in the Forward Matrix !");
	      return false;
      }
    }
  }

  // check measurement regularization matrix sizes
  if (sensorWeighting_)
  {
    if (get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained)
    {
      // check that the matrix is of appropriate size (equal number of rows as rows in fwd matrix)
      if(M != sensorWeighting_->ncols())
      {
        THROW_ALGORITHM_INPUT_ERROR("Data Residual Weighting Matrix must have the same number of rows as the Forward Matrix !");
        return false;
      }
    }
    // otherwise if the source covariance matrix is provided in squared form
    else if  ( get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as rows in fwd matrix)
      if( (M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()) )
      {
        THROW_ALGORITHM_INPUT_ERROR("Squared data Residual Weighting Matrix must have the same number of rows and columns as number of rows in the Forward Matrix !");
        return false;
      }
    }
  }
  return true;
}

AlgorithmOutput TikhonovAlgoAbstractBase::run(const AlgorithmInput & input) const
{
	auto forwardMatrix = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix));
	auto measuredData = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials));
	auto sourceWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace));
	auto sensorWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace));

	auto RegularizationMethod_gotten = getOption(Parameters::RegularizationMethod);
	auto implOption = get(Parameters::TikhonovImplementation).toString();

	// check input MATRICES
	checkInputMatrixSizes( input );

	// Determine specific Tikhonov Implementation
	std::shared_ptr<TikhonovImpl> algoImpl;
	if (implOption == "standardTikhonov")
  {
		int regularizationChoice = get(Parameters::regularizationChoice).toInt();
		int regularizationSolutionSubcase = get(Parameters::regularizationSolutionSubcase).toInt();
		int regularizationResidualSubcase = get(Parameters::regularizationResidualSubcase).toInt();

		algoImpl = std::make_shared<SolveInverseProblemWithStandardTikhonovImpl>( *forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting,
      regularizationChoice, regularizationSolutionSubcase, regularizationResidualSubcase);
	}
	else if (implOption == "TikhonovSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else if (implOption == "TSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else
  {
		THROW_ALGORITHM_PROCESSING_ERROR("Not a valid Tikhonov Implementation selection");
	}

  double lambda = 0;
  int lambda_index = 0;
  AlgorithmOutput output;
  DenseMatrixHandle lambdamatrix;
  //Get Regularization parameter(s) : Lambda
  if ((RegularizationMethod_gotten == "single") || (RegularizationMethod_gotten == "slider"))
  {
    if (RegularizationMethod_gotten == "single")
    {
      // Use single fixed lambda value, entered in UI
      lambda = get(Parameters::LambdaFromDirectEntry).toDouble();
    }
    else if (RegularizationMethod_gotten == "slider")
    {
      // Use single fixed lambda value, select via slider
      lambda = get(Parameters::LambdaSliderValue).toDouble();
    }
    lambdamatrix.reset(new DenseMatrix(1,1,lambda));
  }
  else if (RegularizationMethod_gotten == "lcurve")
  {
    lambda = computeLcurve( *algoImpl, input,  lambdamatrix, lambda_index);
  }
	else
	{
		THROW_ALGORITHM_PROCESSING_ERROR("Lambda selection was never set");
	}

  // compute final inverse solution
	auto solution = algoImpl->computeInverseSolution(lambda, true);

	// Set outputs

	output[InverseSolution] = boost::make_shared<DenseMatrix>(solution);
	output[RegularizationParameter] = boost::make_shared<DenseMatrix>(1, 1, lambda);
  output[LambdaArray] = lambdamatrix;
  output[Lambda_Index]= boost::make_shared<DenseMatrix>(1, 1, lambda_index);

	return output;
}

double TikhonovAlgoAbstractBase::computeLcurve( const SCIRun::Core::Algorithms::Inverse::TikhonovImpl& algoImpl, const AlgorithmInput & input , DenseMatrixHandle& lambdamatrix, int& lambda_index) const
{
	// get inputs
	auto forwardMatrix = input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix);
	auto measuredData = input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials);
	auto sourceWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace);
	auto sensorWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace);
  // define the step size of the lambda vector to be computed  (distance between min and max divided by number of desired lambdas in log scale)
  const int nLambda = get(Parameters::LambdaNum).toInt();
	const double lambdaMin = get(Parameters::LambdaMin).toDouble();
	const double lambdaMax = get(Parameters::LambdaMax).toDouble();
	double lambda = 0;

	// prealocate vector of lambdas and eta and rho
  std::vector<double> rho(nLambda, 0.0);
  std::vector<double> eta(nLambda, 0.0);

  lambdamatrix.reset(new DenseMatrix(nLambda,3,0.0));

  auto lambdaArray = algoImpl.computeLambdaArray( lambdaMin, lambdaMax, nLambda );

  DenseMatrix CAx, Rx;
  DenseMatrix solution;

  lambdaArray[0] = lambdaMin;

  // for all lambdas
  for (int j = 0; j < nLambda; j++)
  {
    solution = algoImpl.computeInverseSolution( lambdaArray[j], false);
    lambdamatrix->put(j,0,lambdaArray[j]);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sourceWeighting)
    {
      if (solution.nrows() == sourceWeighting->ncols()) // check that regularization matrix and solution match sizes
      {
        auto sw = castMatrix::toDense(sourceWeighting);
        Rx = (*sw) * solution;
      }
      else
      {
        BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage(" Solution weighting matrix unexpectedly does not fit to compute the weighted solution norm. "));
      }
    }
    else
      Rx = solution;

    auto forward = castMatrix::toDense(forwardMatrix);
    auto Ax = (*forward) * solution;
    auto measured = castMatrix::toDense(measuredData);
    auto residualSolution = Ax - (*measured);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sensorWeighting)
    {
      auto sw = castMatrix::toDense(sensorWeighting);
      CAx = (*sw) * residualSolution;
    }
    else
      CAx = residualSolution;

    // compute rho and eta. Using Frobenious norm when using matrices
    rho[j] = CAx.norm();
    eta[j] = Rx.norm();
    lambdamatrix->put(j,1,rho[j]);
    lambdamatrix->put(j,2,eta[j]);
  }

  // Find corner in L-curve
  lambda = FindCorner( rho, eta, lambdaArray, nLambda,lambda_index);

	LOG_DEBUG("Lambda: {}", lambda);
  // TODO: update GUI

  return lambda;
}

///// Find Corner, find the maximal curvature which corresponds to the L-curve corner
double TikhonovAlgoAbstractBase::FindCorner( const std::vector<double>& rho, const std::vector<double>& eta, const std::vector<double>& lambdaArray, const int nLambda, int& lambda_index )
{

	DenseColumnMatrix lrho(nLambda);
	DenseColumnMatrix leta(nLambda);

	for (int i = 0; i < nLambda; i++)
	{
		lrho[i] = std::log10(rho[i]);
		leta[i] = std::log10(eta[i]);
	}

	// create L-curve
	DenseMatrix Gamma( 2, lrho.nrows());
	Gamma.row(0) = lrho;
	Gamma.row(1) = leta;//DenseColumnMatrix::LinSpaced(lrho.nrows(),0,1);

	// fit spline and compute curvature
	DenseColumnMatrix kappa = TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( Gamma );

	// select maximum curvature
	kappa.maxCoeff(&lambda_index);

  	return lambdaArray[lambda_index];
}

SCIRun::Core::Datatypes::DenseColumnMatrix TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( SCIRun::Core::Datatypes::DenseMatrix& samplePoints)
{

	// prealoate
	int numSamples = samplePoints.ncols();
	DenseColumnMatrix kappa = DenseMatrix::Zero( numSamples, 1 );

	// typedefs needed for the spline
	typedef Eigen::Spline<double,2> Spline2d;
	typedef Spline2d::KnotVectorType KnotVectorType;
	typedef Spline2d::ControlPointVectorType ControlPointVectorType;

	// fit cubic spline to data points
	ControlPointVectorType points = samplePoints;
	const Spline2d spline = Eigen::SplineFitting<Spline2d>::Interpolate(points,3);

	// determine position of samples along the spline curve
	KnotVectorType chord_lengths; // knot parameters
	Eigen::ChordLengths(points, chord_lengths);

	for (int i=0; i < numSamples; i++)
	{
		// compute derivatives up to 2nd order
		auto dpt = spline.derivatives( chord_lengths(i), 2);

		// compute curvature as:
		//			abs( ( ddrho*ddeta - ddrho*deta ) /  sqrt(  ( deta^2 + drho^2  )^2)  )
		kappa[i] = std::abs( (dpt(0,2) * dpt(1,2) - dpt(0,2) * dpt(1,1)) /  //compute curvature
	                       std::sqrt( std::pow(dpt(1,1)*dpt(1,1)+dpt(0,1)*dpt(0,1) , 3.0)) );

	}

	// return curvature for all points on the Lcurve
	return kappa;
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

// 素因数分解
std::unordered_map<long long, int> factor(long long n) {
	std::unordered_map<long long, int> factors;
	{
		int i = 0;
		while(n % 2 == 0) {
			n /= 2;
			i++;
		}
		if(i != 0) {
			factors[2] = static_cast<long long>(i);
		}
	}
	for(long long i = 3; i * i <= n; i += 2) {
		int j = 0;
		while(n % i == 0) {
			n /= i;
			j++;
		}
		if(j != 0) {
			factors[i] = j;
		}
	}
	if(n != 1) {
		factors[n] = 1;
	}
	return factors;
}

int main() {
	ll n;
	cin >> n;

	unordered_map<ll, int> factors;
	RANGE(i, 1, n + 1) {
		auto f = factor(i);
		EACH(e, f) {
			if(factors[e.first] != 0) {
				chmax(factors[e.first], e.second);
			} else {
				factors[e.first] = e.second;
			}
		}
	}

	ll product = 1;
	EACH(e, factors) {
		REP(i, e.second) { product *= e.first; }
	}
	product += 1;

	cout << product << endl;
	return 0;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_EXPM1_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_EXPM1_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/expm1_kernel.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/logeps.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( expm1_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      using sA0 = bd::scalar_of_t<A0>;
    #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return Nan<A0>();
    #endif
      if((a0 < Logeps<A0>())) return Mone<A0>();
      if((a0 > Maxlog<A0>())) return  Inf<A0>();
      return detail::expm1_kernel<A0, sA0>::expm1(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( expm1_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::expm1(a0);
    }
  };
} } }

#endif


//---------------------------------//
//  This file is part of MuJoCo    //
//  Written by Emo Todorov         //
//  Copyright (C) 2017 Roboti LLC  //
//---------------------------------//

#include "mujoco.h"
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <chrono>
#include <math.h>
#include <time.h>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;
//-------------------------------- macro variables --------------------------------------

// model selection: PENDULUM CARTPOLE CART2POLE CART3POLE SWIMMER3 SWIMMER6 FISH
#define FISH
#define CTRL_LIMITTED false
#define STATE_LINEAR false
#define LOCAL true
#define PI 3.141592653

//-------------------------------- global variables -------------------------------------

// user customized parameters
#if defined(SWIMMER6)
const mjtNum t_step = 0.006;
const mjtNum cal_step = 0.006;
const mjtNum ptb_coef = 0.005;
const int step_max = 1500;
const int32_t roll_max = 1400;
const char* mname = "swimmer6.xml";
const int ctrl_num = 5;
const int NS = 16;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(SWIMMER3)
const mjtNum t_step = 0.005;//0.01
const mjtNum cal_step = 0.005;
const mjtNum ptb_coef = 0.008;
const int step_max = 1600;//800
const int32_t roll_max = 1200;
const char* mname = "swimmer3.xml";
const int ctrl_num = 2;
const int NS = 10;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(FISH)
const mjtNum t_step = 0.001;
const mjtNum cal_step = 0.001;
const mjtNum ptb_coef = 0.005;
const int step_max = 7000;//16000
const int roll_max = 1600;
const char* mname = "fish.xml";
const int ctrl_num = 5;
const int NS = 26;
mjtNum state_nominal[step_max + 1][NS] = { 0.0, 0.0, 0, 0, 0, 0, 1, 0, 0 };
#elif defined(CART2POLE)
const mjtNum t_step = 0.05;
const int32_t roll_max = 20000; //2w
const char* mname = "cart2pole.xml";
const int step_max = 120;
const int ctrl_num = 1;
const int NS = 6;
mjtNum ptb_coef = 0.04;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(CART7POLE)
const mjtNum t_step = 0.1;
const mjtNum ptb_coef = 1;
const int step_max = 100;
const int32_t roll_max = 20000;
const char* mname = "cart7pole.xml";
const int ctrl_num = 1;
const int NS = 16;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(CART3POLE)
const mjtNum t_step = 0.05;
const mjtNum ptb_coef = 0;
const int step_max = 140;
const int32_t roll_max = 20000;
const char* mname = "cart3pole.xml";
const int ctrl_num = 1;
const int NS = 8;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(ACROBOT)
const mjtNum t_step = 0.01;
const mjtNum ptb_coef = 0.02;
const int step_max = 700;
const int32_t roll_max = 20000;
const char* mname = "acrobot.xml";
const int ctrl_num = 1;
const int NS = 4;
mjtNum state_nominal[step_max + 1][NS] = { -PI, 0, 0, 0 };
#elif defined(PENDULUM)
const mjtNum t_step = 0.1;
const mjtNum cal_step = 0.1;
mjtNum ptb_coef = 0.005;
const int step_max = 30;
const int32_t roll_max = 500;
const char* mname = "pendulum.xml";
const int ctrl_num = 1;
const int NS = 2;
mjtNum state_nominal[step_max + 1][NS] = { PI, 0.0 };
#elif defined(CARTPOLE)
const mjtNum t_step = 0.1;
const mjtNum cal_step = 0.1;
mjtNum ptb_coef = 0.003;
const int step_max = 30;
const int32_t roll_max = 600;
const char* mname = "cartpole.xml";
const int ctrl_num = 1;
const int NS = 4;
mjtNum state_nominal[step_max + 1][NS] = { 0, 0, 0, 0.0 };
#endif

// model
mjModel* m = 0;
mjData* d = 0;
char lastfile[1000] = "";
char error[1000];
static int tri_num = 0;
static int32_t trial = 0;
static int index = 0;
static bool cal_flag = true;
const long N = NS + ctrl_num;
const int n_check = 1;
mjtNum sysiderr = 0;
mjtNum delta_x1[12 + ctrl_num] = { 0 };
mjtNum delta_x2[12] = { 0 };
mjtNum mat[step_max][12][12 + ctrl_num] = { 0 };
//mjtNum sum[step_max][NS][NS + ctrl_num] = { 0 };
//mjtNum dx[step_max + 1][NS];
//mjtNum delta_xc[step_max + 1][NS + ctrl_num] = { 0 };
//mjtNum delta_xcs[step_max][NS] = { 0 };
mjtNum res[NS + ctrl_num][NS + ctrl_num], res6[NS + ctrl_num][NS + ctrl_num], res7[NS][NS + ctrl_num];
mjtNum ctrl_nominal[step_max * ctrl_num] = { 0 };
mjtNum ulim = 0;
int stepite = (int)(t_step / cal_step);
char ctrl_buff[20];
char para_buff[20];
char glstr[10];
char *str3, *str;
FILE *fp, *fp1, *fop, *fop1, *fop2;
char mfilename[30];
char kfilename[30];
char dfilename[30];
#if LOCAL == false
char mfilepre[17] = "../../../model/";
char kfilepre[17] = "../../../doc/";
char dfilepre[17] = "../../../data/";
#else
char mfilepre[15] = "";
char kfilepre[15] = "";
char dfilepre[15] = "";
#endif

// timer
double gettm(void)
{
    static chrono::system_clock::time_point _start = chrono::system_clock::now();
    chrono::duration<double> elapsed = chrono::system_clock::now() - _start;
    return elapsed.count();
}

// deallocate and print message
int finish(const char* msg = 0, mjModel* m = 0, mjData* d = 0)
{
    // deallocated everything
    if( d )
        mj_deleteData(d);
    if( m )
        mj_deleteModel(m);
    mj_deactivate();

    // print message
    if( msg )
        printf("%s\n", msg);

    return 0;
}

/* Calculate Determinant of the Matrix */
mjtNum determinant(mjtNum fh[N][N], mjtNum r)
{
	int z, j, i;
	mjtNum m = 1, k, det = 1.0;
	int rr = (int)r;
	mjtNum a[N][N];

	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			a[i][j] = fh[i][j];
		}
	}

	for (z = 0; z<rr - 1; z++)
		for (i = z; i<rr - 1; i++)
		{
			if (a[z][z] == 0)
			{

				for (j = 0; j<rr; j++)
				{
					a[z][j] = a[z][j] + a[i + 1][j];
				}
			}
			if (a[z][z] != 0) {
				k = -a[i + 1][z] / a[z][z];
				for (j = z; j<rr; j++)a[i + 1][j] = k * a[z][j] + a[i + 1][j];
			}
		}

	for (z = 0; z<rr; z++)
	{
		det = det * (a[z][z]);
	}
	return (det);
}

/* Find transpose of matrix */
void transpose(mjtNum num[N][N], mjtNum fac[N][N], mjtNum r)
{
	int i, j, rr = (int)r;
	mjtNum b[N][N], d;

	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			b[i][j] = fac[j][i];
		}
	}
	d = determinant(num, r);
	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			res[i][j] = b[i][j] / d;
		}
	}
}

/* Inverse */
void cofactor(mjtNum num[N][N], mjtNum f)
{
	mjtNum b[N][N] = { 0 }, fac[N][N];
	int p, q, m, n, i, j;
	int ff = (int)f;

	for (q = 0; q < ff; q++)
	{
		for (p = 0; p < ff; p++)
		{
			m = 0;
			n = 0;
			for (i = 0; i < ff; i++)
			{
				for (j = 0; j < ff; j++)
				{
					if (i != q && j != p)
					{
						b[m][n] = num[i][j];
						if (n < (f - 2)) n++;
						else {
							n = 0;
							m++;
						}
					}
				}
			}
			fac[q][p] = pow(mjtNum(-1), q + p) * determinant(b, ff - 1);
		}
	}
	transpose(num, fac, ff);
}

// w x y z to roll pitch yaw
void quat2smpl(double *res, double *quat)
{
	double b0, b1, b2, b3;

	b0 = *quat;
	b1 = *(quat + 1);
	b2 = *(quat + 2);
	b3 = *(quat + 3);

	Quaterniond q = Eigen::Quaterniond(b0, b1, b2, b3); // w x y z
	auto r = q.toRotationMatrix().eulerAngles(0, 1, 2);

	*res = r[0];
	*(res + 1) = r[1];
	*(res + 2) = r[2];
}

// roll pitch yaw to w x y z
void smpl2quat(double *res, double *smpl)
{
	double rx, ry, rz; // roll pitch yaw

	rx = *smpl;
	ry = *(smpl + 1);
	rz = *(smpl + 2);

	if (rx > PI) rx = PI - rx;

	Quaterniond q = AngleAxisd(rx, Vector3d::UnitX())
		* AngleAxisd(ry, Vector3d::UnitY())
		* AngleAxisd(rz, Vector3d::UnitZ());

	Eigen::Vector4d b = q.coeffs(); // x y z w

	*res = b[3];
	*(res + 1) = b[0];
	*(res + 2) = b[1];
	*(res + 3) = b[2];
}

double gaussrand()
{
	static double U, V;
	static int phase = 0;
	double Z;

	if (phase == 0)
	{
		U = rand() / (RAND_MAX + 1.0);
		V = rand() / (RAND_MAX + 1.0);
		if (U == 0) U = 0.00001;
		Z = sqrt(-2.0 * log(U)) * sin(2.0 * PI * V);
	}
	else
	{
		Z = sqrt(-2.0 * log(U)) * cos(2.0 * PI * V);
	}
	phase = 1 - phase;
	return Z;
}

void get_nominal(mjModel* m, mjData* d)
{
	static int index = 0, i, y;
	mjtNum temp[4];
#if STATE_LINEAR == true
	static int step_max = 1;
#endif

	mju_zero(d->qpos, m->nq);
	mju_zero(d->qvel, m->nv);
	for (y = 0; y < NS/2; y++)
	{
		d->qpos[y] = state_nominal[index][2 * y];
	}
	for (y = 0; y < m->nv; y++)
	{
		d->qvel[y] = state_nominal[index][2 * y + 1];
	}
	for (y = 0; y < ctrl_num; y++)
	{
		d->ctrl[y] = ctrl_nominal[index * ctrl_num + y];
	}

	quat2smpl(temp, &(d->qpos[3]));
	for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = temp[k];

	//for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = d->qpos[k + 4];

	while (index < step_max)
	{
		for (i = 0; i < stepite; i++) mj_step(m, d);

		index++;
		for (y = 0; y < 3; y++)
		{
			state_nominal[index][2 * y] = d->qpos[y];
		}

		quat2smpl(temp, &(d->qpos[3]));
		for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = temp[k];

		//for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = d->qpos[k+4];

		for (y = 7; y < m->nq; y++)
		{
			state_nominal[index][2 * (y-1)] = d->qpos[y];
		}
		for (y = 0; y < m->nv; y++)
		{
			state_nominal[index][2 * y + 1] = d->qvel[y];
		}
		for (y = 0; y < ctrl_num; y++)
		{
			d->ctrl[y] = ctrl_nominal[index * ctrl_num + y];
		}
	}
}

void sysidcheck(mjModel* m, mjData* d)
{
	static int index = 0;
	static mjtNum t_init;
	const int step_max = 1500;
	int i, j, k;
	mjtNum dx[n_check][step_max][12];
	mjtNum delta_xc[n_check][step_max][12 + ctrl_num] = { 0 };
	mjtNum delta_xcs[n_check][step_max][12] = { 0 };
	mjtNum temp[4];

	for (i = 0; i < 17; i++)
	{
		for (j = 0; j < n_check; j++)
		{
			for (k = 0; k < step_max; k++) delta_xc[j][k][i] = 0.005 * ulim * gaussrand();
		}
	}
	for (i = 0; i < n_check; i++)
	{
		for (j = 0; j < step_max; j++) mju_mulMatVec(dx[i][j], *mat[j], delta_xc[i][j], 12, 12 + ctrl_num);
	}

	for (i = 0; i < n_check; i++)
	{
		index = 0;
		t_init = d->time;
		for (j = 0; j < 3; j++)
		{
			d->qpos[j] = delta_xc[i][index][2 * j] + state_nominal[index][2 * j];
		}

		//for (k = 0; k < 3; k++) d->qpos[k+4] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
		//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

		for (k = 0; k < 3; k++) temp[k] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
		smpl2quat(&(d->qpos[3]), temp);

		for (j = 7; j < m->nq; j++)
		{
			d->qpos[j] = state_nominal[index][2 * (j-1)];
		}
		//d->qpos[9] = delta_xc[i][index][12] + state_nominal[index][16];
		for (j = 0; j < 6; j++)
		{
			d->qvel[j] = delta_xc[i][index][2 * j + 1] + state_nominal[index][2 * j + 1];
		}
		for (j = 6; j < m->nv; j++)
		{
			d->qvel[j] = state_nominal[index][2 * j + 1];
		}
		//d->qvel[8] = delta_xc[i][index][13] + state_nominal[index][17];
		for (k = 0; k < ctrl_num; k++)
		{
			d->ctrl[k] = delta_xc[i][index][k + 12] + ctrl_nominal[index * ctrl_num + k];
		}
		while (index < step_max - 1)
		{
			if (d->time - t_init < t_step - 0.00001)
			{
				mj_step(m, d);
			}
			else {
				for (int y = 0; y < 3; y++)
				{
					delta_xcs[i][index][2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
				}

				quat2smpl(temp, &(d->qpos[3]));
				for (int k = 0; k < 3; k++) delta_xcs[i][index][2 * k + 6] = temp[k] - state_nominal[index][2 * k + 6];

				//for (int k = 0; k < 3; k++) delta_xcs[i][index][2 * k + 6] = d->qpos[k+4] - state_nominal[index][2 * k + 6];

				//delta_xcs[i][index][12] = d->qpos[9] - state_nominal[index + 1][16];
				for (int y = 0; y < 6; y++)
				{
					delta_xcs[i][index][2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
				}
				//delta_xcs[i][index][13] = d->qvel[8] - state_nominal[index + 1][17];

				index++;
				t_init = d->time;

				for (int y = 0; y < 3; y++)
				{
					d->qpos[y] = delta_xc[i][index][2 * y] + state_nominal[index][2 * y];
				}

				//for (int k = 0; k < 3; k++) d->qpos[k+4] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
				//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

				for (int k = 0; k < 3; k++) temp[k] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
				smpl2quat(&(d->qpos[3]), temp);

				for (j = 7; j < m->nq; j++)
				{
					d->qpos[j] = state_nominal[index][2 * (j - 1)];
				}
				//d->qpos[9] = delta_xc[i][index][12] + state_nominal[index][16];
				for (int y = 0; y < 6; y++)
				{
					d->qvel[y] = delta_xc[i][index][2 * y + 1] + state_nominal[index][2 * y + 1];
				}
				for (int y = 6; y < m->nv; y++)
				{
					d->qvel[y] = state_nominal[index][2 * y + 1];
				}
				//d->qvel[8] = delta_xc[i][index][13] + state_nominal[index][17];
				for (int ci = 0; ci < ctrl_num; ci++)
				{
					d->ctrl[ci] = delta_xc[i][index][ci + 12] + ctrl_nominal[index * ctrl_num + ci];
				}
			}
		}
		for (int t = 0; t < 12; t++)
		{
			for (int h = 0; h < step_max; h++)
			{
				if (delta_xcs[i][h][t] != 0) sysiderr += fabs((dx[i][h][t] - delta_xcs[i][h][t]) / delta_xcs[i][h][t]);
			}
		}
	}
	sysiderr = sysiderr / (1.0*n_check*step_max*12);
}

void check(mjModel* m, mjData* d)
{
//	static int index = 0, h, u, y;
//#if STATE_LINEAR == true
//	static int step_max = 1;
//#endif
//
//	for (u = 0; u < NS + ctrl_num; u++)
//	{
//		for (h = 0; h < step_max; h++) delta_xc[h][u] = ptb_coef * ulim * gaussrand();
//	}
//	for (h = 0; h < step_max; h++)
//	{
//		mju_mulMatVec(dx[h + 1], *mat[h], delta_xc[h], NS, NS + ctrl_num);
//	}
//	while (index < step_max)
//	{
//		for (y = 0; y < m->nq; y++)
//		{
//			d->qpos[y] = delta_xc[index][2 * y] + state_nominal[index][2 * y];
//		}
//		for (y = 0; y < m->nv; y++)
//		{
//			d->qvel[y] = delta_xc[index][2 * y + 1] + state_nominal[index][2 * y + 1];
//		}
//		for (h = 0; h < ctrl_num; h++)
//		{
//			d->ctrl[h] = ctrl_nominal[index * ctrl_num + h] + delta_xc[index][h + NS];
//		}
//
//		for (h = 0; h < stepite; h++) mj_step(m, d);
//
//		for (y = 0; y < m->nq; y++)
//		{
//			delta_xcs[index][2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
//		}
//		for (y = 0; y < m->nv; y++)
//		{
//			delta_xcs[index][2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
//		}
//		index++;
//	}
}

void setcontrol(mjModel* m, mjData* d)
{
	mjtNum temp[4];
	static int index = 0, i, y;
#if STATE_LINEAR == true
	static int step_max = 1;
#endif

	while (index < step_max)
	{
		for (y = 0; y < 3; y++)
		{
			delta_x1[2 * y] = ptb_coef * ulim * gaussrand();
			d->qpos[y] = delta_x1[2 * y] + state_nominal[index][2 * y];
		}
		for (int k = 0; k < 3; k++) {
			delta_x1[2 * k + 6] = ptb_coef * ulim * gaussrand();

			//d->qpos[k+4] = delta_x1[2 * k + 6] + state_nominal[index][2 * k + 6];

			temp[k] = delta_x1[2 * k + 6] + state_nominal[index][2 * k + 6];
		}
		//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

		smpl2quat(&(d->qpos[3]), temp);
		for (int j = 7; j < m->nq; j++)
		{
			d->qpos[j] = state_nominal[index][2 * (j - 1)];
		}
		//delta_x1[12] = ptb_coef * ulim * gaussrand();
		//d->qpos[9] = delta_x1[12] + state_nominal[index][16];
		for (y = 0; y < 6; y++)
		{
			delta_x1[2 * y + 1] = ptb_coef * ulim * gaussrand();
			d->qvel[y] = delta_x1[2 * y + 1]+state_nominal[index][2 * y + 1];
		}
		for (y = 6; y < m->nv; y++)
		{
			d->qvel[y] = state_nominal[index][2 * y + 1];
		}
		//delta_x1[13] = ptb_coef * ulim * gaussrand();
		//d->qvel[8] = delta_x1[13] + state_nominal[index][17];
		for (i = 0; i < ctrl_num; i++)
		{
			delta_x1[i + 12] = ptb_coef * ulim * gaussrand();
			d->ctrl[i] = ctrl_nominal[index * ctrl_num + i] + delta_x1[i + 12];
		}

		for (i = 0; i < stepite; i++) mj_step(m, d);

		for (int y = 0; y < 3; y++)
		{
			delta_x2[2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
		}

		quat2smpl(temp, &(d->qpos[3]));
		for (int k = 0; k < 3; k++) delta_x2[2 * k + 6] = temp[k] - state_nominal[index][2 * k + 6];

		//for (int k = 0; k < 3; k++) delta_x2[2 * k + 6] = d->qpos[k+4] - state_nominal[index][2 * k + 6];

		//delta_x2[12] = d->qpos[9] - state_nominal[index + 1][16];
		for (int y = 0; y < 6; y++)
		{
			delta_x2[2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
		}
		//delta_x2[13] = d->qvel[8] - state_nominal[index + 1][17];
		// iteration method
		for (y = 0; y < 12; y++)
		{
			for (i = 0; i < 17; i++)
			{
				mat[index][y][i] = mat[index][y][i] * (1 - 1 / (trial + 1.0)) + delta_x2[y] * delta_x1[i] / ((trial + 1.0) * ptb_coef * ptb_coef * ulim * ulim);
			}
		}

		if (index == 0)
		{
			str3 = glstr;
			sprintf(str3, "%3.3f", mat[index][0][1]);
			fwrite(str3, 5, 1, fop);
			fputs(" ", fop);
		}
		index++;
	}
	index = 0;
	trial++;
}

// main function
int main(int argc, const char** argv)
{
    // activate MuJoCo Pro license (this must be *your* activation key)
	strcpy(kfilename, kfilepre);
	strcat(kfilename, "mjkeysmall.txt");
    mj_activate(kfilename);

    // get filename, determine file type
    std::string filename(mname);
    bool binary = (filename.find(".mjb")!=std::string::npos);
	strcpy(mfilename, mfilepre);
	strcat(mfilename, mname);

    // load model
    char error[1000] = "Could not load binary model";
    if( binary )
        m = mj_loadModel(mfilename, 0);
    else
        m = mj_loadXML(mfilename, 0, error, 1000);
    if( !m )
        return finish(error);

    // make data
    d = mj_makeData(m);
    if( !d )
        return finish("Could not allocate mjData", m);

	if (STATE_LINEAR == true)
	{
		#if defined(CART2POLE)||defined(CART3POLE)||defined(CART7POLE)
				state_nominal[0][2] = PI;
		#elif defined(PENDULUM)
				state_nominal[0][0] = 0;
		#elif defined(CARTPOLE)
				state_nominal[0][2] = -PI;
		#elif defined(ACROBOT)
				state_nominal[0][0] = 0;
				state_nominal[0][2] = -2 * PI;
		#endif
	}

	srand((unsigned)time(NULL));
	strcpy(dfilename, dfilepre);
	strcat(dfilename, "converge.txt");
	if ((fop = fopen(dfilename, "wt+")) == NULL) {
		return 0;
	}
	strcpy(dfilename, dfilepre);
	strcat(dfilename, "result.txt");
	if ((fp = fopen(dfilename, "r")) != NULL)
	{
		fscanf(fp, "%s", ctrl_buff);
		if (ctrl_buff[0] == 'C') {
			for (int i = 0; i < step_max * ctrl_num; i++)
			{
				fscanf(fp, "%s", ctrl_buff);
				ctrl_nominal[i] = atof(ctrl_buff);
				if (fabs(ctrl_nominal[i]) > ulim) ulim = fabs(ctrl_nominal[i]);

				#if CTRL_LIMITTED == true
					if (ctrl_nominal[i] > m->actuator_ctrlrange[1]) ctrl_nominal[i] = m->actuator_ctrlrange[1];
					else if (ctrl_nominal[i] < m->actuator_ctrlrange[0]) ctrl_nominal[i] = m->actuator_ctrlrange[0];
				#endif
				#if STATE_LINEAR == true
					ctrl_nominal[i] = 0;
				#endif
			}
			fclose(fp);
		}
	}

    // time simulation
    int steps = 0, contacts = 0, constraints = 0;
    double printfraction = 0.1;
    printf("\nSimulation ");
    double start = gettm();
	get_nominal(m, d);
	while (trial < roll_max)
    {
		setcontrol(m, d);

        // accumulate statistics
        steps = steps + stepite;
        contacts += d->ncon;
        constraints += d->nefc;

        // print '.' every 10% of duration
        if( trial >= roll_max*printfraction )
        {
            printf(".");
            printfraction += 0.1;
        }
    }
    double end = gettm();

    // print results
    printf("\n Simulation time      : %.2f s\n", end-start);
    printf(" Time per step        : %.3f ms\n", 1000.0*(end-start)/mjMAX(1,steps));
    printf(" Contacts per step    : %d\n", contacts/mjMAX(1,steps));
    printf(" Constraints per step : %d\n", constraints/mjMAX(1,steps));
    printf(" Degrees of freedom   : %d\n\n", m->nv);

	sysidcheck(m, d);

	// print result
	#if STATE_LINEAR == true
		strcpy(dfilename, dfilepre);
		strcat(dfilename, "linearization_top.txt");
		if ((fop2 = fopen(dfilename, "wt+")) != NULL)
		{
			str3 = glstr;
			for (int t = 0; t < 1; t++)
			{
				for (int h = 0; h < NS; h++)
				{
					for (int d = 0; d < NS + ctrl_num; d++)
					{
						sprintf(str3, "%4.8f", mat[t][h][d]);
						fwrite(str3, 10, 1, fop2);
						fputs(" ", fop2);
					}
					fputs("\n", fop2);
				}
				fputs("\n", fop2);
			}

			// for result checking
			for (int t = 0; t < NS; t++)
			{
				for (int h = 1; h <= 1; h++)
				{
					sprintf(str3, "%2.4f", dx[h][t]);
					fwrite(str3, 6, 1, fop2);
					fputs(" ", fop2);
				}
				fputs("\n", fop2);
				for (int d = 0; d < 1; d++)
				{
					sprintf(str3, "%2.4f", delta_xcs[d][t]);
					fwrite(str3, 6, 1, fop2);
					fputs(" ", fop2);
				}
				fputs("\n", fop2);
				fputs("\n", fop2);
			}
			fputs("\nptb_coef: ", fop2);
			sprintf(str3, "%2.4f\n", ptb_coef);
			fwrite(str3, 6, 1, fop2);
			fclose(fop2);
		}
	#else
		strcpy(dfilename, dfilepre);
		strcat(dfilename, "lnr.txt");
		if ((fop2 = fopen(dfilename, "wt+")) != NULL)
		{
			str3 = glstr;
			for (int t = 0; t < step_max; t++)
			{
				for (int h = 0; h < 12; h++)
				{
					for (int d = 0; d < 12 + ctrl_num; d++)
					{
						sprintf(str3, "%4.8f", mat[t][h][d]);
						fwrite(str3, 10, 1, fop2);
						fputs(" ", fop2);
					}
					fputs("\n", fop2);
				}
				fputs("\n", fop2);
			}

			//// for result checking
			//for (int t = 0; t < NS; t++)
			//{
			//	for (int h = 1; h <= step_max; h++)
			//	{
			//		sprintf(str3, "%2.4f", dx[h][t]);
			//		fwrite(str3, 6, 1, fop2);
			//		fputs(" ", fop2);
			//	}
			//	fputs("\n", fop2);
			//	for (int d = 0; d < step_max; d++)
			//	{
			//		sprintf(str3, "%2.4f", delta_xcs[d][t]);
			//		fwrite(str3, 6, 1, fop2);
			//		fputs(" ", fop2);
			//	}
			//	fputs("\n", fop2);
			//	fputs("\n", fop2);
			//}
			fputs("sysiderr: ", fop2);
			sprintf(str3, "%2.4f\n", sysiderr);
			fwrite(str3, 6, 1, fop2);
			fputs("\nptb_coef: ", fop2);
			sprintf(str3, "%2.4f\n", ptb_coef);
			fwrite(str3, 6, 1, fop2);

			fclose(fop2);
		}

		//// state output
		//strcpy(dfilename, dfilepre);
		//strcat(dfilename, "states.txt");
		//if ((fp1 = fopen(dfilename, "at+")) != NULL)
		//{
		//	str3 = glstr;
		//	for (int h = 0; h < NS; h++)
		//	{
		//		for (int d = 1; d < step_max; d++)
		//		{
		//			sprintf(str3, "%4.8f", state_nominal[d][h]);
		//			fwrite(str3, 10, 1, fp1);
		//			fputs(" ", fp1);
		//		}
		//		fputs("\n", fp1);
		//	}
		//	fclose(fp1);
		//}
	#endif
	// hold cmd
	system("pause");
    // finalize
    return finish(0, m, d);
}

backup/some old code and slides/sample/sysidpartial.cpp


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//
// @file This module implements two binding commitment schemes used in the Groth16
// aggregation.
// The first one is a commitment scheme that commits to a single vector $a$ of
// length n in the second base group $G_1$ (for example):
// * it requires a structured SRS $v_1$ of the form $(h,h^u,h^{u^2}, ...
// ,g^{h^{n-1}})$ with $h \in G_2$ being a random generator of $G_2$ and $u$ a
// random scalar (coming from a power of tau ceremony for example)
// * it requires a second structured SRS $v_2$ of the form $(h,h^v,h^{v^2},
// ...$ with $v$ being a random scalar different than u (coming from another
// power of tau ceremony for example)
// The Commitment is a tuple $(\prod_{i=0}^{n-1} e(a_i,v_{1,i}),
// \prod_{i=0}^{n-1} e(a_i,v_{2,i}))$
//
// The second one takes two vectors $a \in G_1^n$ and $b \in G_2^n$ and commits
// to them using a similar approach as above. It requires an additional SRS
// though:
// * $v_1$ and $v_2$ stay the same
// * An additional tuple $w_1 = (g^{u^n},g^{u^{n+1}},...g^{u^{2n-1}})$ and $w_2 =
// (g^{v^n},g^{v^{n+1},...,g^{v^{2n-1}})$ where $g$ is a random generator of
// $G_1$
// The commitment scheme returns a tuple:
// * $\prod_{i=0}^{n-1} e(a_i,v_{1,i})e(w_{1,i},b_i)$
// * $\prod_{i=0}^{n-1} e(a_i,v_{2,i})e(w_{2,i},b_i)$
//
// The second commitment scheme enables to save some KZG verification in the
// verifier of the Groth16 verification protocol since we pack two vectors in
// one commitment.

#ifndef CRYPTO3_R1CS_GG_PPZKSNARK_AGGREGATE_IPP2_COMMITMENT_HPP
#define CRYPTO3_R1CS_GG_PPZKSNARK_AGGREGATE_IPP2_COMMITMENT_HPP

#include <tuple>
#include <vector>
#include <type_traits>

#include <boost/assert.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/accumulators/accumulators.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

#include <nil/crypto3/algebra/algorithms/pair.hpp>

namespace nil {
    namespace crypto3 {
        namespace zk {
            namespace snark {
                /// Both commitment outputs a pair of $F_q^k$ element.
                template<typename CurveType>
                using r1cs_gg_ppzksnark_ipp2_commitment_output =
                    std::pair<typename CurveType::gt_type::value_type, typename CurveType::gt_type::value_type>;

                /// Key is a generic commitment key that is instantiated with g and h as basis,
                /// and a and b as powers.
                template<typename GroupType>
                struct r1cs_gg_ppzksnark_ipp2_commitment_key {
                    typedef GroupType group_type;
                    typedef typename group_type::curve_type curve_type;
                    typedef typename curve_type::scalar_field_type field_type;

                    typedef typename group_type::value_type group_value_type;
                    typedef typename field_type::value_type field_value_type;

                    /// Exponent is a
                    std::vector<group_value_type> a;
                    /// Exponent is b
                    std::vector<group_value_type> b;

                    /// Returns true if commitment keys have the exact required length.
                    /// It is necessary for the IPP scheme to work that commitment
                    /// key have the exact same number of arguments as the number of proofs to
                    /// aggregate.
                    inline bool has_correct_len(std::size_t n) const {
                        return a.size() == n && n == b.size();
                    }

                    /// Returns both vectors scaled by the given vector entrywise.
                    /// In other words, it returns $\{v_i^{s_i}\}$
                    template<
                        typename InputIterator,
                        typename ValueType = typename std::iterator_traits<InputIterator>::value_type,
                        typename std::enable_if<std::is_same<field_value_type, ValueType>::value, bool>::type = true>
                    r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> scale(InputIterator s_first,
                                                                            InputIterator s_last) const {
                        BOOST_ASSERT(has_correct_len(std::distance(s_first, s_last)));

                        r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> result;
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(s_first, a.begin(), b.begin())),
                                      boost::make_zip_iterator(boost::make_tuple(s_last, a.end(), b.end())),
                                      [&](const boost::tuple<const field_value_type &, const group_value_type &,
                                                             const group_value_type &> &t) {
                                          result.a.emplace_back(t.template get<1>() * t.template get<0>());
                                          result.b.emplace_back(t.template get<2>() * t.template get<0>());
                                      });

                        return result;
                    }

                    /// Returns the left and right commitment key part. It makes copy.
                    std::pair<r1cs_gg_ppzksnark_ipp2_commitment_key<group_type>,
                              r1cs_gg_ppzksnark_ipp2_commitment_key<group_type>>
                        split(std::size_t at) const {
                        BOOST_ASSERT(a.size() == b.size());
                        BOOST_ASSERT(at > 0 && at < a.size());

                        r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> result_l;
                        r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> result_r;

                        auto a_it = a.begin();
                        auto b_it = b.begin();
                        while (a_it != a.begin() + at && b_it != b.begin() + at) {
                            result_l.a.emplace_back(*a_it);
                            result_l.b.emplace_back(*b_it);
                            ++a_it;
                            ++b_it;
                        }
                        while (a_it != a.end() && b_it != b.end()) {
                            result_r.a.emplace_back(*a_it);
                            result_r.b.emplace_back(*b_it);
                            ++a_it;
                            ++b_it;
                        }

                        return std::make_pair(result_l, result_r);
                    }

                    /// Takes a left and right commitment key and returns a commitment
                    /// key $left \circ right^{scale} = (left_i*right_i^{scale} ...)$. This is
                    /// required step during GIPA recursion.
                    r1cs_gg_ppzksnark_ipp2_commitment_key<group_type>
                        compress(const r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> &right,
                                 const field_value_type &scale) const {
                        BOOST_ASSERT(a.size() == right.a.size());
                        BOOST_ASSERT(b.size() == right.b.size());
                        BOOST_ASSERT(a.size() == b.size());

                        r1cs_gg_ppzksnark_ipp2_commitment_key<group_type> result;

                        std::for_each(
                            boost::make_zip_iterator(
                                boost::make_tuple(a.begin(), b.begin(), right.a.begin(), right.b.begin())),
                            boost::make_zip_iterator(boost::make_tuple(a.end(), b.end(), right.a.end(), right.b.end())),
                            [&](const boost::tuple<const group_value_type &, const group_value_type &,
                                                   const group_value_type &, const group_value_type &> &t) {
                                result.a.emplace_back(t.template get<0>() + t.template get<2>() * scale);
                                result.b.emplace_back(t.template get<1>() + t.template get<3>() * scale);
                            });

                        return result;
                    }

                    /// Returns the first values in the vector of v1 and v2 (respectively
                    /// w1 and w2). When commitment key is of size one, it's a proxy to get the
                    /// final values.
                    std::pair<group_value_type, group_value_type> first() const {
                        return std::make_pair(a.front(), b.front());
                    }
                };

                /// Commitment key used by the "single" commitment on G1 values as
                /// well as in the "pair" commitment.
                /// It contains $\{h^a^i\}_{i=1}^n$ and $\{h^b^i\}_{i=1}^n$
                template<typename CurveType>
                using r1cs_gg_ppzksnark_ipp2_vkey = r1cs_gg_ppzksnark_ipp2_commitment_key<typename CurveType::g2_type>;

                /// Commitment key used by the "pair" commitment. Note the sequence of
                /// powers starts at $n$ already.
                /// It contains $\{g^{a^{n+i}}\}_{i=1}^n$ and $\{g^{b^{n+i}}\}_{i=1}^n$
                template<typename CurveType>
                using r1cs_gg_ppzksnark_ipp2_wkey = r1cs_gg_ppzksnark_ipp2_commitment_key<typename CurveType::g1_type>;

                template<typename CurveType>
                struct r1cs_gg_ppzksnark_ipp2_commitment {
                    typedef CurveType curve_type;
                    typedef typename curve_type::pairing pairing;

                    typedef r1cs_gg_ppzksnark_ipp2_wkey<CurveType> wkey_type;
                    typedef r1cs_gg_ppzksnark_ipp2_vkey<CurveType> vkey_type;

                    typedef typename wkey_type::group_value_type g1_value_type;
                    typedef typename vkey_type::group_value_type g2_value_type;
                    typedef typename curve_type::gt_type::value_type gt_value_type;

                    typedef r1cs_gg_ppzksnark_ipp2_commitment_output<CurveType> output_type;

                    /// Commits to a tuple of G1 vector and G2 vector in the following way:
                    /// $T = \prod_{i=0}^n e(A_i, v_{1,i})e(B_i,w_{1,i})$
                    /// $U = \prod_{i=0}^n e(A_i, v_{2,i})e(B_i,w_{2,i})$
                    /// Output is $(T,U)$
                    template<typename InputG1Iterator, typename InputG2Iterator,
                             typename ValueType1 = typename std::iterator_traits<InputG1Iterator>::value_type,
                             typename ValueType2 = typename std::iterator_traits<InputG2Iterator>::value_type,
                             typename std::enable_if<std::is_same<g1_value_type, ValueType1>::value, bool>::type = true,
                             typename std::enable_if<std::is_same<g2_value_type, ValueType2>::value, bool>::type = true>
                    static output_type pair(const vkey_type &vkey, const wkey_type &wkey, InputG1Iterator a_first,
                                            InputG1Iterator a_last, InputG2Iterator b_first, InputG2Iterator b_last) {
                        BOOST_ASSERT(vkey.has_correct_len(std::distance(a_first, a_last)));
                        BOOST_ASSERT(wkey.has_correct_len(std::distance(b_first, b_last)));
                        BOOST_ASSERT(std::distance(a_first, a_last) == std::distance(b_first, b_last));

                        // (A * v)
                        gt_value_type t1 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(a_first, vkey.a.begin())),
                                      boost::make_zip_iterator(boost::make_tuple(a_last, vkey.a.end())),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          t1 = t1 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        // (B * v)
                        gt_value_type t2 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(wkey.a.begin(), b_first)),
                                      boost::make_zip_iterator(boost::make_tuple(wkey.a.end(), b_last)),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          t2 = t2 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        gt_value_type u1 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(a_first, vkey.b.begin())),
                                      boost::make_zip_iterator(boost::make_tuple(a_last, vkey.b.end())),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          u1 = u1 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        gt_value_type u2 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(wkey.b.begin(), b_first)),
                                      boost::make_zip_iterator(boost::make_tuple(wkey.b.end(), b_last)),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          u2 = u2 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        // (A * v)(w * B)
                        return std::make_pair(algebra::final_exponentiation<curve_type>(t1 * t2),
                                              algebra::final_exponentiation<curve_type>(u1 * u2));
                    }

                    /// Commits to a single vector of G1 elements in the following way:
                    /// $T = \prod_{i=0}^n e(A_i, v_{1,i})$
                    /// $U = \prod_{i=0}^n e(A_i, v_{2,i})$
                    /// Output is $(T,U)$
                    template<typename InputG1Iterator,
                             typename ValueType1 = typename std::iterator_traits<InputG1Iterator>::value_type,
                             typename std::enable_if<std::is_same<g1_value_type, ValueType1>::value, bool>::type = true>
                    static output_type single(const vkey_type &vkey, InputG1Iterator a_first, InputG1Iterator a_last) {
                        BOOST_ASSERT(vkey.has_correct_len(std::distance(a_first, a_last)));

                        gt_value_type t1 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(a_first, vkey.a.begin())),
                                      boost::make_zip_iterator(boost::make_tuple(a_last, vkey.a.end())),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          t1 = t1 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        gt_value_type u1 = gt_value_type::one();
                        std::for_each(boost::make_zip_iterator(boost::make_tuple(a_first, vkey.b.begin())),
                                      boost::make_zip_iterator(boost::make_tuple(a_last, vkey.b.end())),
                                      [&](const boost::tuple<const g1_value_type &, const g2_value_type &> &t) {
                                          u1 = u1 * algebra::pair<curve_type>(t.template get<0>(), t.template get<1>());
                                      });

                        return std::make_pair(algebra::final_exponentiation<curve_type>(t1),
                                              algebra::final_exponentiation<curve_type>(u1));
                    }
                };
            }    // namespace snark
        }        // namespace zk
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_R1CS_GG_PPZKSNARK_AGGREGATE_IPP2_COMMITMENT_HPP


// ---------------------------------------------------------------------
//
// Copyright (c) 2018 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibtk/FECache.h"
#include "ibtk/QuadratureCache.h"
#include "ibtk/libmesh_utilities.h"

#include "tbox/Utilities.h"

#if LIBMESH_VERSION_LESS_THAN(1, 2, 0)
#include "libmesh/mesh_tools.h"
#else
#include "libmesh/bounding_box.h"
#endif
#include "libmesh/dof_map.h"
#include "libmesh/elem.h"
#include "libmesh/enum_elem_type.h"
#include "libmesh/enum_order.h"
#include "libmesh/enum_quadrature_type.h"
#include "libmesh/explicit_system.h"
#include "libmesh/fem_context.h"
#include "libmesh/id_types.h"
#include "libmesh/libmesh_config.h"
#include "libmesh/mesh_base.h"
#include "libmesh/node.h"
#include "libmesh/numeric_vector.h"
#include "libmesh/parallel.h"
#include "libmesh/petsc_vector.h"
#include "libmesh/point.h"
#include "libmesh/quadrature.h"
#include "libmesh/system.h"
#include "libmesh/type_vector.h"
#include "libmesh/variant_filter_iterator.h"

#include <petscsys.h>

IBTK_DISABLE_EXTRA_WARNINGS
#include <boost/multi_array.hpp>
IBTK_ENABLE_EXTRA_WARNINGS

#include <mpi.h>

#include <algorithm>
#include <cmath>
#include <fstream>
#include <limits>
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <vector>

/////////////////////////////// NAMESPACE ////////////////////////////////////

namespace IBTK
{
/////////////////////////////// STATIC ///////////////////////////////////////

/////////////////////////////// PUBLIC ///////////////////////////////////////
void
setup_system_vectors(libMesh::EquationSystems* equation_systems,
                     const std::vector<std::string>& system_names,
                     const std::vector<std::string>& vector_names,
                     const bool from_restart)
{
    for (const std::string& system_name : system_names)
    {
        TBOX_ASSERT(equation_systems->has_system(system_name));
        libMesh::System& system = equation_systems->get_system(system_name);
        for (const std::string& vector_name : vector_names)
        {
            setup_system_vector(system, vector_name, from_restart);
            if (vector_name == "RHS Vector")
            {
                auto* explicit_system = dynamic_cast<libMesh::ExplicitSystem*>(&system);
                if (!explicit_system)
                {
                    TBOX_ERROR(
                        "You are attempting to add a RHS vector to a libMesh system that does not have one (i.e., it "
                        "does not inherit from ExplicitSystem).");
                }
                else
                {
                    explicit_system->rhs = &system.get_vector("RHS Vector");
                }
            }
        }
    }
}

void
setup_system_vector(libMesh::System& system, const std::string& vector_name, const bool from_restart)
{
    std::unique_ptr<libMesh::NumericVector<double> > clone_vector;
    if (from_restart)
    {
        libMesh::NumericVector<double>* current = system.request_vector(vector_name);
        if (current != nullptr)
        {
            clone_vector = current->clone();
        }
    }
    system.remove_vector(vector_name);
    system.add_vector(vector_name, /*projections*/ true, /*type*/ libMesh::GHOSTED);

    if (clone_vector != nullptr)
    {
        const auto& parallel_vector = dynamic_cast<const libMesh::PetscVector<double>&>(*clone_vector);
        auto& ghosted_vector = dynamic_cast<libMesh::PetscVector<double>&>(system.get_vector(vector_name));
        TBOX_ASSERT(parallel_vector.size() == ghosted_vector.size());
        TBOX_ASSERT(parallel_vector.local_size() == ghosted_vector.local_size());
        ghosted_vector = parallel_vector;
        ghosted_vector.close();
    }
}

void
apply_transposed_constraint_matrix(const libMesh::DofMap& dof_map, libMesh::PetscVector<double>& rhs)
{
    std::vector<libMesh::dof_id_type> dofs;
    std::vector<double> values_to_add;
    // loop over constraints and do the action of C^T b
    for (auto it = dof_map.constraint_rows_begin(); it != dof_map.constraint_rows_end(); ++it)
    {
        const std::pair<libMesh::dof_id_type, libMesh::DofConstraintRow>& constraint = *it;
        const libMesh::dof_id_type constrained_dof = constraint.first;
        // only resolve constraints if the DoF is locally owned
        if (dof_map.first_dof() <= constrained_dof && constrained_dof < dof_map.end_dof())
        {
            for (const std::pair<const libMesh::dof_id_type, double>& pair : constraint.second)
            {
                dofs.push_back(pair.first);
                values_to_add.push_back(pair.second * rhs(constrained_dof));
            }
        }
    }

    // If we use ghosted RHSs then we cannot use VecSetValues because we are
    // using a replacement for VecStash. Hence do some somewhat ugly
    // transformations in that case to sum into ghost regions:
    if (rhs.type() == libMesh::GHOSTED)
    {
        // Calling operator() above puts the vector in read-only mode, which
        // has to be restored before we can write to it
        rhs.restore_array();
        // At this point rhs contains the full set of ghost data (which we
        // needed for constraint resolution). However, we now want to reuse
        // the ghost region to sum the values from constraint resolution onto
        // their owning processes. Hence we must clear the ghost data and then
        // do another parallel reduction.
        const PetscInt local_size_without_ghosts = rhs.local_size();
        Vec loc_vec = nullptr;
        // we also need the size of the ghost region, which is surprisingly
        // difficult to get
        int ierr = VecGhostGetLocalForm(rhs.vec(), &loc_vec);
        IBTK_CHKERRQ(ierr);
        PetscInt local_size_with_ghosts = -1;
        ierr = VecGetSize(loc_vec, &local_size_with_ghosts);
        IBTK_CHKERRQ(ierr);
        ierr = VecGhostRestoreLocalForm(rhs.vec(), &loc_vec);
        IBTK_CHKERRQ(ierr);

        double* const vec_array = rhs.get_array();
        std::fill(vec_array + local_size_without_ghosts, vec_array + local_size_with_ghosts, 0.0);
        for (std::size_t i = 0; i < dofs.size(); ++i)
        {
            const PetscInt index = rhs.map_global_to_local_index(dofs[i]);
            vec_array[index] += values_to_add[i];
        }
        rhs.restore_array();
    }
    else
    {
        rhs.add_vector(values_to_add.data(), dofs);
    }
}

quadrature_key_type
getQuadratureKey(const libMesh::QuadratureType quad_type,
                 libMesh::Order order,
                 const bool use_adaptive_quadrature,
                 const double point_density,
                 const bool allow_rules_with_negative_weights,
                 const libMesh::Elem* const elem,
                 const boost::multi_array<double, 2>& X_node,
                 const double dx_min)
{
    const libMesh::ElemType elem_type = elem->type();
#ifndef NDEBUG
    TBOX_ASSERT(elem->p_level() == 0); // higher levels are not implemented
#endif
    if (use_adaptive_quadrature)
    {
        const double hmax = get_max_edge_length(elem, X_node);
        int npts = int(std::ceil(point_density * hmax / dx_min));
        if (npts < 3)
        {
            if (elem->default_order() == libMesh::FIRST)
                npts = 2;
            else
                npts = 3;
        }
        switch (quad_type)
        {
        case libMesh::QGAUSS:
            order = static_cast<libMesh::Order>(std::min(2 * npts - 1, static_cast<int>(libMesh::FORTYTHIRD)));
            break;
        case libMesh::QGRID:
            order = static_cast<libMesh::Order>(npts);
            break;
        default:
            TBOX_ERROR("IBTK::getQuadratureKey():\n"
                       << "  adaptive quadrature rules are available only for quad_type = QGAUSS "
                          "or QGRID\n");
        }
    }

    return std::make_tuple(elem_type, quad_type, order, allow_rules_with_negative_weights);
}

void
write_elem_partitioning(const std::string& file_name, const libMesh::System& position_system)
{
    const int current_rank = position_system.comm().rank();
    const unsigned int position_system_n = position_system.number();
    const libMesh::NumericVector<double>& local_position = *position_system.solution.get();
    const libMesh::MeshBase& mesh = position_system.get_mesh();
    const unsigned int spacedim = mesh.spatial_dimension();
    // TODO: there is something wrong with the way we set up the ghost data in
    // the position vectors: not all locally owned nodes are, in fact, locally
    // available. Get around this by localizing first. Since all processes
    // write to the same file this isn't the worst bottleneck in this
    // function, anyway.
    std::vector<double> position(local_position.size());
    local_position.localize(position);
    std::stringstream current_processor_output;

    const auto end_elem = mesh.local_elements_end();
    for (auto elem = mesh.local_elements_begin(); elem != end_elem; ++elem)
    {
        const unsigned int n_nodes = (*elem)->n_nodes();
        libMesh::Point center;
        // TODO: this is a bit crude: if we use isoparametric elements (e.g.,
        // Tri6) then this is not a very accurate representation of the center
        // of the element. We should replace this with something more accurate.
        for (unsigned int node_n = 0; node_n < n_nodes; ++node_n)
        {
            const libMesh::Node& node = (*elem)->node_ref(node_n);
            TBOX_ASSERT(node.n_vars(position_system_n) == spacedim);
            for (unsigned int d = 0; d < spacedim; ++d)
            {
                center(d) += position[node.dof_number(position_system_n, d, 0)];
            }
        }
        center *= 1.0 / n_nodes;

        for (unsigned int d = 0; d < spacedim; ++d)
        {
            current_processor_output << center(d) << ',';
        }
        if (spacedim == 2)
        {
            current_processor_output << 0.0 << ',';
        }
        current_processor_output << current_rank << '\n';
    }

    // clear the file before we append to it
    if (current_rank == 0)
    {
        std::remove(file_name.c_str());
    }
    const int n_processes = position_system.comm().size();
    for (int rank = 0; rank < n_processes; ++rank)
    {
        if (rank == current_rank)
        {
            std::ofstream out(file_name, std::ios_base::app);
            if (rank == 0)
            {
                out << "x,y,z,r\n";
            }
            out << current_processor_output.rdbuf();
        }
        position_system.comm().barrier();
    }
}

void
write_node_partitioning(const std::string& file_name, const libMesh::System& position_system)
{
    const int current_rank = position_system.comm().rank();
    const unsigned int position_system_n = position_system.number();
    const libMesh::NumericVector<double>& local_position = *position_system.solution.get();
    const libMesh::MeshBase& mesh = position_system.get_mesh();
    const unsigned int spacedim = mesh.spatial_dimension();

    // TODO: there is something wrong with the way we set up the ghost data in
    // the position vectors: not all locally owned nodes are, in fact,
    // locally available. Get around this by localizing first. Since all
    // processes write to the same file this isn't the worst bottleneck in
    // this function, anyway.
    std::vector<double> position(local_position.size());
    local_position.localize(position);
    std::stringstream current_processor_output;

    const auto end_node = mesh.local_nodes_end();
    for (auto node_it = mesh.local_nodes_begin(); node_it != end_node; ++node_it)
    {
        const libMesh::Node* const node = *node_it;
        if (node->n_vars(position_system_n))
        {
            TBOX_ASSERT(node->n_vars(position_system_n) == spacedim);
            for (unsigned int d = 0; d < spacedim; ++d)
            {
                current_processor_output << position[node->dof_number(position_system_n, d, 0)] << ',';
            }
            if (spacedim == 2)
            {
                current_processor_output << 0.0 << ',';
            }
            current_processor_output << current_rank << '\n';
        }
    }

    // clear the file before we append to it
    if (current_rank == 0)
    {
        std::remove(file_name.c_str());
    }
    const int n_processes = position_system.comm().size();
    for (int rank = 0; rank < n_processes; ++rank)
    {
        if (rank == current_rank)
        {
            std::ofstream out(file_name, std::ios_base::app);
            if (rank == 0)
            {
                out << "x,y,z,r\n";
            }
            out << current_processor_output.rdbuf();
        }
        position_system.comm().barrier();
    }
}

std::vector<libMeshWrappers::BoundingBox>
get_local_element_bounding_boxes(const libMesh::MeshBase& mesh,
                                 const libMesh::System& X_system,
                                 const libMesh::QuadratureType quad_type,
                                 const libMesh::Order quad_order,
                                 const bool use_adaptive_quadrature,
                                 const double point_density,
                                 bool allow_rules_with_negative_weights,
                                 const double patch_dx_min)
{
    const unsigned int dim = mesh.mesh_dimension();
    const unsigned int spacedim = mesh.spatial_dimension();
    TBOX_ASSERT(spacedim == NDIM);
    const unsigned int X_sys_num = X_system.number();
    auto X_ghost_vec_ptr = X_system.current_local_solution->zero_clone();
    auto& X_ghost_vec = dynamic_cast<libMesh::PetscVector<double>&>(*X_ghost_vec_ptr);
    X_ghost_vec = *X_system.solution;

    std::vector<libMeshWrappers::BoundingBox> bboxes;

    std::vector<std::vector<libMesh::dof_id_type> > dof_indices(NDIM);
    boost::multi_array<double, 2> X_node;
    QuadratureCache quad_cache(dim);
    FECache fe_cache(dim, X_system.get_dof_map().variable_type(0), update_phi);
    using quad_key_type = quadrature_key_type;
    const auto el_begin = mesh.local_elements_begin();
    const auto el_end = mesh.local_elements_end();
    for (auto el_it = el_begin; el_it != el_end; ++el_it)
    {
        // 0. Set up bounding box
        bboxes.emplace_back();
        auto& box = bboxes.back();
        libMesh::Point& lower_bound = box.first;
        libMesh::Point& upper_bound = box.second;
        for (unsigned int d = 0; d < LIBMESH_DIM; ++d)
        {
            lower_bound(d) = std::numeric_limits<double>::max();
            upper_bound(d) = -std::numeric_limits<double>::max();
        }

        // As mentioned in the documentation of this function we do not set up
        // bounding boxes for inactive elements
        if (!(*el_it)->active()) continue;

        // 1. extract node locations
        const libMesh::Elem* const elem = *el_it;
        const unsigned int n_nodes = elem->n_nodes();
        for (unsigned int d = 0; d < NDIM; ++d) dof_indices[d].clear();

        for (unsigned int k = 0; k < n_nodes; ++k)
        {
            const libMesh::Node* const node = elem->node_ptr(k);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                TBOX_ASSERT(node->n_dofs(X_sys_num, d) == 1);
                dof_indices[d].push_back(node->dof_number(X_sys_num, d, 0));
            }
        }
        get_values_for_interpolation(X_node, X_ghost_vec, dof_indices);

        // 2. compute mapped quadrature points in the deformed configuration
        const quad_key_type key = getQuadratureKey(quad_type,
                                                   quad_order,
                                                   use_adaptive_quadrature,
                                                   point_density,
                                                   allow_rules_with_negative_weights,
                                                   elem,
                                                   X_node,
                                                   patch_dx_min);
        libMesh::QBase& quadrature = quad_cache[key];
        libMesh::FEBase& fe = fe_cache(key, elem);
        const std::vector<std::vector<double> >& phi_X = fe.get_phi();
        const std::vector<libMesh::Point>& q_points = quadrature.get_points();
        for (unsigned int qp = 0; qp < q_points.size(); ++qp)
        {
            libMesh::Point mapped_point;
            for (unsigned int k = 0; k < n_nodes; ++k)
            {
                for (unsigned int d = 0; d < NDIM; ++d)
                {
                    mapped_point(d) += phi_X[k][qp] * X_node[k][d];
                }
            }
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                lower_bound(d) = std::min(lower_bound(d), mapped_point(d));
                upper_bound(d) = std::max(upper_bound(d), mapped_point(d));
            }

            // fill extra dimension with 0.0, which is libMesh's convention
            for (unsigned int d = NDIM; d < LIBMESH_DIM; ++d)
            {
                lower_bound(d) = 0.0;
                upper_bound(d) = 0.0;
            }
        }
    }

    return bboxes;
} // get_local_element_bounding_boxes

std::vector<libMeshWrappers::BoundingBox>
get_local_element_bounding_boxes(const libMesh::MeshBase& mesh, const libMesh::System& X_system)
{
    const unsigned int spacedim = mesh.spatial_dimension();
    TBOX_ASSERT(spacedim == NDIM);
    const unsigned int X_sys_num = X_system.number();
    auto X_ghost_vec_ptr = X_system.current_local_solution->zero_clone();
    auto& X_ghost_vec = dynamic_cast<libMesh::PetscVector<double>&>(*X_ghost_vec_ptr);
    X_ghost_vec = *X_system.solution;

    std::vector<libMeshWrappers::BoundingBox> bboxes;
    bboxes.reserve(mesh.n_local_elem());

    std::vector<libMesh::dof_id_type> dof_indices;
    std::vector<double> X_node;
    const auto el_begin = mesh.local_elements_begin();
    const auto el_end = mesh.local_elements_end();
    for (auto el_it = el_begin; el_it != el_end; ++el_it)
    {
        const libMesh::Elem* const elem = *el_it;
        const unsigned int n_nodes = elem->n_nodes();
        bboxes.emplace_back();
        auto& box = bboxes.back();
        libMesh::Point& lower_bound = box.first;
        libMesh::Point& upper_bound = box.second;
        for (unsigned int d = 0; d < LIBMESH_DIM; ++d)
        {
            lower_bound(d) = std::numeric_limits<double>::max();
            upper_bound(d) = -std::numeric_limits<double>::max();
        }

        // As mentioned in the documentation of this function we do not set up
        // bounding boxes for inactive elements
        if (!(*el_it)->active()) continue;

        dof_indices.clear();
        for (unsigned int k = 0; k < n_nodes; ++k)
        {
            const libMesh::Node* const node = elem->node_ptr(k);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                TBOX_ASSERT(node->n_dofs(X_sys_num, d) == 1);
                dof_indices.push_back(node->dof_number(X_sys_num, d, 0));
            }
        }

        X_node.resize(dof_indices.size());
        X_ghost_vec.get(dof_indices, X_node.data());
        for (unsigned int k = 0; k < n_nodes; ++k)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const double& X = X_node[k * NDIM + d];
                lower_bound(d) = std::min(lower_bound(d), X);
                upper_bound(d) = std::max(upper_bound(d), X);
            }
        }

        // fill extra dimension with 0.0, which is libMesh's convention
        for (unsigned int d = NDIM; d < LIBMESH_DIM; ++d)
        {
            lower_bound(d) = 0.0;
            upper_bound(d) = 0.0;
        }
    }

    return bboxes;
} // get_local_element_bounding_boxes

std::vector<libMeshWrappers::BoundingBox>
get_global_element_bounding_boxes(const libMesh::MeshBase& mesh,
                                  const std::vector<libMeshWrappers::BoundingBox>& local_bboxes)
{
    const std::size_t n_elem = mesh.n_elem();
    std::vector<double> flattened_bboxes(2 * LIBMESH_DIM * n_elem);
    std::size_t elem_n = 0;
    const auto el_begin = mesh.local_elements_begin();
    const auto el_end = mesh.local_elements_end();
    for (auto el_it = el_begin; el_it != el_end; ++el_it)
    {
        const auto id = (*el_it)->id();
        TBOX_ASSERT((2 * id + 2) * LIBMESH_DIM - 1 < flattened_bboxes.size());
        for (unsigned int d = 0; d < LIBMESH_DIM; ++d)
        {
            flattened_bboxes[2 * id * LIBMESH_DIM + d] = local_bboxes[elem_n].first(d);
            flattened_bboxes[(2 * id + 1) * LIBMESH_DIM + d] = local_bboxes[elem_n].second(d);
        }
        ++elem_n;
    }
    const int ierr = MPI_Allreduce(
        MPI_IN_PLACE, flattened_bboxes.data(), flattened_bboxes.size(), MPI_DOUBLE, MPI_SUM, mesh.comm().get());
    TBOX_ASSERT(ierr == 0);

    std::vector<libMeshWrappers::BoundingBox> global_bboxes(n_elem);
    for (unsigned int e = 0; e < n_elem; ++e)
    {
        for (unsigned int d = 0; d < LIBMESH_DIM; ++d)
        {
            global_bboxes[e].first(d) = flattened_bboxes[2 * e * LIBMESH_DIM + d];
            global_bboxes[e].second(d) = flattened_bboxes[(2 * e + 1) * LIBMESH_DIM + d];
        }
    }
    return global_bboxes;
} // get_local_element_bounding_boxes

std::vector<libMeshWrappers::BoundingBox>
get_global_element_bounding_boxes(const libMesh::MeshBase& mesh, const libMesh::System& X_system)
{
    static_assert(NDIM <= LIBMESH_DIM,
                  "NDIM should be no more than LIBMESH_DIM for this function to "
                  "work correctly.");
    return get_global_element_bounding_boxes(mesh, get_local_element_bounding_boxes(mesh, X_system));
} // get_global_element_bounding_boxes
//////////////////////////////////////////////////////////////////////////////

} // namespace IBTK

//////////////////////////////////////////////////////////////////////////////