#pragma once

#include <Eigen/Core>

#include <memory>
#include <utility>
#include <vector>

namespace eq {
    const int INIT_TIME       = 0;
    const double MAX_MIN_DIFF = 10e-10;
    const double DEFAULT_STEP = 0.1;

    const double TASK_DIFF_STEP = 0.001;
    const double MAX_TIME_RUNGE = 0.2;
    const double MAX_TIME_DP    = 10;

    struct task_answer {
        double answer_max_diff_runge{-1};
        double answer_step_runge{-1};
        double answer_min_step_dp{-1};
        int answer_total_steps_dp{-1};
    };

    class EquationSolver;

    class Solver {
    public:
        virtual ~Solver() = default;
        explicit Solver(EquationSolver &eq_solver, double step) : _solver(eq_solver), _step(step) {}

        Solver() = delete;
        Solver(const Solver&) = delete;
        void operator=(const Solver&) = delete;

        virtual void Calculate() = 0;
        virtual void Solver_set_matrix() = 0;

        Eigen::MatrixXd Generate_mat();

        double Get_step() const { return _step; }
        void Set_step(double step) {_step = step; }

    protected:
        EquationSolver &_solver;
        double _step;
    };

    class RungeSolver : public Solver {
    public:
        explicit RungeSolver(EquationSolver &eq_solver, double step = DEFAULT_STEP) : Solver(eq_solver, step) {}

        RungeSolver() = delete;
        RungeSolver(const RungeSolver&) = delete;
        void operator=(const RungeSolver&) = delete;

        void Calculate() override;
        void Solver_set_matrix() override;

    private:
        void calc_step();
    };

    class DPSolver : public Solver {
    public:
        explicit DPSolver(EquationSolver &eq_solver, double max_diff = MAX_MIN_DIFF, double min_diff = MAX_MIN_DIFF,
                          double step = DEFAULT_STEP) :
                          Solver(eq_solver, step), _max_diff(max_diff), _min_diff(min_diff) {}

        DPSolver() = delete;
        DPSolver(const DPSolver&) = delete;
        void operator=(const DPSolver&) = delete;

        void Calculate() override;
        void Solver_set_matrix() override;

    private:
        void calc_step();

    private:
        double _max_diff;
        double _min_diff;
    };

    class EquationSolver {
    public:
        explicit EquationSolver(std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> recalc_function,
                        Eigen::VectorXd start_vals, double start_time, std::function<Eigen::VectorXd(double)> result_function) :
                        _recalc_function(std::move(recalc_function)), _values(std::move(start_vals)), _time(start_time),
                        _result_function(std::move(result_function)) {}

        EquationSolver() = delete;
        EquationSolver(const EquationSolver&) = delete;
        void operator=(const EquationSolver&) = delete;

        void Set_solver(std::unique_ptr<Solver>&& solver);
        void Reload_values(const Eigen::VectorXd &init_vals, double init_time);

        void Solve();
        task_answer Get_answer() const;

        double Get_time() const { return _time; }
        Eigen::VectorXd& Get_values() { return _values; }

    public:
        friend Solver;
        friend RungeSolver;
        friend DPSolver;

    private:
        std::unique_ptr<Solver> _solver;

        std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> _recalc_function;
        Eigen::VectorXd _values;
        Eigen::VectorXd _steps;
        std::function<Eigen::VectorXd(double)> _result_function;

        Eigen::MatrixXd _a;
        std::vector<Eigen::MatrixXd> _b;

        double _time{0};

    private:
        double _answer_max_diff_runge{-1};
        double _answer_step_runge{-1};
        double _answer_min_step_dp{-1};
        int _answer_total_steps_dp{-1};
    };

    task_answer Calculate(std::function<Eigen::VectorXd(double, const Eigen::VectorXd &)> src_function,
                   std::function<Eigen::VectorXd(double)> result_function, const Eigen::VectorXd& init_val);
}


#include <iostream>
#include <iomanip>
#include <algorithm>
#include <functional>
#include <limits>
#include <ctime>
#include <cmath>
#include <cassert>

#include <boost/timer.hpp>
#include <boost/random.hpp>

#include "StochasticFunctionMinimization.h"

namespace Grante {

double StochasticFunctionMinimization::StochasticSubgradientMethodMinimize(
	StochasticFunctionMinimizationProblem& prob, std::vector<double>& x_opt,
	double conv_tol, unsigned int max_epochs, bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	// Random instance generator
	size_t N = prob.NumberOfElements();
	assert(N > 0);
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_int<unsigned int> rdestd(0, static_cast<unsigned int>(N-1));
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_n(rgen, rdestd);

	// Optimize a given number of epochs
	boost::timer total_timer;
	std::vector<double> avg_grad(dim, 0.0);
	double lambda = 1.0;	// (should be lambda=1/C)
	double avg_obj = 0.0;
	for (unsigned int epoch = 0; max_epochs == 0 || epoch < max_epochs;
		++epoch) {
		avg_obj = 0.0;
		std::fill(avg_grad.begin(), avg_grad.end(), 0.0);

		// Choose epoch-wide step size
		double alpha = 1.0 / (static_cast<double>(epoch + 1) * lambda);

		// Optimize by sampling instances
		for (size_t n = 0; n < N; ++n) {
			unsigned int id = rand_n();
			// Update average objective and averaged gradient of this epoch
			avg_obj += prob.Eval(id, x, grad);
			std::transform(grad.begin(), grad.end(), avg_grad.begin(),
				avg_grad.begin(), std::plus<double>());

			// Perform incremental subgradient update
			for (unsigned int d = 0; d < dim; ++d)
				x[d] -= alpha * grad[d];
		}
		// Compute mean gradient and estimated objective
		double avg_grad_norm = 0.0;
		for (unsigned int d = 0; d < dim; ++d)
			avg_grad_norm += avg_grad[d]*avg_grad[d];
		avg_grad_norm = std::sqrt(avg_grad_norm);

		// Output statistics
		if (verbose && (epoch % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time        avg_obj  |avg_grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << epoch << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << avg_obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << avg_grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Convergence check
		if (avg_grad_norm < conv_tol)
			break;
	}

	x_opt = x;
	return (avg_obj);	// This is not exact, but stochastic anyway
}

}


/*
 * Copyright (C) 2021, unclearness
 * All rights reserved.
 */

#include "ugu/optimizer/optimizer.h"

#include <Eigen/LU>
#include <iostream>

namespace {

using UpdateFunc = std::function<void(const ugu::OptimizerInput& in,
                                      ugu::OptimizerOutput& out)>;

double LineSearchBacktracking(const ugu::GradVec& update_direc,
                              const ugu::OptimizerInput& in,
                              ugu::OptimizerOutput& out) {
  // BACK_TRACKING
  double a = in.line_search_max;
  double r = 0.5;
  while (true) {
    // Armijo rule
    double c1 = 1e-4;
    auto new_param = out.best_param + a * update_direc;
    double new_val = in.loss_func(new_param);

    double constraint = out.best + c1 * a * out.best_grad.dot(update_direc);

    if (new_val <= constraint) {
      return a;
    }

    a *= r;
  }
}

double LineSearch(const ugu::GradVec& update_direc,
                  const ugu::OptimizerInput& in, ugu::OptimizerOutput& out) {
  if (in.line_search_method == ugu::LineSearchMethod::BACK_TRACKING) {
    return LineSearchBacktracking(update_direc, in, out);
  }

  throw std::invalid_argument("This type is not implemented");

  //return 0.0;
}

void LoopBody(const ugu::OptimizerInput& input, ugu::OptimizerOutput& output,
              UpdateFunc update_func) {
  output.Clear();
  output.best_param = input.init_param;
  output.best = input.loss_func(output.best_param);
  output.best_grad = input.grad_func(output.best_param);

  output.val_history.push_back(output.best);
  output.param_history.push_back(output.best_param);
  output.grad_history.push_back(output.best_grad);

  double prev = output.best;
  double diff = std::numeric_limits<double>::max();
  size_t max_size = static_cast<size_t>(input.LBFGS_memoery_num);
  while (!input.terminate_criteria.isTerminated(output.best_iter, diff)) {
    prev = output.best;

    update_func(input, output);

    output.best = input.loss_func(output.best_param);
    output.best_grad = input.grad_func(output.best_param);

    output.val_history.push_back(output.best);
    output.param_history.push_back(output.best_param);
    output.grad_history.push_back(output.best_grad);
    while (max_size < output.val_history.size()) {
      output.val_history.erase(output.val_history.begin());
      output.param_history.erase(output.param_history.begin());
      output.grad_history.erase(output.grad_history.begin());
    }

    // output.best = input.loss_func(output.best_param);
    diff = prev - output.best;
    output.best_iter++;
    // std::cout << prev << std::endl;
  }
}

void GradientDescentUdpateFunc(const ugu::OptimizerInput& in,
                               ugu::OptimizerOutput& out) {
  // Eval grad
  auto grad = in.grad_func(out.best_param);

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(-grad, in, out);
  }

  out.best_param += (lr * -grad);
}

// http://www.dais.is.tohoku.ac.jp/~shioura/teaching/mp13/mp13-13.pdf
void NewtonUpdateFunc(const ugu::OptimizerInput& in,
                      ugu::OptimizerOutput& out) {
  ugu::GradVec grad = in.grad_func(out.best_param);
  // out.best_grad = grad;
  ugu::Hessian hessian = in.hessian_func(out.best_param);
  ugu::OptParams delta = (-hessian.inverse() * grad);

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(delta, in, out);
  }

  out.best_param += (lr * delta);
}

// https://en.wikipedia.org/wiki/Limited-memory_BFGS
// https://abicky.net/2010/06/22/114613/
void LBFGSUpdateFunc(const ugu::OptimizerInput& in, ugu::OptimizerOutput& out) {
  ugu::GradVec grad = in.grad_func(out.best_param);
  // out.best_grad = grad;
  ugu::GradVec q = grad;

  if (out.grad_history.size() < 2) {
    // Initialize by GradientDescent
    GradientDescentUdpateFunc(in, out);
    return;
  }

  ugu::OptIndex k = static_cast<ugu::OptIndex>(out.grad_history.size() - 1);
  std::vector<ugu::GradVec> y_list;
  std::vector<ugu::GradVec> s_list;
  std::vector<double> rho_list;

  for (ugu::OptIndex i = k; i >= 1; i--) {
    ugu::GradVec y = out.grad_history[i] - out.grad_history[i - 1];
    ugu::GradVec s = out.param_history[i] - out.param_history[i - 1];
    double dot = y.dot(s);

#if 0
    // Guard
    if (std::abs(dot) < 1e-10) {
      if (dot > 0) {
        dot = 1e-10;
      } else {
        dot = -1e-10;
      }
    }
    if (std::abs(dot) < 1e10) {
      if (dot > 0) {
        dot = 1e10;
      } else {
        dot = -1e10;
      }
    }
#endif

    double rho = 1.0 / dot;
#if 0
    if (std::isnan(rho)) {
        rho = 0;
    }
    std::cout << y << std::endl;
    std::cout << s << std::endl;
    std::cout << rho << std::endl;
    std::cout << std::endl;

    if (std::abs(rho) < 1e-10) {
      if (rho > 0) {
        rho = 1e-10;
      } else {
        rho = -1e-10;
      }
    }
#endif
    y_list.push_back(y);
    s_list.push_back(s);
    rho_list.push_back(rho);
  }

  std::vector<double> alpha_list;

  for (ugu::OptIndex i = 0; i < k; i++) {
    double alpha = rho_list[i] * s_list[i].dot(q);
    q = q - alpha * y_list[i];
    alpha_list.push_back(alpha);
  }

  double gamma = s_list[0].dot(y_list[0]) / y_list[0].dot(y_list[0]);
  Eigen::SparseMatrix<double> I(in.init_param.rows(), in.init_param.rows());
  I.setIdentity();
  Eigen::SparseMatrix<double> H = gamma * I;

  // std::cout << H << std::endl;

  ugu::GradVec z = H * q;

  for (ugu::OptIndex i = k - 1; i >= 0; i--) {
    double beta = rho_list[i] * y_list[i].dot(z);
    z = z + s_list[i] * (alpha_list[i] - beta);
  }

  z = -z;

  double lr = in.lr;

  if (in.use_line_search) {
    lr = LineSearch(z, in, out);
  }

  out.best_param += (lr * z);
}

}  // namespace

namespace ugu {

GradFunc GenNumericalGrad(LossFunc loss_func, double h) {
  return [loss_func, h](const OptParams& params) {
    GradVec grad(params.size());
    const double inv_2h = 1.0 / (2 * h);
    for (OptIndex i = 0; i < params.size(); i++) {
      auto params_plus = params;
      params_plus[i] += h;
      auto params_minus = params;
      params_minus[i] -= h;
      grad[i] = (loss_func(params_plus) - loss_func(params_minus)) * inv_2h;
    }
    return grad;
  };
}

HessianFunc GenNumericalHessian(LossFunc loss_func, double h) {
  return [loss_func, h](const OptParams& params) {
    Hessian hessian(params.rows(), params.rows());
    const double inv_2h = 1.0 / (2 * h);
    for (OptIndex i = 0; i < params.size(); i++) {
      auto params_i_p = params;
      params_i_p[i] += h;
      auto params_i_n = params;
      params_i_n[i] -= h;
      for (OptIndex j = 0; j < params.size(); j++) {
        auto params_i_p_j_p = params_i_p;
        params_i_p_j_p[j] += h;
        auto params_i_p_j_n = params_i_p;
        params_i_p_j_n[j] -= h;

        auto grad_ij_p =
            (loss_func(params_i_p_j_p) - loss_func(params_i_p_j_n)) * inv_2h;

        auto params_i_n_j_p = params_i_n;
        params_i_n_j_p[j] += h;
        auto params_i_n_j_n = params_i_n;
        params_i_n_j_n[j] -= h;

        auto grad_ij_n =
            (loss_func(params_i_n_j_p) - loss_func(params_i_n_j_n)) * inv_2h;

        hessian(i, j) = (grad_ij_p - grad_ij_n) * inv_2h;
        // std::cout << grad_ij_p << " " << grad_ij_n << " " << hessian(i, j)
        //          << std::endl;
      }
    }
    return hessian;
  };
}

void GradientDescent(const OptimizerInput& input, OptimizerOutput& output) {
  LoopBody(input, output, GradientDescentUdpateFunc);
}

void Newton(const OptimizerInput& input, OptimizerOutput& output) {
  LoopBody(input, output, NewtonUpdateFunc);
}

void QuasiNewton(const OptimizerInput& input, OptimizerOutput& output,
                 QuasiNewtonMethod method) {
  if (method == QuasiNewtonMethod::LBFGS) {
    LoopBody(input, output, LBFGSUpdateFunc);
  } else {
    throw std::invalid_argument("This method is not supported");
  }
}

}  // namespace ugu


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/lagrangespace.hh"
#include "fem/hierarchicspace.hh"
#include "linalg/direct.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "utilities/enums.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare, createUnitCube
#include "io/vtk.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"

#include "mg/pcg.hh"
#include "mg/apcg.hh"

#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "poisson.hh"

bool compareAbs(const double x1, const double  x2)
{
  return fabs(x1)>fabs(x2);
}

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

  std::cout << "Start heat transfer tutorial program using HB error estimation (SpaceDimension=3)"
            << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int refinements   = getParameter(pt, "refinements", 2),
      order         = getParameter(pt, "order", 1),
      maxAdaptSteps = getParameter(pt, "maxAdaptSteps", 2),
      verbosity     = getParameter(pt, "verbosity", 1);
  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "max. adaptive refine steps     : " << maxAdaptSteps << std::endl;
  std::cout << "output level (verbosity)       : " << verbosity << std::endl << std::endl;

  int  direct, onlyLowerTriangle = false;

  DirectType directType;
  IterateType iterateType = IterateType::PCG;
  PrecondType precondType = PrecondType::NONE;
  MatrixProperties property;
  std::string empty, geoFile("cube.am");

  geoFile = getParameter(pt, "file", geoFile);

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  std::cout << "Solver type is " << (direct ? "direct" : "iterative") << std::endl;

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 2));

  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || ( (precondType == PrecondType::ICC) && !direct ) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl << std::endl;
  }

  //   three-dimensional space: dim=3
  constexpr int dim=3;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = PoissonFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using H1ExSpace = FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,LeafView> >;
  using H1ExSpaces = boost::fusion::vector<H1Space const*,H1ExSpace const*>;
  using ExVariableDescriptions = boost::fusion::vector<VariableDescription<1,1,0> >;
  using ExVariableSet = VariableSetDescription<H1ExSpaces,ExVariableDescriptions>;
  using ErrorEstimator = HierarchicErrorEstimator<LinearizationAt<Functional>,ExVariableSet>;
  using EstAssembler = VariationalFunctionalAssembler<ErrorEstimator>;
  using ExCoefficientVectors = ExVariableSet::CoefficientVectorRepresentation<0,1>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using AssOperator = AssembledGalerkinOperator<Assembler,0,1,0,1>;
  using AssEstOperator = AssembledGalerkinOperator<EstAssembler>;

  GridManager<Grid> gridManager( createUnitCube<Grid>(0.5) );

  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " tetrahedra, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim-1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim) << " points" << std::endl;
  // a gridmanager is constructed
  // as connector between geometric and algebraic information
  gridManager.setVerbosity(verbosity);

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[1] = { "T" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;

  Assembler assembler(gridManager,spaces);

  H1ExSpace spaceEx(gridManager,gridManager.grid().leafGridView(), order+1);

  H1ExSpaces exSpaces(&temperatureSpace,&spaceEx);

  std::string exVarNames[2] = { "l", "e"};
  ExVariableSet exVariableSet(exSpaces, exVarNames);

  EstAssembler estAssembler(gridManager,exSpaces);

  auto const& is = gridManager.grid().leafIndexSet();

  VariableSet::VariableSet x(variableSet), dx(variableSet), tmp(variableSet);

  double rTolX = 2.0e-3, aTolX = 2.0e-3, minRefine = 0.2;

  std::vector<std::pair<double,double> > tolX(variableSet.noOfVariables);
  std::vector<std::pair<double,double> > tolXC(variableSet.noOfVariables);
  for (int i=0; i<tolX.size(); ++i)
  {
    tolX[i] = std::make_pair(aTolX,rTolX);
    tolXC[i] = std::make_pair(aTolX/100,rTolX/100);
  }
  std::cout << "Accuracy: rTol = " << rTolX << ", aTol = " << aTolX << std::endl;
  std::cout << "Minimal number of cells to refine in an adaptive step: " << minRefine *100
            << " % of all" << std::endl << std::endl;

  int refSteps = -1;
  bool accurate = false;
  int iteSteps = getParameter(pt, "solver.iteMax", 1000);
  double iteEps = getParameter(pt, "solver.iteEps", 1.0e-6);
  if (!direct)
  {
    std::cout << "max. number of iterations in iterative solver = " << iteSteps << std::endl;
    std::cout << "requested accuracy in iterate solver = " << iteEps << std::endl << std::endl;
  };
  Dune::InverseOperatorResult res;
  double errNorm = 0;

  size_t size = variableSet.degreesOfFreedom(0,1);
  double gamma = 1.0, d = dim;
  double beta = 1.0/sqrt(d*gamma);
  double alpha = (d*gamma-1.0)/(d*(1.0+gamma));
  double qk = 1.0, dNk = size, zk = 0.0;
  double requested = sqrt(1-beta*beta)*tolX[0].first;
  double safety = 1.0;
  double yk = pow(dNk,alpha);
  //printf("gamma=%e, d=%e, beta=%e, alpha=%e, requested=%e, safety=%e\n",
  //        gamma,d,beta,alpha,requested,safety);

  do
  {
    refSteps++;

    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    solution = 0;
    CoefficientVectors hilfe(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    hilfe = 0;
    assembler.assemble(linearization(F,x));
    CoefficientVectors rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);

    if (direct)
    {
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    }
    else
    {
      switch (iterateType)
      {
      case IterateType::CG:
      {
        if ( verbosity>0 ) std::cout << "preconditioned cg solver is used" << std::endl;
        JacobiPreconditioner<AssOperator> jacobi(A,1.0);
        const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
        StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
        int lookAhead = getParameter(pt, "solver.lookAhead", 3);
        termination.setLookAhead(lookAhead);
        CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity-1);
        cg.apply(hilfe,rhs,res);
      }
      break;
      case IterateType::PCG:
      {
        if ( verbosity>0) std::cout << "   preconditioned cascadic multigrid solver I is used" << std::endl;
        JacobiPreconditioner<AssOperator> jacobi(A,1.0);
        NMIIIPCGSolver<LinearSpace> pcg(A,jacobi,iteEps,iteSteps,verbosity-1);
        pcg.apply(hilfe,rhs,res);
      }
      break;
      case IterateType::APCG:
      {
        if ( verbosity>0) std::cout << "   preconditioned cascadic multigrid solver II is used" << std::endl;
        int addedIterations = getParameter(pt, "solver.APCG.addedIterations", 10);
        JacobiPreconditioner<AssOperator> jacobiPCG(A,1.0);
        NMIIIAPCGSolver<LinearSpace> apcg(A,jacobiPCG,iteEps,iteSteps,verbosity-1,addedIterations);
        apcg.apply(hilfe,rhs,res);
      }
      break;
      default:
        std::cout << "Solver not available" << std::endl;
        throw -111;
      }
      solution.axpy(-1,hilfe);
    }
    dx.data = solution.data;

    // Do hierarchical error estimation. Remember to provide the very same underlying problem to the
    // error estimator functional as has been used to compute dx (do not modify x!).
    std::vector<double> errorDistribution(is.size(0),0.0);
    double maxErr = 0.0;
    double errLevel = 0.0;

    if (!tolX.empty())
    {
      tmp *= 0 ;
      estAssembler.assemble(ErrorEstimator(LinearizationAt<Functional>(F,x),dx));
      constexpr int estNvars = ErrorEstimator::AnsatzVars::noOfVariables;
      constexpr int estNeq = ErrorEstimator::TestVars::noOfVariables;
      size_t  estNnz = estAssembler.nnz(0,estNeq,0,estNvars,false);
      size_t  estSize = exVariableSet.degreesOfFreedom(0,estNvars);

      std::vector<int> estRidx(estNnz), estCidx(estNnz);
      std::vector<double> estData(estNnz), estRhs(estSize), estSolVec(estSize);
      estAssembler.toSequence(0,estNeq,estRhs.begin());

      // iterative solution of error estimator

      AssEstOperator agro(estAssembler);
      Dune::InverseOperatorResult estRes;
      ExCoefficientVectors estRhside(estAssembler.rhs());
      ExCoefficientVectors estSol(ExVariableSet::CoefficientVectorRepresentation<0,1>::init(exSpaces));
      estSol = 1.0 ;
      JacobiPreconditioner<AssEstOperator> jprec(agro, 1.0);
      jprec.apply(estSol,estRhside); //single Jacobi iteration
      estSol.write(estSolVec.begin());

      // Transfer error indicators to cells.
      for (auto ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
      {
        double err = 0;
        auto gix = spaceEx.mapper().globalIndices(*ci);
        for (auto j=gix.begin(); j!=gix.end(); ++j)
          err += fabs(component<0>(estSol)[*j]);
        errorDistribution[is.index(*ci)] = err;
        if (fabs(err)>maxErr) maxErr = fabs(err);
      }

      errLevel = 0.5*maxErr;
      if (minRefine>0.0)
      {
        std::vector<double> eSort(errorDistribution);
        std::sort(eSort.begin(),eSort.end(),compareAbs);
        int minRefineIndex = minRefine*(eSort.size()-1);
        double minErrLevel = fabs(eSort[minRefineIndex])+1.0e-15;
        if (minErrLevel<errLevel)
          errLevel = minErrLevel;
      }

      errNorm = 0 ;
      for (int k=0; k < estRhs.size() ; k++ )
        errNorm +=  estRhs[k]*estSolVec[k] ;
    }
    errNorm = sqrt(errNorm) ;
    // apply the Newton correction here
    x += dx;

    if ( verbosity>0)
            std::cout << "step = " << refSteps << ": " << size << " points,   ||estim. err|| = " << std::scientific << std::setprecision(3) << errNorm << std::resetiosflags( ::std::ios::scientific ) << std::setprecision(6) << std::endl;

	// graphical output of solution, mesh is not yet refined
    std::ostringstream fn;
    fn << "graph3d/peak-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(x,fn.str(),IoOptions().setOrder(order).setPrecision(7));
    std::cout << "   output written to file " << fn.str() << std::endl;

    if (refSteps>maxAdaptSteps)
    {
      std::cout << "max. number of refinement steps is reached" << std::endl;
      break;
    }

	// Evaluation of (global/local) error estimator information
    if (!tolX.empty())
    {
      if (errNorm<requested)
      {
        accurate = true ;
        std::cout << "||estim. error|| is smaller than requested" << std::endl;
      }
      else
      {
        // Refine mesh.

        int noToRefine = 0;
        double alphaSave = 1.0 ;
        std::vector<bool> toRefine( is.size(0), false ) ; //for adaptivity in compression
        std::vector< std::vector<bool> > refinements;
        for (auto ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
          if (fabs(errorDistribution[is.index(*ci)]) >= alphaSave*errLevel)
          {
            noToRefine++;
            toRefine[is.index(*ci)] = true ;
            gridManager.mark(1,*ci);
          }

        refinements.push_back(toRefine);
        accurate = !gridManager.adaptAtOnce();
      }
    }

    size = variableSet.degreesOfFreedom(0,1);
    qk = size/dNk;
    dNk = size;
    yk += pow(dNk,alpha);
    zk = pow(dNk,alpha)*(pow(errNorm/requested,d*alpha)-pow(qk,alpha))/(pow(qk,alpha)-1.0);
    iteEps = safety*beta*errNorm*yk/(yk+zk);

  } while (!accurate);

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End peak source problem" << std::endl;
}


#include "Circle2D.h"
#include "Line2D.h"
#include "Pose2D.h"
#include "Geometry.h"

#include <cmath>
#include <boost/concept_check.hpp>

using namespace A2O;
using namespace Eigen;

Circle2D::Circle2D()
      : _origin(0, 0), _radius(1)
{
}

Circle2D::Circle2D(const double& xOrigin,
	       const double& yOrigin,
	       const double& radius)
      : _origin(xOrigin, yOrigin), _radius(radius >= 0 ? radius : -radius)
{
}

Circle2D::Circle2D(const Vector2d& origin,
	       const double& radius)
      : Circle2D(origin(0), origin(1), radius)
{
}

Circle2D::Circle2D(const Vector2d& origin,
		   const Vector2d& point)
      : _origin(origin), _radius(Geometry::getDistance(origin, point))
{
}

Circle2D::Circle2D(const double& xOrigin,
	       const double& yOrigin,
	       const double& px,
	       const double& py)
      : _origin(xOrigin, yOrigin), _radius(Geometry::getDistance(xOrigin, yOrigin, px, py))
{
}

Circle2D::Circle2D(const Pose2D tangent, Vector2d point)
{
  // transform point in to an tangent touching point local coordinate system (tangent angle = x-axis + pi)
  Eigen::Vector2d tmpVector = point - tangent.getPosition();
  Eigen::Vector2d rotatedVector(0,0);
  Angle::rotate(tmpVector, - tangent.getAngle().rad(), rotatedVector);
  // calculate y-coordinate of the circle origin
  double my = (- pow(rotatedVector(0), 2) - pow(rotatedVector(1), 2) ) / (-2 * rotatedVector(1));
  // transform back to world coordinate system
  Eigen::Vector2d shiftedCircleOrigin(0,0);
  Angle::rotate(Eigen::Vector2d(0, my), tangent.getAngle().rad(), shiftedCircleOrigin);
  _origin = shiftedCircleOrigin + tangent.getPosition();
  _radius = Geometry::getDistance(_origin, tangent.getPosition());
}

Circle2D::~Circle2D()
{
}

const Vector2d& Circle2D::origin() const
{
  return _origin;
}

const double& Circle2D::radius() const
{
  return _radius;
}

const bool Circle2D::checkInnerTouching(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return distance > 0
      && (_radius == distance + other._radius || other._radius == distance + _radius);
}

const bool Circle2D::checkOuterTouching(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return _radius + other._radius == distance;
}

const bool Circle2D::checkIntersect(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return distance > 0
      && distance < _radius + other._radius
      && _radius < distance + other._radius
      && other._radius < distance + _radius;
}

const bool Circle2D::checkInnerCircle(const Circle2D& other) const
{
  double distance = getDistanceTo(other);

  return _radius > distance + other._radius || other._radius > distance + _radius;
}

const double Circle2D::getDistanceTo(const Circle2D& other) const
{
  return Geometry::getDistance(_origin, other._origin);
}

const Vector2d Circle2D::getPointOnCircle(const Angle& angle) const
{
  return Vector2d(_origin(0) + (std::cos(angle.rad()) * _radius),
		  _origin(1) + (std::sin(angle.rad()) * _radius));
}

const bool Circle2D::isOnCircle(const Vector2d& point) const
{
  double distance = Geometry::getDistance(_origin, point);
  double diff = std::abs(distance - _radius);
  return (diff < 0.000001);
}

const Angle Circle2D::getAngle(const Vector2d& point)
{
  Vector2d shiftedPoint = point - _origin;
  // point is equal to origin
  if(std::fabs(point(0)- _origin(0)) < 0.001 && std::fabs(point(1)- _origin(1)) < 0.001)
    return Angle(0);
  // threshold x = 0
  if(std::fabs<double>(shiftedPoint(0)) < 0.0001)
  {
    if(shiftedPoint(1) > 0)
      return Angle(M_PI * 0.5);
    else
      return Angle(-M_PI * 0.5);
  }

  double angle = atan2(shiftedPoint(1), shiftedPoint(0));
  return Angle(angle);
}

const Angle Circle2D::getAngle(const double& segmentLength)
{
  return Angle(segmentLength / _radius);
}

Pose2D Circle2D::getClosestPose(const Vector2d& point)
{
 Angle angle = Angle::to(point - _origin);
 Vector2d closestPoint = getPointOnCircle(angle);
 return Pose2D(closestPoint, slope(closestPoint));
}

Angle Circle2D::slope(const Vector2d& point)
{
  // calculate slope on upper semicircle
  double slope = -0.5 * sqrt(1 / (pow(_radius, 2) - pow(point(0) - _origin(0), 2))) * (2 * point(0) - 2 * _origin(0));
  // check if point is on lower semicircle
  if(_origin(1) > point(1))
  {
    slope *= -1;
  }
  return Angle(atan(slope));
}

short Circle2D::getDirectionOfRotation(const Vector2d& point, const Angle& angle)
{
  Angle angle2 = getAngle(point);
  angle2 += Angle(M_PI * 0.5);
  double angleResult  = std::fabs<double>(angle2.rad() - angle.rad()) + M_PI * 0.5;

  if(angleResult < M_PI || angleResult > 2 * M_PI)
    return 1;
  else
    return -1;
}

short int Circle2D::getDirectionOfRotation(const Pose2D pose)
{
  return getDirectionOfRotation(pose.getPosition(), pose.getAngle());
}

std::vector< Vector2d > Circle2D::getTrail(const Vector2d& start, const Vector2d& end)
{
  Angle startAngle = getAngle(start);
  Angle endAngle = getAngle(end);
  Angle diff = endAngle - startAngle;
  Angle delta =  Angle(diff.rad() / 10);
  Vector2d point;
  std::vector<Vector2d> points;
  int i = 0;
  do{
    startAngle += delta;
    point = getPointOnCircle(startAngle);
    points.push_back(point);
  } while(++i < 10);
  return points;
}

bool Circle2D::operator==(const Circle2D& other) const
{
  return _origin == other._origin && _radius == other._radius;
}

bool Circle2D::operator!=(const Circle2D& other) const
{
  return !(*this == other);
}

const Circle2D Circle2D::avg(const Circle2D& c1,
			     const Circle2D& c2,
			     const double& weight1,
			     const double& weight2)
{
  Vector2d avgOrigin = Geometry::avg(c1._origin, c2._origin, weight1, weight2);

  return Circle2D(avgOrigin, c1._radius * weight1 + c2._radius * weight2);
}

const Circle2D Circle2D::avg(const std::vector<Circle2D>& circles)
{
  size_t size = circles.size();
  if (size == 0) {
    return Circle2D(0, 0, 1);
  } else if (size == 1) {
    return circles[0];
  }

  double x = 0;
  double y = 0;
  double radius = 0;

  for (Circle2D circle : circles) {
    x += circle._origin(0);
    y += circle._origin(1);
    radius += circle._radius;
  }

  return Circle2D(x / size, y / size, radius / size);
}


// Before doing anything else, grab in the definitions of the test
// space and trial spaces that make up the finite element system.
#include <qdove/base/test_space.h>
#include <qdove/base/trial_space.h>
#include <qdove/materials/constants.h>

// Start by solving Schroedinger's problem
#include <qdove/models/schroedinger.h>

// Let's use a function from the qdove library for a psudo potential.
#include <qdove/psuedopotentials/function_library.h>

// Also make use of the predefined generalised eigenspectrum system -
// we need this for the Schroedinger problem.
#include <qdove/generic_linear_algebra/eigenspectrum_system.h>

// Next up are some deal.II objects that have not been generalised
// away yet...
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/numerics/data_out.h>

// C++
#include <cassert>
#include <iostream>
#include <fstream>

// The purpose of this example is to solve a problem with a known
// analytical solution.
template<int dim>
class SchroedingerProblem
{
public:
  SchroedingerProblem (dealii::Triangulation<dim> &triangulation);
  ~SchroedingerProblem ();

  void write_gnuplot (const dealii::PETScWrappers::Vector &vector,
		      const std::string                   &name);
  void run ();

private:
  // The description of the finite element basis
  qdove::TrialSpace<dim> trial_space;

  // Geometry description and boundary constraints
  qdove::TestSpace<dim> test_space;

  // The eigenspectrum system that will be solved by SLEPc
  qdove::Schroedinger::Problem<dim> schroedinger_problem;

  // The eigenpairs from schroedinger's problem
  std::vector<dealii::PETScWrappers::Vector> eigenvectors;
  std::vector<double>                        eigenvalues;
};

template<int dim>
SchroedingerProblem<dim>::SchroedingerProblem (dealii::Triangulation<dim> &triangulation)
  :
  trial_space (triangulation),
  test_space (trial_space),
  schroedinger_problem (trial_space, test_space)
{}

template<int dim>
SchroedingerProblem<dim>::~SchroedingerProblem ()
{}

template<int dim>
void
SchroedingerProblem<dim>::write_gnuplot (const dealii::PETScWrappers::Vector &vector,
					 const std::string                   &name)
{
  // Output a vector to gnuplot style file.
  std::ostringstream filename;
  filename << "solution-" << name << ".gpl";
  std::ofstream output (filename.str ().c_str ());

  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler (test_space.dofs ());
  data_out.add_data_vector (vector, name);

  // generate default patches and output.
  data_out.build_patches ();
  data_out.write_gnuplot (output);
}

template<int dim>
void
SchroedingerProblem<dim>::run ()
{
  std::cout << "Test space:" << std::endl
	    << "   Finite element type:          "
	    << test_space.fe ().get_name ()
	    << std::endl
	    << "   Number of degrees of freedom: "
	    << test_space.n_dofs ()
	    << std::endl;

  const double radius = 50e-10;

  dealii::PETScWrappers::Vector material_function (test_space.n_dofs ());
  for (unsigned int i=0; i<material_function.size (); ++i)
    material_function[i] = 1.;
  write_gnuplot (material_function, "material_function");

  // Get started on Schroedinger's problem
  std::cout << "Schroedinger's problem:"
	    << std::endl;
  schroedinger_problem.reinit ();

  // set up the effective mass
  dealii::PETScWrappers::Vector eff_mass (test_space.n_dofs ());
  const double ke_term = (qdove::HBAR*qdove::HBAR) / (2.*qdove::mstar_p_CdTe*qdove::M0);
  std::cout << "   Kinetic energy term:          "
	    << ke_term << std::endl;
  eff_mass.add (ke_term, material_function);
  write_gnuplot (eff_mass, "effective_mass");

  // set up the potential
  dealii::PETScWrappers::Vector potential (test_space.n_dofs ());

  schroedinger_problem.assemble (eff_mass, potential);
  schroedinger_problem.solve ();
  schroedinger_problem.get_solution_eigenpairs (eigenvalues, eigenvectors);
  write_gnuplot (eigenvectors[0], "electron_function");

  // output
  std::cout << "   Eigenvalues:                  ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    std::cout << eigenvalues[i] << " ";
  std::cout << std::endl;

  std::cout << "   Scaled values (eV):           ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    std::cout << eigenvalues[i]/(1e-03*qdove::E0) << " ";
  std::cout << std::endl;

  std::cout << "   Scaled analytic values:       ";
  for (unsigned int i=0; i<eigenvalues.size (); ++i)
    {
      // This equation is equation 3.13 in Harrisson's book. It reads:
      // E_n = (hbar*pi*n)^2 / (2mL^2)
      const unsigned int n          = i+1;
      const double L                = 2.*radius;
      const double analytical_value = (qdove::HBAR*qdove::HBAR*qdove::PI*qdove::PI*n*n) / (2*qdove::mstar_p_CdTe*qdove::M0*L*L);
      std::cout << analytical_value/(1e-03*qdove::E0) << " ";
    }
  std::cout << std::endl;
}

int main (int argc, char **argv)
{
  try
    {
      dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);
      {
	// Create a grid
	dealii::Triangulation<1> triangulation;
	dealii::GridGenerator::hyper_cube (triangulation, -50e-10, 50e-10);
	triangulation.refine_global (9);

	// Run Schroedinger's problem on that grid
	SchroedingerProblem<1> schroedinger_problem (triangulation);
	schroedinger_problem.run ();
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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

  return 0;
}


#pragma once

#include <Eigen/Dense>

namespace vision_filter {
/**
 * Abstract class that should be inherited from to create specific
 * kalman filters
 *
 * Must initialize:
 *  x_k1_k1, x_k_k1, x_k_k,
 *  P_k1_k1, P_k_k1, P_k_k,
 *  F_k, B_k, H_k,
 *  Q_k, R_k
 *
 * Every predict, must update:
 *  u_k
 *
 * Every PredictWithUpdate, must update:
 *  u_k, z_k
 *
 * The most recently updated values
 *  x_k_k, P_k_k
 *
 * Taken from https://en.wikipedia.org/wiki/Kalman_filter
 *
 * Conversion between code notation and wiki notation is...
 * x_k1_k1 is X_(k-1, k-1)
 * x_k_k is X_(k, k)
 * etc
 */
class KalmanFilter {
public:
    /**
     * Creates a general kalman filter with the given sizes
     * Use a child class to setup the specific state matricies
     * Assumes 1 input
     *
     * @param state_size The size of the state vector
     * @param observation_size The size of the observation vector
     */
    KalmanFilter(unsigned int state_size, unsigned int observation_size)
        : x_k1_k1_(state_size),
          x_k_k1_(state_size),
          x_k_k_(state_size),
          u_k_(1),
          z_k_(observation_size),
          y_k_k1_(observation_size),
          y_k_k_(observation_size),
          P_k1_k1_(state_size, state_size),
          P_k_k1_(state_size, state_size),
          P_k_k_(state_size, state_size),
          S_k_(observation_size, observation_size),
          K_k_(state_size, observation_size),
          F_k_(state_size, state_size),
          B_k_(state_size, 1),
          H_k_(observation_size, state_size),
          Q_k_(state_size, state_size),
          R_k_(observation_size, observation_size),
          identity_(Eigen::MatrixXd::Identity(state_size, state_size)) {}

    /**
     * Predicts without update
     */
    void predict();

    /**
     * Predicts with update
     * z_k must be set with the observation
     */
    void predict_with_update();

protected:
    Eigen::VectorXd x_k1_k1_;
    Eigen::VectorXd x_k_k1_;
    Eigen::VectorXd x_k_k_;

    Eigen::VectorXd u_k_;
    Eigen::VectorXd z_k_;

    Eigen::VectorXd y_k_k1_;
    Eigen::VectorXd y_k_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k1_k1_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k_k1_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd P_k_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd S_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd K_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd F_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd B_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd H_k_;

    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd Q_k_;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::MatrixXd R_k_;

    Eigen::MatrixXd identity_;
};
}  // namespace vision_filter


#include <iostream>
#include <boost/program_options.hpp>
#include "vptree.h"

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

/* Functions taken from Laurens van der Maaten's original implementation of t-SNE */
/* Source: https://github.com/lvdmaaten/bhtsne                                    */
static void computeGaussianPerplexity(double* X, int N, int D, unsigned int** _row_P, unsigned int** _col_P, double** _val_P, double perplexity, int K);
static void symmetrizeMatrix(unsigned int** _row_P, unsigned int** _col_P, double** _val_P, int N);
static void computeSquaredEuclideanDistance(double* X, int N, int D, double* DD);
static void zeroMean(double* X, int N, int D);
/**********************************************************************************/

static bool load_data(string infile, double **data, int &num_instances, int &num_features) {

  FILE *fp = fopen(infile.c_str(), "rb");
	if (fp == NULL) {
		cout << "Error: could not open data file " << infile << endl;
		return false;
	}

  uint64_t ret;
	ret = fread(&num_instances, sizeof(int), 1, fp);
	ret = fread(&num_features, sizeof(int), 1, fp);

	*data = (double *)malloc(num_instances * num_features * sizeof(double));
  if (*data == NULL) {
    cout << "Error: memory allocation of " << num_instances << " by "
         << num_features << " matrix failed" << endl;
    return false;
  }

  uint64_t nelem = (uint64_t)num_instances * num_features;

  size_t batch_size = 1e8;
  double *ptr = *data;
  ret = 0;
  for (uint64_t remaining = nelem; remaining > 0; remaining -= batch_size) {
    if (remaining < batch_size) {
      batch_size = remaining;
    }
    ret += fread(ptr, sizeof(double), batch_size, fp);
    ptr += batch_size;
  }

  if (ret != nelem) {
    cout << "Error: reading input returned incorrect number of elements (" << ret
         << ", expected " << nelem << ")" << endl;
    return false;
  }

	fclose(fp);

	return true;
}

static void truncate_data(double *X, int num_instances, int num_features, int target_dims) {
  size_t i_old = 0;
  size_t i_new = 0;
  for (int r = 0; r < num_instances; r++) {
    for (int c = 0; c < num_features; c++) {
      if (c < target_dims) {
        X[i_new++] = X[i_old];
      }
      i_old++;
    }
  }
}

static bool run(double *X, int num_instances, int num_features, double perplexity, string outfile) {

  // Apply lower bound on perplexity from original t-SNE implementation
  if (num_instances - 1 < 3 * perplexity) {
    cout << "Error: target perplexity (" << perplexity << ") is too large "
         << "for the number of data points (" << num_instances << ")" << endl;
    return false;
  }

  printf("Processing %d data points, %d features with target perplexity %f\n",
         num_instances, num_features, perplexity);

  // Normalize input data (to prevent numerical problems)
  zeroMean(X, num_instances, num_features);
  cout << "Normalizing the features" << endl;
  double max_X = 0;
  for (size_t i = 0; i < num_instances * num_features; i++) {
    if (fabs(X[i]) > max_X) {
      max_X = fabs(X[i]);
    }
  }

  for (size_t i = 0; i < num_instances * num_features; i++) {
    X[i] /= max_X;
  }

  // Compute input similarities for exact t-SNE
  double* P; unsigned int* row_P; unsigned int* col_P; double* val_P;

  // Compute asymmetric pairwise input similarities
  cout << "Computing conditional distributions" << endl;
  computeGaussianPerplexity(X, num_instances, num_features,
    &row_P, &col_P, &val_P, perplexity, (int) (3 * perplexity));

  // Symmetrize input similarities
  cout << "Symmetrizing matrix" << endl;
  symmetrizeMatrix(&row_P, &col_P, &val_P, num_instances);
  double sum_P = .0;
  for (int i = 0; i < row_P[num_instances]; i++) {
    sum_P += val_P[i];
  }
  for (int i = 0; i < row_P[num_instances]; i++) {
    val_P[i] /= sum_P;
  }

  cout << "Saving to " << outfile << endl;
	FILE *fp = fopen(outfile.c_str(), "wb");
	if (fp == NULL) {
		cout << "Error: could not open output file " << outfile << endl;
    return false;
	}

	fwrite(&num_instances, sizeof(int), 1, fp);
  fwrite(row_P, sizeof(unsigned int), num_instances + 1, fp);
	fwrite(col_P, sizeof(unsigned int), row_P[num_instances], fp);
  fwrite(val_P, sizeof(double), row_P[num_instances], fp);

  free(row_P);
  free(col_P);
  free(val_P);

  return true;
}

int main(int argc, char **argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-file", po::value<string>()->value_name("FILE")->default_value("data.dat"), "name of binary input file (see prepare_input.m)")
    ("output-file", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of output file to be created")
    ("perp", po::value<double>()->value_name("NUM")->default_value(30, "30"), "set target perplexity for conditional distributions")
    ("num-dims", po::value<int>()->value_name("NUM"), "if provided, only the first NUM features in the input will be used")
  ;

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
                options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << "Usage: ComputeP [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

	double perplexity = vm["perp"].as<double>();
  string infile = vm["input-file"].as<string>();
  string outfile = vm["output-file"].as<string>();

  double *data;
  int num_instances;
  int num_features;

	if (!load_data(infile, &data, num_instances, num_features)) {
    return 1;
  }

  cout << infile << " successfully loaded" << endl;

  if (vm.count("num-dims")) {
    int num_dims = vm["num-dims"].as<int>();
    cout << "Using only the first " << num_dims << " dimensions" << endl;
    truncate_data(data, num_instances, num_features, num_dims);
    num_features = num_dims;
  }

  if (!run(data, num_instances, num_features, perplexity, outfile)) {
    return 1;
  }

  cout << "Done" << endl;

  return 0;
}

// Compute input similarities with a fixed perplexity using ball trees (this function allocates memory another function should free)
// Source: https://github.com/lvdmaaten/bhtsne
static void computeGaussianPerplexity(double* X, int N, int D, unsigned int** _row_P, unsigned int** _col_P, double** _val_P, double perplexity, int K) {

    if(perplexity > K) printf("Perplexity should be lower than K!\n");

    // Allocate the memory we need
    *_row_P = (unsigned int*)    malloc((N + 1) * sizeof(unsigned int));
    *_col_P = (unsigned int*)    calloc(N * K, sizeof(unsigned int));
    *_val_P = (double*) calloc(N * K, sizeof(double));
    if(*_row_P == NULL || *_col_P == NULL || *_val_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    unsigned int* row_P = *_row_P;
    unsigned int* col_P = *_col_P;
    double* val_P = *_val_P;
    double* cur_P = (double*) malloc((N - 1) * sizeof(double));
    if(cur_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    row_P[0] = 0;
    for(int n = 0; n < N; n++) row_P[n + 1] = row_P[n] + (unsigned int) K;

    // Build ball tree on data set
    VpTree<DataPoint, euclidean_distance>* tree = new VpTree<DataPoint, euclidean_distance>();
    vector<DataPoint> obj_X(N, DataPoint(D, -1, X));
    for(int n = 0; n < N; n++) obj_X[n] = DataPoint(D, n, X + n * D);
    tree->create(obj_X);

    // Loop over all points to find nearest neighbors
    printf("Building tree...\n");
    vector<DataPoint> indices;
    vector<double> distances;
    for(int n = 0; n < N; n++) {

        if(n % 10000 == 0) printf(" - point %d of %d\n", n, N);

        // Find nearest neighbors
        indices.clear();
        distances.clear();
        tree->search(obj_X[n], K + 1, &indices, &distances);

        // Initialize some variables for binary search
        bool found = false;
        double beta = 1.0;
        double min_beta = -DBL_MAX;
        double max_beta =  DBL_MAX;
        double tol = 1e-5;

        // Iterate until we found a good perplexity
        int iter = 0; double sum_P;
        while(!found && iter < 200) {

            // Compute Gaussian kernel row
            for(int m = 0; m < K; m++) cur_P[m] = exp(-beta * distances[m + 1] * distances[m + 1]);

            // Compute entropy of current row
            sum_P = DBL_MIN;
            for(int m = 0; m < K; m++) sum_P += cur_P[m];
            double H = .0;
            for(int m = 0; m < K; m++) H += beta * (distances[m + 1] * distances[m + 1] * cur_P[m]);
            H = (H / sum_P) + log(sum_P);

            // Evaluate whether the entropy is within the tolerance level
            double Hdiff = H - log(perplexity);
            if(Hdiff < tol && -Hdiff < tol) {
                found = true;
            }
            else {
                if(Hdiff > 0) {
                    min_beta = beta;
                    if(max_beta == DBL_MAX || max_beta == -DBL_MAX)
                        beta *= 2.0;
                    else
                        beta = (beta + max_beta) / 2.0;
                }
                else {
                    max_beta = beta;
                    if(min_beta == -DBL_MAX || min_beta == DBL_MAX)
                        beta /= 2.0;
                    else
                        beta = (beta + min_beta) / 2.0;
                }
            }

            // Update iteration counter
            iter++;
        }

        // Row-normalize current row of P and store in matrix
        for(unsigned int m = 0; m < K; m++) cur_P[m] /= sum_P;
        for(unsigned int m = 0; m < K; m++) {
            col_P[row_P[n] + m] = (unsigned int) indices[m + 1].index();
            val_P[row_P[n] + m] = cur_P[m];
        }
    }

    // Clean up memory
    obj_X.clear();
    free(cur_P);
    delete tree;
}

// Symmetrizes a sparse matrix
// Source: https://github.com/lvdmaaten/bhtsne
static void symmetrizeMatrix(unsigned int** _row_P, unsigned int** _col_P, double** _val_P, int N) {

    // Get sparse matrix
    unsigned int* row_P = *_row_P;
    unsigned int* col_P = *_col_P;
    double* val_P = *_val_P;

    // Count number of elements and row counts of symmetric matrix
    int* row_counts = (int*) calloc(N, sizeof(int));
    if(row_counts == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    for(int n = 0; n < N; n++) {
        for(int i = row_P[n]; i < row_P[n + 1]; i++) {

            // Check whether element (col_P[i], n) is present
            bool present = false;
            for(int m = row_P[col_P[i]]; m < row_P[col_P[i] + 1]; m++) {
                if(col_P[m] == n) present = true;
            }
            if(present) row_counts[n]++;
            else {
                row_counts[n]++;
                row_counts[col_P[i]]++;
            }
        }
    }
    int no_elem = 0;
    for(int n = 0; n < N; n++) no_elem += row_counts[n];

    // Allocate memory for symmetrized matrix
    unsigned int* sym_row_P = (unsigned int*) malloc((N + 1) * sizeof(unsigned int));
    unsigned int* sym_col_P = (unsigned int*) malloc(no_elem * sizeof(unsigned int));
    double* sym_val_P = (double*) malloc(no_elem * sizeof(double));
    if(sym_row_P == NULL || sym_col_P == NULL || sym_val_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }

    // Construct new row indices for symmetric matrix
    sym_row_P[0] = 0;
    for(int n = 0; n < N; n++) sym_row_P[n + 1] = sym_row_P[n] + (unsigned int) row_counts[n];

    // Fill the result matrix
    int* offset = (int*) calloc(N, sizeof(int));
    if(offset == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    for(int n = 0; n < N; n++) {
        for(unsigned int i = row_P[n]; i < row_P[n + 1]; i++) {                                  // considering element(n, col_P[i])

            // Check whether element (col_P[i], n) is present
            bool present = false;
            for(unsigned int m = row_P[col_P[i]]; m < row_P[col_P[i] + 1]; m++) {
                if(col_P[m] == n) {
                    present = true;
                    if(n <= col_P[i]) {                                                 // make sure we do not add elements twice
                        sym_col_P[sym_row_P[n]        + offset[n]]        = col_P[i];
                        sym_col_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = n;
                        sym_val_P[sym_row_P[n]        + offset[n]]        = val_P[i] + val_P[m];
                        sym_val_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = val_P[i] + val_P[m];
                    }
                }
            }

            // If (col_P[i], n) is not present, there is no addition involved
            if(!present) {
                sym_col_P[sym_row_P[n]        + offset[n]]        = col_P[i];
                sym_col_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = n;
                sym_val_P[sym_row_P[n]        + offset[n]]        = val_P[i];
                sym_val_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = val_P[i];
            }

            // Update offsets
            if(!present || (present && n <= col_P[i])) {
                offset[n]++;
                if(col_P[i] != n) offset[col_P[i]]++;
            }
        }
    }

    // Divide the result by two
    for(int i = 0; i < no_elem; i++) sym_val_P[i] /= 2.0;

    // Return symmetrized matrices
    free(*_row_P); *_row_P = sym_row_P;
    free(*_col_P); *_col_P = sym_col_P;
    free(*_val_P); *_val_P = sym_val_P;

    // Free up some memery
    free(offset); offset = NULL;
    free(row_counts); row_counts  = NULL;
}

// Compute squared Euclidean distance matrix
// Source: https://github.com/lvdmaaten/bhtsne
static void computeSquaredEuclideanDistance(double* X, int N, int D, double* DD) {
    const double* XnD = X;
    for(int n = 0; n < N; ++n, XnD += D) {
        const double* XmD = XnD + D;
        double* curr_elem = &DD[n*N + n];
        *curr_elem = 0.0;
        double* curr_elem_sym = curr_elem + N;
        for(int m = n + 1; m < N; ++m, XmD+=D, curr_elem_sym+=N) {
            *(++curr_elem) = 0.0;
            for(int d = 0; d < D; ++d) {
                *curr_elem += (XnD[d] - XmD[d]) * (XnD[d] - XmD[d]);
            }
            *curr_elem_sym = *curr_elem;
        }
    }
}

// Makes data zero-mean
// Source: https://github.com/lvdmaaten/bhtsne
static void zeroMean(double* X, int N, int D) {

	// Compute data mean
	double* mean = (double*) calloc(D, sizeof(double));
    if(mean == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    int nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			mean[d] += X[nD + d];
		}
        nD += D;
	}
	for(int d = 0; d < D; d++) {
		mean[d] /= (double) N;
	}

	// Subtract data mean
    nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			X[nD + d] -= mean[d];
		}
        nD += D;
	}
    free(mean); mean = NULL;
}


// Copyright (c) 2006-2007 Michael B. Edwin Rickert
//
// 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 )
//
// $LastChangedBy$ - $LastChangedDate$
//
// Dec 25, 2006 - industry.math.vector.hpp => industry/math/vector.hpp
// Dec  1, 2006 - Created

#ifndef BOOST_PP_IS_ITERATING
	#ifndef IG_INDUSTRY_MATH_VECTOR
	#define IG_INDUSTRY_MATH_VECTOR

	#ifndef INDUSTRY_MATH_VECTOR_DN_LIMIT
	#define INDUSTRY_MATH_VECTOR_DN_LIMIT 3
	#endif //ndef  INDUSTRY_MATH_VECTOR_DN_LIMIT

	#include <boost/preprocessor/iteration.hpp>
	#include <boost/preprocessor/repetition.hpp>
	#include <boost/static_assert.hpp>
	#include <cassert>
	#include <cmath>
	#include <numeric>

namespace industry {
	namespace math {
		template < typename T , unsigned Dn >
		class vector;

		template < typename T , unsigned Dn >
		struct vector_base;

		template < typename T >
		struct vector_base< T , 0 >; //no implementation

		template < typename T >
		struct vector_base< T , 1 > {
			T x;
			      T & operator[]( unsigned n )       { assert(n<1); return x; }
			const T & operator[]( unsigned n ) const { assert(n<1); return x; }
		};
		template < typename T >
		struct vector_base< T , 2 > {
			T x,y;
			      T & operator[]( unsigned n )       { assert(n<2); return *(&x+n); }
			const T & operator[]( unsigned n ) const { assert(n<2); return *(&x+n); }
		};
		template < typename T >
		struct vector_base< T , 3 > {
			T x,y,z;
			      T & operator[]( unsigned n )       { assert(n<3); return *(&x+n); }
			const T & operator[]( unsigned n ) const { assert(n<3); return *(&x+n); }
		};
		template < typename T , unsigned Dn >
		struct vector_base {
			BOOST_STATIC_ASSERT(( Dn > 3 ));
			T x,y,z;
		protected:
			T etc[ Dn-3 ];
			      T & operator[]( unsigned n )       { assert(n<Dn); return (n<=3) ? *(&x+n) : (etc[n-3]); }
			const T & operator[]( unsigned n ) const { assert(n<Dn); return (n<=3) ? *(&x+n) : (etc[n-3]); }
		};

		#define BOOST_PP_ITERATION_LIMITS (1,INDUSTRY_MATH_VECTOR_DN_LIMIT)
		#define BOOST_PP_FILENAME_1 <industry/math/vector.hpp>
		#include BOOST_PP_ITERATE()

		template < typename T , unsigned N >
		bool operator==( const vector<T,N> & lhs , const vector<T,N> & rhs ) {
			for ( unsigned i = 0 ; i < N ; ++i ) {
				if ( lhs[i] != rhs[i] ) return false;
			}
			return true;
		}
		template < typename T , unsigned N >
		bool operator!=( const vector<T,N> & lhs , const vector<T,N> & rhs ) {
			return !(lhs==rhs);
		}
	}
}

	#endif //ndef IG_INDUSTRY_MATH_VECTOR

#else //BOOST_PP_IS_ITERATING
	#define DN                                             BOOST_PP_ITERATION()
	#define INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS(z,n,src)   (*this)[n]  = src ## n;

template < typename T >
class vector< T , DN > : public vector_base< T , DN > {
public:
	vector() { std::fill_n(&(*this)[0], DN, T()); }
	vector( BOOST_PP_ENUM_PARAMS(DN,T elem) ) { BOOST_PP_REPEAT(DN,INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS,elem); }

	vector& operator+=( const vector & other ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] += other[i]; return *this; }
	vector& operator-=( const vector & other ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] -= other[i]; return *this; }
	vector& operator*=( const T &      value ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] *= value; return *this; }
	vector& operator/=( const T &      value ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] /= value; return *this; }

	friend vector operator+( const vector & lhs , const vector & rhs ) { vector c(lhs); c += rhs; return c; }
	friend vector operator-( const vector & lhs , const vector & rhs ) { vector c(lhs); c -= rhs; return c; }
	friend vector operator*( const vector & lhs , const T      & rhs ) { vector c(lhs); c *= rhs; return c; }
	friend vector operator/( const vector & lhs , const T      & rhs ) { vector c(lhs); c /= rhs; return c; }

	friend T square_magnitude( const vector & v ) { return std::inner_product(&v[0], &v[0] + DN, &v[0], T()); }
	friend T magnitude ( const vector & v ) { return T( std::sqrt( square_magnitude( v ) ) ); }

	template < typename U > friend U square_magnitude( const vector & v ) { return static_cast<U>(square_magnitude(v)); }
	template < typename U > friend U magnitude ( const vector & v ) { return U( std::sqrt( square_magnitude/*<U>*/(v) ) ); }
};

	#undef DN
	#undef INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS
#endif //BOOST_PP_IS_ITERATING


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_ENERGYMODEL_HPP
#define MCL_ENERGYMODEL_HPP 1

#include <memory>
#include <Eigen/Dense>
#include "MCL/Lame.hpp"
#include "MCL/XuSpline.hpp"

namespace mcl
{

enum
{
	ENERGY_MODEL_INVALID = -1,
	ENERGY_MODEL_ARAP,
	ENERGY_MODEL_XUSPLINE_NH, // Xu spline (neo-Hookean)
	ENERGY_MODEL_XUSPLINE_STVK, // Xu spline (St. VK)
	ENERGY_MODEL_XUSPLINE_COROTATE, // Xu spline (CR)
	ENERGY_MODEL_STABLE_NH, // Stable neo-Hookean
	ENERGY_MODEL_NH, // iso log-barrier neo-Hookean
	ENERGY_MODEL_SYMM_DIR, // Symmetric Dirichlet
	ENERGY_MODEL_NUM
};

// Nonlinear Material Design Using Principal Stretches, Xu et al. 2015.
template <int DIM>
class XuSplineModel
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const XuSpline<double> *s, const VecD &x);
	static double gradient(const XuSpline<double> *s, const VecD &x, VecD &g);
	static void hessian(const XuSpline<double> *s, const VecD &x, MatD &H);
};

// Simplified stable Neo-Hookean (similar to Smith et al. '17)
template <int DIM>
class StableNeoHookean
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const Lame &lame, const VecD &x);
	static double gradient(const Lame &lame, const VecD &x, VecD &g);
	static void hessian(const Lame &lame, const VecD &x, MatD &H);
};

// Classic (isotropic) Neo-Hookean
template <int DIM>
class IsoNeoHookean
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const Lame &lame, const VecD &x);
	static double gradient(const Lame &lame, const VecD &x, VecD &g);
	static void hessian(const Lame &lame, const VecD &x, MatD &H);
};

// SLIM Symmetric Dirichlet
// f(x) = (||F||^2 + ||F^-1||^2)/2
template <int DIM>
class SymmDirichlet
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const VecD &x);
	static double gradient(const VecD &x, VecD &g);
	static void hessian(const VecD &x, MatD &H);
};

//
//	Implementation
//

//================================================
//	XuSpline
//================================================

template <int DIM>
double XuSplineModel<DIM>::energy_density(const XuSpline<double> *s, const VecD &x)
{
	if (DIM==3)
	{
		return s->f(x[0]) + s->f(x[1]) + s->f(x[2]) +
			s->g(x[0]*x[1]) + s->g(x[1]*x[2]) + s->g(x[2]*x[0]) +
			s->h(x[0]*x[1]*x[2]);
	}
	return s->f(x[0]) + s->f(x[1]) + s->g(x[0]*x[1]) + s->h(x[0]*x[1]);
}

template <int DIM>
double XuSplineModel<DIM>::gradient(const XuSpline<double> *s, const VecD &x, VecD &g)
{
	if (g.size() != x.size()) { g.resize(x.size()); }
	if (DIM==3)
	{
		double hprime = s->dh(x[0]*x[1]*x[2]);
		g[0] = s->df(x[0]) + s->dg(x[0]*x[1])*x[1] + s->dg(x[2]*x[0])*x[2] + hprime*x[1]*x[2];
		g[1] = s->df(x[1]) + s->dg(x[1]*x[2])*x[2] + s->dg(x[0]*x[1])*x[0] + hprime*x[2]*x[0];
		g[2] = s->df(x[2]) + s->dg(x[2]*x[0])*x[0] + s->dg(x[1]*x[2])*x[1] + hprime*x[0]*x[1];
		return energy_density(s, x);
	}
	double hprime = s->dh(x[0]*x[1]);
	g[0] = s->df(x[0]) + s->dg(x[0]*x[1])*x[1] + hprime*x[1];
	g[1] = s->df(x[1]) + s->dg(x[0]*x[1])*x[0] + hprime*x[0];
	return energy_density(s, x);
}

template <int DIM>
void XuSplineModel<DIM>::hessian(const XuSpline<double> *s, const VecD &x, MatD &H)
{
	if (DIM==3)
	{
		double hprime = s->dh(x[0]*x[1]*x[2]);
		double hprimeprime = s->ddh(x[0]*x[1]*x[2]);
		H(0,0) = s->ddf(x[0]) + s->ddg(x[0]*x[1])*x[1]*x[1] + s->ddg(x[2]*x[0])*x[2]*x[2] + hprimeprime*x[1]*x[1]*x[2]*x[2];
		H(1,1) = s->ddf(x[1]) + s->ddg(x[1]*x[2])*x[2]*x[2] + s->ddg(x[0]*x[1])*x[0]*x[0] + hprimeprime*x[2]*x[2]*x[0]*x[0];
		H(2,2) = s->ddf(x[2]) + s->ddg(x[2]*x[0])*x[0]*x[0] + s->ddg(x[1]*x[2])*x[1]*x[1] + hprimeprime*x[0]*x[0]*x[1]*x[1];
		H(0,1) = s->ddg(x[0]*x[1])*x[0]*x[1] + s->dg(x[0]*x[1]) + hprimeprime*x[0]*x[1]*x[2]*x[2] + hprime*x[2];
		H(1,0) = H(0,1);
		H(0,2) = s->ddg(x[0]*x[2])*x[0]*x[2] + s->dg(x[0]*x[2]) + hprimeprime*x[0]*x[2]*x[1]*x[1] + hprime*x[1];
		H(2,0) = H(0,2);
		H(1,2) = s->ddg(x[1]*x[2])*x[1]*x[2] + s->dg(x[1]*x[2]) + hprimeprime*x[1]*x[2]*x[0]*x[0] + hprime*x[0];
		H(2,1) = H(1,2);
		return;
	}
	double hprimeprime = s->ddh(x[0]*x[1]);
	H(0,0) = s->ddf(x[0]) + s->ddg(x[0]*x[1])*x[1]*x[1] + hprimeprime*x[1]*x[1];
	H(1,1) = s->ddf(x[1]) + s->ddg(x[0]*x[1])*x[0]*x[0] + hprimeprime*x[0]*x[0];
	H(0,1) = s->ddg(x[0]*x[1])*x[0]*x[1] + s->dg(x[0]*x[1]) + hprimeprime*x[0]*x[1];
	H(1,0) = H(0,1);
}

//================================================
//	Stable Neo-Hookean
//================================================

template <int DIM>
double StableNeoHookean<DIM>::energy_density(const Lame &lame, const VecD &x)
{
	if (!x.allFinite() || x.minCoeff() <= 0)
		return std::numeric_limits<float>::max();

	double alpha = 1.0 + lame.mu()/lame.lambda();
	constexpr double d = double(DIM);
	double I_1 = x.squaredNorm();
	double J = x.prod();
	return 0.5*lame.lambda()*(J-alpha)*(J-alpha) + 0.5*lame.mu()*(I_1-d);
}

template <int DIM>
double StableNeoHookean<DIM>::gradient(const Lame &lame, const VecD &x, VecD &grad)
{
	double alpha = 1.0 + lame.mu()/lame.lambda();
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	grad = lame.mu()*x + lame.lambda()*(J-alpha)*J*x_inv;
	return energy_density(lame, x);
}

template <int DIM>
void StableNeoHookean<DIM>::hessian(const Lame &lame, const VecD &x, MatD &hess)
{
	double alpha = 1.0 + lame.mu()/lame.lambda();
	double J = x.prod();
	if(DIM == 3)
	{
		VecD coeffs;
		coeffs[0] = x[1]*x[2];
		coeffs[1] = x[0]*x[2];
		coeffs[2] = x[0]*x[1];
		MatD coeffsMat;
		coeffsMat.setZero();
		coeffsMat(0,1) = x[2];
		coeffsMat(1,0) = x[2];
		coeffsMat(0,2) = x[1];
		coeffsMat(2,0) = x[1];
		coeffsMat(1,2) = x[0];
		coeffsMat(2,1) = x[0];
		hess = lame.mu()*MatD::Identity() +
			lame.lambda()*coeffs*coeffs.transpose() +
			lame.lambda()*(J-alpha)*coeffsMat;
		return;
	}
	hess(0,0) = lame.lambda()*x[1]*x[1] + lame.mu();
	hess(1,1) = lame.lambda()*x[0]*x[0] + lame.mu();
	hess(0,1) = lame.lambda()*(J-alpha) + lame.lambda()*J;
	hess(1,0) = hess(0,1);
}

//================================================
//	Isotropic Neo-Hookean
//================================================

template <int DIM>
double IsoNeoHookean<DIM>::energy_density(const Lame &lame, const VecD &x)
{
	if (!x.allFinite() || x.minCoeff() <= 0)
		return std::numeric_limits<float>::max();

	double J = x.prod();
	double I_1 = x.squaredNorm();
	double I_3 = J*J;
	double log_I3 = std::log( I_3 );
	double t1 = 0.5 * lame.mu() * ( I_1 - log_I3 - 3.0 );
	double t2 = 0.125 * lame.lambda() * log_I3 * log_I3;
	double r = t1 + t2;
	return r;
}

template <int DIM>
double IsoNeoHookean<DIM>::gradient(const Lame &lame, const VecD &x, VecD &grad)
{
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	grad = (lame.mu() * (x - x_inv) + lame.lambda() * std::log(J) * x_inv);
	return energy_density(lame, x);
}

template <int DIM>
void IsoNeoHookean<DIM>::hessian(const Lame &lame, const VecD &x, MatD &hess)
{
	static const MatD Iden = MatD::Identity();
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	MatD invXmat;
	invXmat.setZero();
	invXmat(0,0) = 1.0 / (x[0]*x[0]);
	invXmat(1,1) = 1.0 / (x[1]*x[1]);
	if(DIM == 3){ invXmat(2,2) = 1.0 / (x[2]*x[2]); }
	hess = lame.mu()*(Iden - 2.0*invXmat) +
		lame.lambda()*std::log(J)*invXmat +
		lame.lambda() * x_inv * x_inv.transpose();
}

//================================================
//	Symmetric Dirichlet
//================================================

template <int DIM>
double SymmDirichlet<DIM>::energy_density(const VecD &x)
{
	double e = 0;
	for (int i=0; i<DIM; ++i)
		e += x[i]*x[i] + std::pow(x[i],-2.0);

	return e;
}

template <int DIM>
double SymmDirichlet<DIM>::gradient(const VecD &x, VecD &grad)
{
	double e = 0;
	VecD pow_x;
	for(int i=0; i<DIM; ++i)
	{
		pow_x[i] = std::pow(x[i], -3.0);
		e += x[i]*x[i] + std::pow(x[i],-2.0);
	}
	grad = 2.0 * (x - pow_x);
	return e;
}

template <int DIM>
void SymmDirichlet<DIM>::hessian(const VecD &x, MatD &H)
{
	H.setZero();
	for(int i=0; i<DIM; ++i)
		H(i,i) = 2.0 + 6.0 * std::pow(x[i],-4.0);
}

} // end namespace mcl

#endif


//==================================================================================================
/*!
  @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_FUNCTION_ACSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acscd capabilities

    inverse cosecant in degree: \f$(180/\pi) \arcsin(1/x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = acscd(x);
    @endcode

    @see acsc, acscpi, asind, sind

  **/
  Value acscd(Value const & v0);
} }
#endif

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

#endif


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

// 前面几个包括的内容和前面的程序一样，所以不需要额外的注释。

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

// 然而，下一个文件是新的。我们需要这个包含文件来将自由度（DoF）与顶点、直线和单元联系起来。

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

// 以下文件包含了对双线性有限元的描述，包括它在三角形的每个顶点上有一个自由度，但在面和单元内部没有自由度。

// (事实上，该文件包含了对拉格朗日元素的一般描述，即还有二次、三次等版本，而且不仅是2d，还有1d和3d。)

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

// 在下面的文件中，可以找到几个操作自由度的工具。

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

// 我们将使用一个稀疏矩阵来可视化自由度在网格上的分布所产生的非零条目模式。这个类可以在这里找到。

#include <deal.II/lac/sparse_matrix.h>

// 我们还需要使用一个中间的稀疏模式结构，可以在这个文件中找到。

#include <deal.II/lac/dynamic_sparsity_pattern.h>

// 我们希望使用一种特殊的算法来重新计算自由度。它被声明在这里。

#include <deal.II/dofs/dof_renumbering.h>

// 而这又是C++输出所需要的。

#include <fstream>

// 最后，和 step-1 一样，我们将deal.II命名空间导入到全局范围。

using namespace dealii;
// @sect3{Mesh generation}

// 这就是前面 step-1 例子程序中产生圆形网格的函数，细化步骤较少。唯一不同的是，它通过其参数返回它所产生的网格。

void make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  for (unsigned int step = 0; step < 3; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        for (const auto v : cell->vertex_indices())
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) <=
                1e-6 * inner_radius)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}
// @sect3{Creation of a DoFHandler}

// 到目前为止，我们只有一个网格，即一些几何信息（顶点的位置）和一些拓扑信息（顶点如何与线相连，线与单元格相连，以及哪些单元格与哪些其他单元格相邻）。要使用数值算法，还需要一些逻辑信息：我们希望将自由度数字与每个顶点（或线，或单元，如果我们使用高阶元素的话）联系起来，以便以后生成描述三角形上有限元场的矩阵和矢量。

// 这个函数显示了如何做到这一点。要考虑的对象是 DoFHandler 类模板。 然而，在这之前，我们首先需要一些东西来描述这些对象中的每一个要与多少个自由度相关联。由于这是有限元空间定义的一个方面，有限元基类存储了这个信息。在目前的情况下，我们因此创建了一个描述拉格朗日元素的派生类 FE_Q 的对象。它的构造函数需要一个参数，说明元素的多项式程度，这里是1（表示一个双线性元素）；这就对应于每个顶点的一个自由度，而线和四边形内部没有自由度。如果给构造函数的值是3，我们就会得到一个双立方体元素，每个顶点有一个自由度，每条线有两个自由度，单元内有四个自由度。一般来说， FE_Q 表示具有完整多项式（即张量积多项式）的连续元素家族，直到指定的顺序。

// 我们首先需要创建一个这个类的对象，然后把它传递给 DoFHandler 对象，为自由度分配存储空间（用deal.II的行话说：我们distribute degrees of freedom）。

void distribute_dofs(DoFHandler<2> &dof_handler)
{
  const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs(finite_element);

// 现在我们已经将自由度与每个顶点的全局数字联系起来，我们想知道如何将其可视化？ 没有简单的方法可以直接将与每个顶点相关的自由度数字可视化。然而，这样的信息几乎不会真正重要，因为编号本身或多或少是任意的。还有更重要的因素，我们将在下文中展示其中一个。

// 与三角形的每个顶点相关的是一个形状函数。假设我们想解决类似拉普拉斯方程的问题，那么不同的矩阵条目将是每对这样的形状函数的梯度的积分。显然，由于形状函数只在与它们相关的顶点相邻的单元格上是非零的，所以只有当与该列和行%号相关的形状函数的支持相交时，矩阵条目才是非零的。这只是相邻形状函数的情况，因此也只是相邻顶点的情况。现在，由于顶点被上述函数 (DoFHandler::distribute_dofs), 或多或少地随机编号，矩阵中非零项的模式将有些参差不齐，我们现在就来看看它。

// 首先，我们要创建一个结构，用来存储非零元素的位置。然后，这个结构可以被一个或多个稀疏矩阵对象使用，这些对象在这个稀疏模式所存储的位置上存储条目的值。存储这些位置的类是SparsityPattern类。然而，事实证明，当我们试图立即填充这个类时，它有一些缺点：它的数据结构的设置方式是，我们需要对我们可能希望在每一行的最大条目数有一个估计。在两个空间维度上，通过 DoFHandler::max_couplings_between_dofs() 函数可以得到合理的估计值，但是在三个维度上，该函数几乎总是严重高估真实的数字，导致大量的内存浪费，有时对于所使用的机器来说太多，即使未使用的内存可以在计算稀疏模式后立即释放。为了避免这种情况，我们使用了一个中间对象DynamicSparsityPattern，该对象使用了一个不同的%内部数据结构，我们可以随后将其复制到SparsityPattern对象中，而不需要太多的开销。关于这些数据结构的一些更多信息可以在 @ref Sparsity 模块中找到）。为了初始化这个中间数据结构，我们必须给它提供矩阵的大小，在我们的例子中，矩阵是正方形的，行和列的数量与网格上的自由度相同。

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

// 然后我们在这个对象中填入非零元素的位置，考虑到目前自由度的编号。

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

// 现在我们已经准备好创建实际的稀疏模式了，以后我们可以用在我们的矩阵上。它将包含已经在DynamicSparsityPattern中集合的数据。

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

// 有了这个，我们现在可以把结果写到一个文件里。

  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

// 结果被存储在一个 .svg 文件中，矩阵中的每个非零条目都对应于图像中的一个红色方块。输出结果将显示如下。

// 如果你看一下，你会注意到稀疏性模式是对称的。这不应该是一个惊喜，因为我们没有给 DoFTools::make_sparsity_pattern 任何信息，表明我们的双线性形式可能以非对称的方式耦合形状函数。你还会注意到它有几个明显的区域，这源于编号从最粗的单元开始，然后到较细的单元；由于它们都是围绕原点对称分布的，这在稀疏模式中再次显示出来。

}
// @sect3{Renumbering of DoFs}

// 在上面产生的稀疏模式中，非零条目在对角线上延伸得很远。对于某些算法来说，例如不完全LU分解或Gauss-Seidel预处理，这是不利的，我们将展示一个简单的方法来改善这种情况。

// 请记住，为了使矩阵中的一个条目 $(i,j)$ 不为零，形状函数i和j的支持需要相交（否则在积分中，积分将到处为零，因为在某个点上，一个或另一个形状函数为零）。然而，形状函数的支撑点只有在彼此相邻的情况下才会相交，所以为了使非零条目聚集在对角线周围（其中 $i$ 等于 $j$ ），我们希望相邻的形状函数的索引（DoF编号）相差不大。

// 这可以通过一个简单的前行算法来实现，即从一个给定的顶点开始，给它的索引为0。然后，依次对其邻居进行编号，使其指数接近于原始指数。然后，他们的邻居，如果还没有被编号，也被编号，以此类推。

// 有一种算法沿着这些思路增加了一点复杂性，那就是Cuthill和McKee的算法。我们将在下面的函数中使用它来对自由度进行重新编号，从而使产生的稀疏模式在对角线周围更加本地化。该函数唯一有趣的部分是对 DoFRenumbering::Cuthill_McKee 的第一次调用，其余部分基本上与以前一样。

void renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);
}

// 再次，输出如下。请注意，非零项在对角线附近的聚类情况要比以前好得多。这种效果对于较大的矩阵来说更加明显（目前的矩阵有1260行和列，但是大的矩阵往往有几十万行）。

// 值得注意的是， DoFRenumbering 类也提供了一些其他的算法来重新编号自由度。例如，如果所有的耦合都在矩阵的下三角或上三角部分，那当然是最理想的，因为这样的话，解决线性系统就只需要向前或向后替换。当然，这对于对称稀疏模式来说是无法实现的，但在一些涉及传输方程的特殊情况下，通过列举从流入边界沿流线到流出边界的自由度，这是可能的。毫不奇怪， DoFRenumbering 也有这方面的算法。

//  @sect3{The main function}

// 最后，这是主程序。它所做的唯一一件事就是分配和创建三角形，然后创建一个 DoFHandler 对象并将其与三角形相关联，最后对其调用上述两个函数。

int main()
{
  Triangulation<2> triangulation;
  make_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  distribute_dofs(dof_handler);
  renumber_dofs(dof_handler);
}


///////////////////////////////////////////////////////////////////////////////
// kernel::multivariate::mono_bw.hpp                                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICEMSE_1_0.txt or copy at http://www.boost.org/LICEMSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_MULTIVARIATE_MONO_BW_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_MULTIVARIATE_MONO_BW_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <boost/lambda/lambda.hpp>
#include <boost/vector_space/data/lazy_difference.hpp>
#include <boost/statistics/detail/kernel/kernels/multivariate/crtp.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{
namespace multivariate{

// Overview: Multivariate kernel, common bandwidth across coordinates.
//
// Notation: Let x denote a vector of size M,
//
// Usage:
// typedef mono_bw_kernel<scalar::gaussian_kernel<T>,M> mult_;
// typedef mult_::result_type result_;
// mult_ mult(bandwidth);
// result_ r = mult(x);
template<typename K,unsigned M>
class mono_bw_kernel : K{
    public:
    typedef typename K::result_type result_type;

    //Construction
    mono_bw_kernel();
    template<typename K1> mono_bw_kernel(K1 k1); //pass bandwith, or kernel

    // Evaluate
    template<typename X> result_type profile(const X& x)const;
    template<typename X> result_type operator()(const X& x)const;
    template<typename X,typename X1>
    result_type operator()(const X& x,const X1& x1)const;

    // Access
    static unsigned dimension;
    result_type radius()const;
    result_type normalizing_constant()const;

    private:
    result_type normalizing_constant_; //no ambiguity because inherit K privately
    result_type comp_nc()const;
};

// Construction
template<typename K,unsigned M>
mono_bw_kernel<K,M>::mono_bw_kernel():K(),normalizing_constant_(comp_nc()){}

template<typename K,unsigned M>
template<typename K1>
mono_bw_kernel<K,M>::mono_bw_kernel(K1 k1):K(k1),normalizing_constant_(comp_nc()){}

// Evaluate
template<typename K,unsigned M>
template<typename X>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::profile(const X& x)const{
    static result_type init = static_cast<result_type>(0);
    const K& k = static_cast<const K&>(*this);
    result_type norm = std::accumulate(
        boost::begin(x),
        boost::end(x),
        init,
        ( lambda::_1 + (lambda::_2 * lambda::_2 ) )
    );
    norm = std::sqrt(norm);
    return k.profile(norm);
}

template<typename K,unsigned M>
template<typename X>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::operator()(const X& x)const{
    return this->profile(x) / this->normalizing_constant();
}

template<typename K,unsigned M>
template<typename X,typename X1>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::operator()(const X& x,const X1& x1)const{
    typedef vector_space::lazy_difference<X,X1> diff_;
    typedef typename range_size<X>::type size_type;
    BOOST_ASSERT(size(x) == static_cast<size_type>(size(x1)));
    BOOST_ASSERT(size(x) == static_cast<size_type>(M));
    diff_ diff(x,x1);
    return (*this)(diff);
}

// Access
template<typename K,unsigned M> unsigned mono_bw_kernel<K,M>::dimension = M;

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::radius()const{
    const K& k = static_cast<const K&>(*this);
    return k.radius();
}

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::normalizing_constant()const{ return normalizing_constant_; }

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::comp_nc()const{
    const K& k = static_cast<const K&>(*this);
    static result_type m = static_cast<result_type>(M);
    result_type nc = k.normalizing_constant();
    return std::pow(nc,m);
}

}// multivariate
}// kernel
}// detail
}// statistics
}// boost

#endif