/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_JACOBIAN_HPP
#define RW_MATH_JACOBIAN_HPP

/**
 * @file math/Jacobian.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Q.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/VelocityScrew6D.hpp>

#include <Eigen/Core>
#endif
namespace rw {
namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A Jacobian class. A jacobian with m rows and n columns.
     *
     * An ordinary robot jacobian defined over the joints 0 to n with
     * configuration \b q is expressed as a @f$ 6\times n @f$ matrix:
     * \f[
     * \robabx{0}{n}{\bf{J}}(\bf{q}) = [
     * \robabx{0}{1}{\bf{J}}(\bf{q}),
     * \robabx{1}{2}{\bf{J}}(\bf{q}),...,
     * \robabx{n-1}{n}{\bf{J}}(\bf{q}) ]
     * \f]
     *
     */
    class Jacobian
    {
      public:
        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Base;

        /**
         * @brief Creates an empty @f$ m\times n @f$ (uninitialized) Jacobian matrix
         *
         * @param m [in] number of rows
         *
         * @param n [in] number of columns
         */
        Jacobian (size_t m, size_t n) : _jac (m, n) {}

        /**
         * @brief Default constructor
         */
        Jacobian () {}

        /**
           @brief The number of rows.
         */
        size_t size1 () const { return _jac.rows (); }

        /**
           @brief The number of columns.
         */
        size_t size2 () const { return _jac.cols (); }

        /**
         * @brief Creates an empty @f$ 6\times n @f$ (uninitialized) Jacobian matrix
         *
         * @param n [in] number of columns
         */
        explicit Jacobian (size_t n) : _jac (6, n) {}

        /**
         * @brief Creates a Jacobian from a Eigen::MatrixBase
         *
         * @param r [in] an Eigen Matrix
         */
        template< class R > explicit Jacobian (const Eigen::MatrixBase< R >& r) : _jac (r) {}

        /**
         * @brief Construct zero initialized Jacobian.
         * @param size1 [in] number of rows.
         * @param size2 [in] number of columns.
         * @return zero-initialized jacobian.
         */
        static Jacobian zero (size_t size1, size_t size2)
        {
            return Jacobian (
                Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero (size1, size2));
        }

        /**
         * @brief Accessor for the internal Eigen matrix state.
         */
        Base& e () { return _jac; }

        /**
         * @brief Accessor for the internal Eigen matrix state.
         */
        const Base& e () const { return _jac; }

#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        double& operator() (size_t row, size_t column) { return _jac (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const double& operator() (size_t row, size_t column) const { return _jac (row, column); }
#else
        MATRIXOPERATOR (double);
#endif
        /**
         * @brief Get an element of the jacobian.
         * @param row [in] the row.
         * @param col [in] the column.
         * @return reference to the element.
         */
        double& elem (size_t row, size_t col) { return _jac (row, col); }

#if !defined(SWIGJAVA)

        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{a}{b}{a}{b}{\bf{J_v}} @f$
         * for transforming both the reference frame and the velocity
         * reference point from one frame \b b to another frame \b a
         *
         * @param aTb [in] @f$ \robabx{a}{b}{\bf{T}} @f$
         *
         * @return @f$ \robabcdx{a}{b}{a}{b}{\bf{J_v}} @f$
         *
         * \f[
         * \robabcdx{a}{b}{a}{b}{\bf{J_v}} =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{d}})\robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \f]
         *
         * Change the frame of reference from \b b to frame \b a and reference point
         * from frame \b a to frame \b b:
         * @f$ \robabx{a}{b}{\bf{J}} =  \robabcdx{a}{b}{a}{b}{\bf{J}_v} \cdot \robabx{b}{a}{\bf{J}}
         * @f$
         */

#endif
        explicit Jacobian (const rw::math::Transform3D<double>& aTb);

#if !defined(SWIGJAVA)
        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{a}{b}{i}{i}{\bf{J_v}} @f$
         * for transforming a velocity screw from one frame of reference \b b to
         * another frame \b a
         *
         * @param aRb [in] @f$ \robabx{a}{b}{\bf{R}} @f$
         *
         * @return @f$ \robabcdx{a}{b}{i}{i}{\bf{J}_v} @f$
         *
         * \f[
         * \robabcdx{a}{b}{i}{i}{\bf{J_v}} =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & \mathbf{0}^{3x3} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \f]
         *
         * Change the frame of reference from \b b to frame \b a :
         * @f$ \robabx{a}{c}{\bf{J}} =  \robabcdx{a}{b}{c}{c}{\bf{J}_v} \cdot \robabx{b}{c}{\bf{J}}
         * @f$
         *
         */

#endif
        explicit Jacobian (const rw::math::Rotation3D<>& aRb);

#if !defined(SWIGJAVA)

        /**
         * @brief Creates the velocity transform jacobian
         * @f$ \robabcdx{i}{i}{b}{a}{\bf{J}_v} @f$
         * for transforming the reference point of a velocity screw from one
         * frame \b b to another frame \b a
         *
         * @param aPb [in] @f$ \robabx{a}{b}{\bf{P}} @f$
         *
         * @return @f$ \robabcdx{i}{i}{b}{a}{\bf{J}_v} @f$
         *
         * \f[
         * \robabcdx{i}{i}{b}{a}{\bf{J}_v} =
         * \left[
         *  \begin{array}{cc}
         *    \bf{I}^{3x3} & S(\robabx{a}{b}{\bf{P}}) \\
         *    \bf{0}^{3x3} & \bf{I}^{3x3}
         *  \end{array}
         * \right]
         * \f]
         *
         *  transforming the reference point of a Jacobian from
         * frame \b c to frame \b d :
         * @f$ \robabx{a}{d}{\mathbf{J}} =  \robabcdx{a}{a}{c}{d}{\mathbf{J_v}} \cdot
         * \robabx{a}{c}{\mathbf{J}} @f$
         */

#endif
        explicit Jacobian (const rw::math::Vector3D<>& aPb);

        /**
         * @brief add rotation jacobian to a specific row and column in this jacobian
         * @param part
         * @param row
         * @param col
         */
        void addRotation (const rw::math::Vector3D<>& part, size_t row, size_t col);

        /**
         * @brief add position jacobian to a specific row and column in this jacobian
         * @param part
         * @param row
         * @param col
         */
        void addPosition (const rw::math::Vector3D<>& part, size_t row, size_t col);

      private:
        Base _jac;
    };

    /**
     * @brief Calculates velocity vector
     * @param Jq [in] the jacobian @f$ \mathbf{J}_{\mathbf{q}} @f$
     * @param dq [in] the joint velocity vector @f$ \dot{\mathbf{q}} @f$
     * @return the velocity vector @f$ \mathbf{\nu} @f$
     * @relates Jacobian
     */
    inline const rw::math::VelocityScrew6D<> operator* (const Jacobian& Jq, const rw::math::Q& dq)
    {
        return rw::math::VelocityScrew6D<> (Jq.e () * dq.e ());
    }

    /**
     * @brief Calculates joint velocities
     *
     * @param JqInv [in] the inverse jacobian @f$ \mathbf{J}_{\mathbf{q}}^{-1} @f$
     *
     * @param v [in] the velocity vector @f$ \mathbf{\nu} @f$
     *
     * @return the joint velocity vector @f$ \dot{\mathbf{q}} @f$
     *
     * @relates Jacobian
     */
    inline const rw::math::Q operator* (const Jacobian& JqInv, const rw::math::VelocityScrew6D<>& v)
    {
        return rw::math::Q (JqInv.e () * v.e ());
        // prod(JqInv.m(), v.m()));
    }

    /**
     * @brief Multiplies jacobians @f$ \mathbf{J} = \mathbf{J}_1 *
     * \mathbf{J}_2 @f$
     *
     * @param j1 [in] @f$ \mathbf{J}_1 @f$
     *
     * @param j2 [in] @f$ \mathbf{J}_2 @f$
     *
     * @return @f$ \mathbf{J} @f$
     *
     * @relates Jacobian
     */
    inline const Jacobian operator* (const Jacobian& j1, const Jacobian& j2)
    {
        return Jacobian (j1.e () * j2.e ());
        // return Jacobian(prod(j1.m(), j2.m()));
    }

    /**
       @brief Streaming operator.

       @relates Jacobian
    */
    inline std::ostream& operator<< (std::ostream& out, const Jacobian& v) { return out << v.e (); }

    /**
       @brief Rotates each column of \b v by \b r.

       The Jacobian must be of height 6.

       @relates Jacobian
    */
    const Jacobian operator* (const rw::math::Rotation3D<>& r, const Jacobian& v);

    /*@}*/
}
}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Jacobian
         */
        template<>
        void write (const rw::math::Jacobian& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Jacobian
         */
        template<>
        void read (rw::math::Jacobian& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file chi_squared.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <boost/math/distributions.hpp>

#include <fl/util/meta.hpp>
#include <fl/util/scalar_matrix.hpp>

#include "uniform_distribution.hpp"
#include "interface/evaluation.hpp"
#include "interface/standard_gaussian_mapping.hpp"

namespace fl
{

/**
 * \ingroup distributions
 *
 * \brief ChiSquared represents a univariate Chi-squared distribution
 * \f$\chi^2_k\f$, with \f$k \in \mathbb{N}^{*}\f$ degrees-of-freedom
 */
class ChiSquared
    : public Evaluation<ScalarMatrix>,
      public StandardGaussianMapping<ScalarMatrix, 1>
{
private:
    typedef StandardGaussianMapping<ScalarMatrix, 1> StdGaussianMappingBase;

public:
    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the \c TDistribution \c Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this \c TDistribution
     */
    typedef ScalarMatrix Variate;

    /**
     * \brief StandardVariate type which is used to sample from and mapped it
     * into the distribution space
     */
    typedef typename StdGaussianMappingBase::StandardVariate StandardVariate;

public:
    /**
     * Creates a dynamic or fixed size t-distribution.
     *
     * \param degrees_of_freedom
     *                  t-distribution degree-of-freedom
     * \param dimension Dimension of the distribution. The default is defined by
     *                  the dimension of the variable type \em Vector. If the
     *                  size of the Vector at compile time is fixed, this will
     *                  be adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit ChiSquared(Real degrees_of_freedom)
       : StdGaussianMappingBase(1),
        chi2_(degrees_of_freedom)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~ChiSquared() noexcept { }

    /**
     * \brief Returns aa t-distribution sample of the type \c Variate determined
     * by mapping a standard normal sample into the t-distribution sample space
     *
     * \param n    Standard normal sample
     *
     * \throws See Gaussian<Variate>::map_standard_normal
     */
    virtual Variate map_standard_uniform(const StandardVariate& n) const
    {
        return boost::math::quantile(chi2_, n);
    }

    /**
     * \brief Returns a t-distribution sample of the type \c Variate determined
     * by mapping a standard normal sample into the t-distribution sample space
     *
     * \param n    Standard normal sample
     *
     * \throws See Gaussian<Variate>::map_standard_normal
     */
    Variate map_standard_normal(const StandardVariate& n) const override
    {
        return map_standard_uniform(uniform_.map_standard_normal(n));
    }

    /**
     * \brief Returns the log probability of the given sample \c variate
     *
     * \param variate sample which should be evaluated
     *
     * \throws See Gaussian<Variate>::has_full_rank()
     */
    Real log_probability(const Variate& variate) const override
    {
        assert(variate.size() == 1);
        return std::log(probability(variate));
    }

    /**
     * \brief Evaluates the probability for the specified variate.
     *
     * \param variate Sample \f$x\f$ to evaluate
     *
     * \return \f$p(x)\f$
     */
    Real probability(const Variate& variate) const override
    {
        assert(variate.size() == 1);
        return boost::math::pdf(chi2_, variate);
    }

    /**
     * \brief Returns t-distribution degree-of-freedom
     */
    Real degrees_of_freedom() const
    {
        return chi2_.degrees_of_freedom();
    }

    /**
     * \brief Sets t-distribution degree-of-freedom
     */
    void degrees_of_freedom(Real dof)
    {
        chi2_ = boost::math::chi_squared_distribution<Real>(dof);
    }

protected:
    /** \cond internal */
    UniformDistribution uniform_;
    boost::math::chi_squared_distribution<Real> chi2_;
    /** \endcond */
};

}


#pragma once

#include "PolynomialBasisGen.hh"
#include "RBFKernel.hh"
#include "FiniteDifferentiator.hh"
#include <Eigen/Dense>
#include <utility>
#include <vector>

namespace kt84 {

template <int _DimIn, int _DimOut, class _RBFKernel_Core, int _DegreePolynomial>
struct RBF
    : public FiniteDifferentiator<RBF<_DimIn, _DimOut, _RBFKernel_Core, _DegreePolynomial>, _DimIn, _DimOut>
{
    enum {
        DimIn  = _DimIn,
        DimOut = _DimOut,
        DegreePolynomial = _DegreePolynomial,
    };

    typedef Eigen::Matrix<double, DimIn , 1> Point;                 // TODO: treat 1x1 matrix as scalar using Matrix11ToScalar
    typedef Eigen::Matrix<double, DimOut, 1> Value;
    typedef Eigen::Matrix<double, DimOut, DimIn> Gradient;
    typedef PolynomialBasisGenT<DimIn, DegreePolynomial> PolynomialBasisGen;
    typedef RBFKernel_Bivariate<DimIn, _RBFKernel_Core> Kernel;
    typedef std::pair<Point, Value> Constraint;

    std::vector<Constraint> constraints;
    Kernel kernel;
    Eigen::Matrix<double, -1, DimOut> weights;
    Eigen::MatrixXd A_matrix;
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> A_factorized;

    void clear_constraints() {
        constraints.clear();
    }
    void add_constraint(const Point& point, const Value& value) {
        constraints.push_back(Constraint(point, value));
    }
    void factorize() {
        const int P = PolynomialBasisGen::DimOut;
        const size_t n = constraints.size();
        const int m = n + P;
        A_matrix = Eigen::MatrixXd::Zero(m, m);
        for (size_t i = 0; i < n; ++i) {
            const Point& point_i = constraints[i].first;
            // rbf part
            A_matrix(i, i) = kernel.univariate(0);
            for (size_t j = i + 1; j < n; ++j) {
                const Point& point_j = constraints[j].first;
                A_matrix(i, j) = A_matrix(j, i) = kernel(point_i, point_j);
            }
            // polynomial part
            A_matrix.block<P, 1>(n, i) << PolynomialBasisGen::basis(point_i);
            A_matrix.block<1, P>(i, n) = A_matrix.block<P, 1>(n, i).transpose();
        }
        A_factorized.compute(A_matrix);         // factorize
    }
    void solve() {
        const int P = PolynomialBasisGen::DimOut;
        const size_t n = constraints.size();
        const int m = n + P;
        Eigen::Matrix<double, -1, DimOut> b;
        b.setZero(m, DimOut);
        // constraint part
        for (size_t i = 0; i < n; ++i) {
            const Value& value_i = constraints[i].second;
            b.row(i).transpose() << value_i;
        }
        // polynomial part is just 0
        weights = A_factorized.solve(b);        // solve
    }
    void factorize_and_solve() {
        factorize();
        solve();
    }
    Value operator()(const Point& point) const {
        const int P = PolynomialBasisGen::DimOut;
        Value result = Value::Zero();
        // rbf part
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            result += kernel(point, point_i) * weights.row(i).transpose();
        }
        // polynomial part
        auto basis = PolynomialBasisGen::basis(point);
        result += (basis.transpose() * weights.bottomRows(P)).transpose();
        return result;
    }
    Gradient gradient(const Point& point) const {
        const int P = PolynomialBasisGen::DimOut;
        Gradient result = Gradient::Zero();
        // rbf part
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            result += weights.row(i).transpose() * kernel.gradient(point, point_i);
        }
        // polynomial part
        auto b_gradient = PolynomialBasisGen::gradient(point);
        result += weights.bottomRows(P).transpose() * b_gradient;
        return result;
    }
};

}


/**
 * @file expfittedupwind_main.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>

#include "expfittedupwind.h"

int main() {
  // Define Mesh-independent Data:
  //====================
  // Your code goes here
  //====================

  // Output file
  std::ofstream L2output;
  L2output.open("L2error.txt");
  L2output << "No. of dofs, L2 error" << std::endl;

  // generate a mesh hierarchy:
  unsigned int reflevels = 6;
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  auto top_mesh = builder.Build();

  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(top_mesh,
                                                              reflevels);
  lf::refinement::MeshHierarchy& multi_mesh{*multi_mesh_p};
  multi_mesh.PrintInfo(std::cout);

  // get number of levels:
  auto L = multi_mesh.NumLevels();

  // perform computations on all levels:
  for (int l = 0; l < L; ++l) {
    // Compute finite element solution and compute L2 error on current level:
    double L2_err = 1.0;

    // get current mesh and fe space
    auto mesh_p = multi_mesh.getMesh(l);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};
    const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

    //====================
    // Your code goes here
    //====================

    L2output << N_dofs << ", " << L2_err << std::endl;
    std::cout << N_dofs << "," << L2_err << std::endl;
  }

  L2output.close();

  // Plot the computed L2 error
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/L2error.txt " CURRENT_BINARY_DIR "/results.eps");

  return 0;
}


/*
 * H2L2.cpp
 *
 *  Created on: 13.03.2018
 *      Author: thies
 */

#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/timer.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>
#include <deal.II/lac/vector.h>
#include <norms/H2L2.h>
#include <stddef.h>
#include <cmath>
#include <iostream>
#include <vector>

namespace wavepi {
namespace norms {

using namespace dealii;

inline double square(const double x) { return x * x; }
inline double pow4(const double x) { return x * x * x * x; }

template <int dim>
H2L2<dim>::H2L2(double alpha, double beta) : alpha_(alpha), beta_(beta) {}

template <int dim>
double H2L2<dim>::norm(const DiscretizedFunction<dim>& u) const {
  auto mesh = u.get_mesh();

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv = u.calculate_derivative();

  // using deriv.calculate_derivative feels wrong, better use a specialized formula.
  auto deriv2 = u.calculate_second_derivative();

  double result = 0;

  for (size_t i = 0; i < mesh->length(); i++) {
    double nrm2        = mesh->get_mass_matrix(i)->matrix_norm_square(u[i]);
    double nrm2_deriv  = mesh->get_mass_matrix(i)->matrix_norm_square(deriv[i]);
    double nrm2_deriv2 = mesh->get_mass_matrix(i)->matrix_norm_square(deriv2[i]);

    // + trapezoidal rule in time:
    if (i > 0)
      result += (nrm2 + alpha_ * nrm2_deriv + beta_ * nrm2_deriv2) / 2 *
                (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (nrm2 + alpha_ * nrm2_deriv + beta_ * nrm2_deriv2) / 2 *
                (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return std::sqrt(result);
}

template <int dim>
double H2L2<dim>::dot(const DiscretizedFunction<dim>& u, const DiscretizedFunction<dim>& v) const {
  auto mesh     = u.get_mesh();
  double result = 0.0;

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv  = u.calculate_derivative();
  auto Vderiv = v.calculate_derivative();

  // using deriv.calculate_derivative feels wrong, better use a specialized formula.
  auto deriv2  = u.calculate_second_derivative();
  auto Vderiv2 = v.calculate_second_derivative();

  for (size_t i = 0; i < mesh->length(); i++) {
    double doti        = mesh->get_mass_matrix(i)->matrix_scalar_product(u[i], v[i]);
    double doti_deriv  = mesh->get_mass_matrix(i)->matrix_scalar_product(deriv[i], Vderiv[i]);
    double doti_deriv2 = mesh->get_mass_matrix(i)->matrix_scalar_product(deriv2[i], Vderiv2[i]);

    // + trapezoidal rule in time
    if (i > 0)
      result += (doti + alpha_ * doti_deriv + beta_ * doti_deriv2) / 2 *
                (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (doti + alpha_ * doti_deriv + beta_ * doti_deriv2) / 2 *
                (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return result;
}

template <int dim>
void H2L2<dim>::dot_transform(DiscretizedFunction<dim>& u) {
  u.mult_mass();
  dot_solve_mass_and_transform(u);
}

template <int dim>
void H2L2<dim>::dot_transform_inverse(DiscretizedFunction<dim>& u) {
  u.solve_mass();
  dot_mult_mass_and_transform_inverse(u);
}

template <int dim>
void H2L2<dim>::dot_solve_mass_and_transform(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  // X = (T + \alpha D^t T D + \beta D_2^t T D_2) * M,
  // M (blocks of mass matrices) is already taken care of, D = derivative, T = trapezoidal rule

  auto dx  = u.calculate_derivative();
  auto d2x = u.calculate_second_derivative();

  // trapezoidal rule
  // (has to happen between D and D^t for dx)
  for (size_t i = 0; i < mesh->length(); i++) {
    double factor = 0.0;

    if (i > 0) factor += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;
    if (i < mesh->length() - 1) factor += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;

    dx[i] *= factor;
    d2x[i] *= factor;
    u[i] *= factor;
  }

  auto dtdx   = dx.calculate_derivative_transpose();
  auto d2td2x = d2x.calculate_second_derivative_transpose();

  // add derivative terms
  u.add(alpha_, dtdx);
  u.add(beta_, d2td2x);
}

template <int dim>
void H2L2<dim>::factorize_matrix(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  deallog << "factorizing matrix" << std::endl;

  SparsityPattern pattern(mesh->length(), mesh->length(), 5);

  for (size_t i = 0; i < 3; i++)
    for (size_t j = 0; j < 3; j++)
      pattern.add(i, j);

  for (size_t i = 3; i < mesh->length() - 3; i++) {
    // fill row i and column i
    for (int j = -2; j <= 2; j++) {
      pattern.add(i, i + j);
      pattern.add(i + j, i);
    }
  }

  for (size_t i = 0; i < 3; i++)
    for (size_t j = 0; j < 3; j++)
      pattern.add(mesh->length() - 1 - i, mesh->length() - 1 - j);

  pattern.compress();

  // coefficients of trapezoidal rule
  std::vector<double> lambdas(mesh->length(), 0.0);

  for (size_t i = 0; i < mesh->length(); i++) {
    if (i > 0) lambdas[i] += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;

    if (i < mesh->length() - 1) lambdas[i] += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;
  }

  SparseMatrix<double> matrix(pattern);

  double p20 = 1.0 * 16 / pow4(mesh->get_time(2) - mesh->get_time(0));
  double p10 = 1.0 / pow4(mesh->get_time(1) - mesh->get_time(0));
  double p31 = 1.0 * 16 / pow4(mesh->get_time(3) - mesh->get_time(1));
  double p42 = 1.0 * 16 / pow4(mesh->get_time(4) - mesh->get_time(2));

  matrix.set(0, 0, lambdas[0] * p10 + lambdas[1] * p20);
  matrix.set(1, 1, 4 * lambdas[0] * p10 + 4 * lambdas[1] * p20 + lambdas[2] * p31);
  matrix.set(2, 2, lambdas[0] * p10 + lambdas[1] * p20 + 4 * lambdas[2] * p31 + lambdas[3] * p42);

  matrix.set(0, 1, -2 * lambdas[0] * p10 - 2 * lambdas[1] * p20);
  matrix.set(1, 0, -2 * lambdas[0] * p10 - 2 * lambdas[1] * p20);

  matrix.set(0, 2, lambdas[0] * p10 + lambdas[1] * p20);
  matrix.set(2, 0, lambdas[0] * p10 + lambdas[1] * p20);

  matrix.set(1, 2, -2 * lambdas[0] * p10 - 2 * lambdas[1] * p20 - 2 * lambdas[2] * p31);
  matrix.set(2, 1, -2 * lambdas[0] * p10 - 2 * lambdas[1] * p20 - 2 * lambdas[2] * p31);

  for (size_t i = 3; i < mesh->length() - 3; i++) {
    // fill row i and column i

    double p20  = 1.0 * 16 / pow4(mesh->get_time(i + 2) - mesh->get_time(i));
    double p0m2 = 1.0 * 16 / pow4(mesh->get_time(i - 2) - mesh->get_time(i));
    double p1m1 = 1.0 * 16 / pow4(mesh->get_time(i + 1) - mesh->get_time(i - 1));

    matrix.set(i, i, lambdas[i + 1] * p20 + 4 * lambdas[i] * p1m1 + lambdas[i - 1] * p0m2);

    matrix.set(i, i - 1, -2 * lambdas[i - 1] * p0m2 - 2 * lambdas[i] * p1m1);
    matrix.set(i - 1, i, -2 * lambdas[i - 1] * p0m2 - 2 * lambdas[i] * p1m1);

    matrix.set(i, i + 1, -2 * lambdas[i + 1] * p20 - 2 * lambdas[i] * p1m1);
    matrix.set(i + 1, i, -2 * lambdas[i + 1] * p20 - 2 * lambdas[i] * p1m1);

    matrix.set(i, i + 2, lambdas[i + 1] * p20);
    matrix.set(i + 2, i, lambdas[i + 1] * p20);

    matrix.set(i, i - 2, lambdas[i - 1] * p0m2);
    matrix.set(i - 2, i, lambdas[i - 1] * p0m2);
  }

  // (symmetric to the first entries)
  size_t N = mesh->length() - 1;  // makes it easier to read

  p20 = 1.0 * 16 / pow4(mesh->get_time(N - 2) - mesh->get_time(N - 0));
  p10 = 1.0 / pow4(mesh->get_time(N - 1) - mesh->get_time(N - 0));
  p31 = 1.0 * 16 / pow4(mesh->get_time(N - 3) - mesh->get_time(N - 1));
  p42 = 1.0 * 16 / pow4(mesh->get_time(N - 4) - mesh->get_time(N - 2));

  matrix.set(N - 0, N - 0, lambdas[N - 0] * p10 + lambdas[N - 1] * p20);
  matrix.set(N - 1, N - 1, 4 * lambdas[N - 0] * p10 + 4 * lambdas[N - 1] * p20 + lambdas[N - 2] * p31);
  matrix.set(N - 2, N - 2,
             lambdas[N - 0] * p10 + lambdas[N - 1] * p20 + 4 * lambdas[N - 2] * p31 + lambdas[N - 3] * p42);

  matrix.set(N - 0, N - 1, -2 * lambdas[N - 0] * p10 - 2 * lambdas[N - 1] * p20);
  matrix.set(N - 1, N - 0, -2 * lambdas[N - 0] * p10 - 2 * lambdas[N - 1] * p20);

  matrix.set(N - 0, N - 2, lambdas[N - 0] * p10 + lambdas[N - 1] * p20);
  matrix.set(N - 2, N - 0, lambdas[N - 0] * p10 + lambdas[N - 1] * p20);

  matrix.set(N - 1, N - 2, -2 * lambdas[N - 0] * p10 - 2 * lambdas[N - 1] * p20 - 2 * lambdas[N - 2] * p31);
  matrix.set(N - 2, N - 1, -2 * lambdas[N - 0] * p10 - 2 * lambdas[N - 1] * p20 - 2 * lambdas[N - 2] * p31);

  matrix *= beta_;

  // H1 part (+ trapezoidal rule)
  SparseMatrix<double> matrixH1(pattern);

  double sq20 = 1.0 / square(mesh->get_time(2) - mesh->get_time(0));
  double sq10 = 1.0 / square(mesh->get_time(1) - mesh->get_time(0));
  double sq31 = 1.0 / square(mesh->get_time(3) - mesh->get_time(1));

  matrixH1.set(0, 0, lambdas[1] * sq20 + lambdas[0] * sq10);
  matrixH1.set(1, 1, lambdas[2] * sq31 + lambdas[0] * sq10);
  matrixH1.set(0, 1, -lambdas[0] * sq10);
  matrixH1.set(1, 0, -lambdas[0] * sq10);

  for (size_t i = 2; i < mesh->length() - 2; i++) {
    // fill row i and column i

    double sq20  = 1.0 / square(mesh->get_time(i + 2) - mesh->get_time(i));
    double sq0m2 = 1.0 / square(mesh->get_time(i) - mesh->get_time(i - 2));

    matrixH1.set(i, i, lambdas[i + 1] * sq20 + lambdas[i - 1] * sq0m2);

    matrixH1.set(i, i - 2, -lambdas[i - 1] * sq0m2);
    matrixH1.set(i - 2, i, -lambdas[i - 1] * sq0m2);

    matrixH1.set(i, i + 2, -lambdas[i + 1] * sq20);
    matrixH1.set(i + 2, i, -lambdas[i + 1] * sq20);
  }

  // (symmetric to the first entries)
  sq20 = 1.0 / square(mesh->get_time(N - 2) - mesh->get_time(N));
  sq10 = 1.0 / square(mesh->get_time(N - 1) - mesh->get_time(N));
  sq31 = 1.0 / square(mesh->get_time(N - 3) - mesh->get_time(N - 1));

  matrixH1.set(N, N - 0, lambdas[N - 1] * sq20 + lambdas[N] * sq10);
  matrixH1.set(N - 1, N - 1, lambdas[N - 2] * sq31 + lambdas[N] * sq10);
  matrixH1.set(N, N - 1, -lambdas[N] * sq10);
  matrixH1.set(N - 1, N, -lambdas[N] * sq10);

  matrix.add(alpha_, matrixH1);

  // L2 part (+ trapezoidal rule)
  for (size_t i = 0; i < mesh->length(); i++)
    matrix.add(i, i, lambdas[i]);

  umfpack.factorize(matrix);
}

template <int dim>
void H2L2<dim>::dot_mult_mass_and_transform_inverse(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  LogStream::Prefix p("h2l2_transform");
  Timer timer;
  timer.start();

  if (umfpack.n() != mesh->length()) factorize_matrix(mesh);

  // just to be sure
  for (size_t i = 0; i < mesh->length(); i++)
    Assert(u[i].size() == u[0].size(), ExcInternalError());

  // solve for every DoF
  Vector<double> tmp(mesh->length());

  for (size_t i = 0; i < u[0].size(); i++) {
    for (size_t j = 0; j < mesh->length(); j++)
      tmp[j] = u[j][i];

    umfpack.solve(tmp);

    for (size_t j = 0; j < mesh->length(); j++)
      u[j][i] = tmp[j];
  }

  deallog << "solved in " << Util::format_duration(timer.wall_time()) << std::endl;
}

template <int dim>
std::string H2L2<dim>::name() const {
  return "H²([0,T], L²(Ω))";
}

template <int dim>
std::string H2L2<dim>::unique_id() const {
  return "H²([0,T], L²(Ω)) with α=" + std::to_string(alpha_) + ", β=" + std::to_string(beta_);
}

template class H2L2<1>;
template class H2L2<2>;
template class H2L2<3>;

} /* namespace norms */
} /* namespace wavepi */


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Written by Cornelius Steinhardt

#ifndef ITL_GMRES_INCLUDE
#define ITL_GMRES_INCLUDE

#include <algorithm>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/multi_vector.hpp>
#include <boost/numeric/mtl/operation/givens.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>

namespace itl {

/// Generalized Minimal Residual method (without restart)
/** It computes at most kmax_in iterations (or size(x) depending on what is smaller)
    regardless on whether the termination criterion is reached or not.   **/
template < typename Matrix, typename Vector, typename LeftPreconditioner, typename RightPreconditioner, typename Iteration >
int gmres_full(const Matrix &A, Vector &x, const Vector &b,
               LeftPreconditioner &L, RightPreconditioner &R, Iteration& iter)
{
    using mtl::size; using mtl::irange; using mtl::iall; using std::abs; using std::sqrt;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar                zero= math::zero(Scalar());
    Scalar                      rho, nu, hr;
    Size                        k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));
    Vector                      r0(b - A *x), r(solve(L,r0)), va(resource(x)), va0(resource(x)), va00(resource(x));
    mtl::matrix::multi_vector<Vector>   V(Vector(resource(x), zero), kmax+1);
    mtl::vector::dense_vector<Scalar>   s(kmax+1, zero), c(kmax+1, zero), g(kmax+1, zero), y(kmax, zero);  // replicated in distributed solvers
    mtl::matrix::dense2D<Scalar>        H(kmax+1, kmax);                                             // dito
    H= 0;

    rho= g[0]= two_norm(r);
    if (iter.finished(rho))
	return iter;
    V.vector(0)= r / rho;
    H= zero;

    // GMRES iteration
    for (k= 0; k < kmax ; ++k, ++iter) {
        va0= A * Vector(solve(R, V.vector(k)));
        V.vector(k+1)= va= solve(L,va0);
	// orth(V, V[k+1], false);
        // modified Gram Schmidt method
        for (Size j= 0; j < k+1; j++) {
	    H[j][k]= dot(V.vector(j), V.vector(k+1));
	    V.vector(k+1)-= H[j][k] * V.vector(j);
        }
        H[k+1][k]= two_norm(V.vector(k+1));
        //reorthogonalize
        for(Size j= 0; j < k+1; j++) {
	    hr= dot(V.vector(k+1), V.vector(j));
            H[j][k]+= hr;
            V.vector(k+1)-= hr * V.vector(j);
        }
        H[k+1][k]= two_norm(V.vector(k+1));
	if (H[k+1][k] != zero)                // watch for breakdown
            V.vector(k+1)*= 1. / H[k+1][k];

        // k Given's rotations
	for(Size i= 0; i < k; i++)
	    mtl::matrix::givens<mtl::matrix::dense2D<Scalar> >(H, H[i][k-1], H[i+1][k-1]).trafo(i);

       nu= sqrt(H[k][k]*H[k][k]+H[k+1][k]*H[k+1][k]);
       if(nu != zero){
            c[k]=  H[k][k]/nu;
            s[k]= -H[k+1][k]/nu;
            H[k][k]=c[k]*H[k][k]-s[k]*H[k+1][k];
            H[k+1][k]=0;
 	    mtl::vector::givens<mtl::vector::dense_vector<Scalar> >(g, c[k], s[k]).trafo(k);
        }
	rho= abs(g[k+1]);
    }

    //reduce k, to get regular matrix
    while (k > 0 && abs(g[k-1]<= iter.atol())) k--;

    // iteration is finished -> compute x: solve H*y=g as far as rank of H allows
    irange                  range(k);
    for (; !range.empty(); --range) {
	try {
	    y[range]= lu_solve(H[range][range], g[range]);
	} catch (mtl::matrix_singular) { continue; } // if singular then try with sub-matrix
	break;
    }

    if (range.finish() < k)
  	std::cerr << "GMRES orhogonalized with " << k << " vectors but matrix singular, can only use "
		  << range.finish() << " vectors!\n";
    if (range.empty())
        return iter.fail(2, "GMRES did not find any direction to correct x");
    x+= Vector(solve(R, Vector(V.vector(range)*y[range])));

    r= b - A*x;
    return iter.terminate(r);
}

/// Generalized Minimal Residual method with restart
template < typename Matrix, typename Vector, typename LeftPreconditioner,
           typename RightPreconditioner, typename Iteration >
int gmres(const Matrix &A, Vector &x, const Vector &b,
          LeftPreconditioner &L, RightPreconditioner &R,
	  Iteration& iter, typename mtl::Collection<Vector>::size_type restart)
{
     do {
	 Iteration inner(iter);
	 inner.set_max_iterations(std::min(int(iter.iterations()+restart), iter.max_iterations()));
	 inner.suppress_resume(true);
	 gmres_full(A, x, b, L, R, inner);
	 iter.update_progress(inner);
     } while (!iter.finished());

     return iter;
}

/// Solver class for GMRES; right preconditioner ignored (prints warning if not identity)
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class gmres_solver
{
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit gmres_solver(const LinearOperator& A, size_t restart= 8)
      : A(A), restart(restart), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    gmres_solver(const LinearOperator& A, size_t restart, const Preconditioner& L)
      : A(A), restart(restart), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    gmres_solver(const LinearOperator& A, size_t restart, const Preconditioner& L, const RightPreconditioner& R)
      : A(A), restart(restart), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceB, typename HilbertSpaceX, typename Iteration >
    int solve(const HilbertSpaceB& b, HilbertSpaceX& x, Iteration& iter) const
    {
	return gmres(A, x, b, L, R, iter, restart);
    }

    /// Perform one GMRES iteration on linear system
    template < typename HilbertSpaceB, typename HilbertSpaceX >
    int solve(const HilbertSpaceB& b, HilbertSpaceX& x) const
    {
	itl::basic_iteration<double> iter(x, 1, 0, 0);
	return solve(b, x, iter);
    }

  private:
    const LinearOperator& A;
    size_t                restart;
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_GMRES_INCLUDE


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

/*
 Copyright (C) 2015 Peter Caspers

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

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

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

/*! \file qg1dlocalvolmodel.hpp
    \brief base class for one factor quasi gaussian models with local
           volatility
*/

#ifndef quantlib_quasigaussian1d_model_hpp
#define quantlib_quasigaussian1d_model_hpp

#include <ql/handle.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/integrals/integral.hpp>
#include <ql/models/model.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

#include <boost/make_shared.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

// the integrator_ is used for h,G,yApprox,varSApprox => use different ones ?
// for the linear model h is overwritten

namespace QuantLib {

class Qg1dLocalVolModel : public TermStructureConsistentModel {
  public:
    /*! the model is specified by a function \kappa(t) and a function g(t,x,y),
        with \kappa(t) = -h'(t) / h(t), \sigma_f(t,T) = g(t)h(T), the HJM
        specification
        df(t,T) = \sigma_f(t,T) ( ( \int_t^T sigma_f(t,u) du ) du + dW(t) )
        and
        dx = (y - \kappa x) dt + \sigma_f(t,t) dW
        dy = (\sigma_f(t,t)^2 - 2 \kappa y) dt
        x(0) = y(0) = 0 */
    Qg1dLocalVolModel(const Handle<YieldTermStructure> &yts);

    /* core interface, these methods must be implemented by derived classes
       the other virtual methods may be overwritten by more efficient versions
       applicable to the particular model specification. */
    virtual Real kappa(const Real t) const = 0;
    virtual Real g(const Real t, const Real x, const Real y) const = 0;

    virtual Real h(const Real t) const;

    /*! \int_t^T h(s) ds / h(t) */
    virtual Real G(const Real t, const Real T) const;

    virtual Real sigma_f(const Real t, const Real T, const Real x,
                         const Real y) const;

    Real zerobond(const Real T, const Real t, const Real x, const Real y,
                  const Handle<YieldTermStructure> &yts =
                      Handle<YieldTermStructure>()) const;

    /*! swap rate is calculated with forward = discount, no indexed coupons
        T0 is the start date of the swap, fixedTimes the payment times of
        the fixed leg and taus the year fractions of the fixed leg */
    Real swapRate(const Real T0, const Real t,
                  const std::vector<Real> &fixedTimes,
                  const std::vector<Real> &taus, const Real x, const Real y,
                  const Handle<YieldTermStructure> &yts =
                      Handle<YieldTermStructure>()) const;

    Real dSwapRateDx(const Real T0, const Real t,
                     const std::vector<Real> &fixedTimes,
                     const std::vector<Real> &taus, const Real x, const Real y,
                     const Handle<YieldTermStructure> &yts =
                         Handle<YieldTermStructure>()) const;

    Real d2SwapRateDx2(const Real T0, const Real t,
                       const std::vector<Real> &fixedTimes,
                       const std::vector<Real> &taus, const Real x,
                       const Real y, const Handle<YieldTermStructure> &yts =
                                         Handle<YieldTermStructure>()) const;

    /*! local volatility using yApprox and sInvX (see Piterbarg, equation 13.19
        and what follows immediately after that), if numericalInversion is true,
        otherwise xi is used (Piterbarg, prop 13.1.8) */
    Real
    phi(const Real t, const Real s, const Real T0,
        const std::vector<Real> &fixedTimes, const std::vector<Real> &taus,
        const Handle<YieldTermStructure> &yts = Handle<YieldTermStructure>(),
        bool numericalInversion = false) const;

    Disposable<std::vector<Real> >
    phi(const Real t, const std::vector<Real> &s, const Real T0,
        const std::vector<Real> &fixedTimes, const std::vector<Real> &taus,
        const Handle<YieldTermStructure> &yts = Handle<YieldTermStructure>(),
        bool numericalInversion = false) const;

    /*! date based variants, only the forwarding curve from the swap index
        (if given) is used and no indexed coupons are used, see above */
    Real zerobond(const Date &maturiy, const Date &referenceDate, const Real x,
                  const Real y, const Handle<YieldTermStructure> &yts =
                                    Handle<YieldTermStructure>());

    Real swapRate(const Date &startDate, const Date &referenceDate,
                  const boost::shared_ptr<SwapIndex> &index,
                  const Period &tenor, const Real x, const Real y) const;

    Real dSwapRateDx(const Date &startDate, const Date &referenceDate,
                     const boost::shared_ptr<SwapIndex> &index,
                     const Period &tenor, const Real x, const Real y) const;

    Real d2SwapRateDx2(const Date &startDate, const Date &referenceDate,
                       const boost::shared_ptr<SwapIndex> &index,
                       const Period &tenor, const Real x, const Real y) const;

    /*! utilitiy function that fills T0, tau and a times vector based on
      a given swap index */
    void timesAndTaus(const Date &startDate,
                      const boost::shared_ptr<SwapIndex> &index,
                      const Period &tenor, Real &T0, std::vector<Real> &times,
                      std::vector<Real> &taus) const;

    virtual Real yApprox(const Real t) const;

    /*! this is \overline{x(t)} in Piterbarg */
    virtual Real xApprox(const Real t, const Real T0,
                         const std::vector<Real> &fixedTimes,
                         const std::vector<Real> &taus,
                         const Handle<YieldTermStructure> &yts) const;

    /*! approximate inversion of s in the sense of
        13.28 in Piterbarg */
    virtual Real xi(const Real t, const Real T0,
                    const std::vector<Real> &fixedTimes,
                    const std::vector<Real> &taus,
                    const Handle<YieldTermStructure> &yts, const Real s) const;

    virtual Disposable<std::vector<Real> >
    xi(const Real t, const Real T0, const std::vector<Real> &fixedTimes,
       const std::vector<Real> &taus, const Handle<YieldTermStructure> &yts,
       const std::vector<Real> &s) const;

    /*! numerical inversion of s with y = yApprox fixed,
        i.e. this is X(t,s) in Piterbarg's notation */
    Real sInvX(const Real t, const Real T0, const std::vector<Real> &fixedTimes,
               const std::vector<Real> &taus,
               const Handle<YieldTermStructure> &yts, const Real s) const;

    /*! Var(S(T)) approximation like in Piterbarg, remark 13.1.7 */
    Real varSApprox(const Real T, const Real T0,
                    const std::vector<Real> &fixedTimes,
                    const std::vector<Real> &taus,
                    const Handle<YieldTermStructure> &yts) const;

  protected:
    /*! compute swap rate, the first and second derivative w.r.t. x
        (since they share a lot of intermediate results this is more
        efficient than computing each single number) */
    void swapRate_d0_d1_d2(const Real T0, const Real t,
                           const std::vector<Real> &fixedTimes,
                           const std::vector<Real> &taus, const Real x,
                           const Real y, const Handle<YieldTermStructure> &yts,
                           Real &result_d0, Real &result_d1, Real &result_d2,
                           const bool compute_d0, const bool compute_d1,
                           const bool compute_d2) const;

    /*! sigma_f(t,t,0,0)^2*h(t)^{-2}, precondition (not checked) is t > 0 */
    virtual Real sigma_r_0_0_h_sqr(const Real t) const;

    /*! sigma_f(t,t,0.0)^2*dS/dx(s,0,0)^2 */
    virtual Real
    sigma_r_0_0_dSdx_sqr(const Real T0, const Real t,
                         const std::vector<Real> &fixedTimes,
                         const std::vector<Real> &taus,
                         const Handle<YieldTermStructure> &yts) const;

    boost::shared_ptr<Integrator> integrator_;

  private:
    Real sInvX_helper(const Real t, const Real T0,
                      const std::vector<Real> &fixedTimes,
                      const std::vector<Real> &taus,
                      const Handle<YieldTermStructure> &yts, const Real s,
                      const Real x) const;
};

// inline

inline Real Qg1dLocalVolModel::h(const Real t) const {
    return std::exp(-integrator_->operator()(
        boost::bind(&Qg1dLocalVolModel::kappa, this, _1), 0.0, t));
}

inline Real Qg1dLocalVolModel::G(const Real t, const Real T) const {
    return integrator_->operator()(boost::bind(&Qg1dLocalVolModel::h, this, _1),
                                   t, T);
}

inline Real Qg1dLocalVolModel::sigma_f(const Real t, const Real T, const Real x,
                                       const Real y) const {
    return g(t, x, y) * h(T);
}

inline Real Qg1dLocalVolModel::yApprox(const Real t) const {
    if (t < 1E-10)
        return 0.0;
    Real tmp = h(t);
    return tmp * tmp *
           integrator_->operator()(
               boost::bind(&Qg1dLocalVolModel::sigma_r_0_0_h_sqr, this, _1),
               0.0, t);
}

inline Real Qg1dLocalVolModel::sigma_r_0_0_h_sqr(const Real t) const {
    Real tmp = g(t, 0.0, 0.0); // this is sigma_f(t, t, 0.0, 0.0) / h(t);
    return tmp * tmp;
}

inline Real Qg1dLocalVolModel::sigma_r_0_0_dSdx_sqr(
    const Real T0, const Real t, const std::vector<Real> &fixedTimes,
    const std::vector<Real> &taus,
    const Handle<YieldTermStructure> &yts) const {
    Real tmp = sigma_f(t, t, 0.0, 0.0) *
               dSwapRateDx(T0, t, fixedTimes, taus, 0.0, 0.0, yts);
    return tmp * tmp;
}

inline Real Qg1dLocalVolModel::sInvX_helper(
    const Real t, const Real T0, const std::vector<Real> &fixedTimes,
    const std::vector<Real> &taus, const Handle<YieldTermStructure> &yts,
    const Real s, const Real x) const {
    Real y = yApprox(t);
    return swapRate(T0, t, fixedTimes, taus, x, y, yts) - s;
}

inline Real Qg1dLocalVolModel::sInvX(const Real t, const Real T0,
                                     const std::vector<Real> &fixedTimes,
                                     const std::vector<Real> &taus,
                                     const Handle<YieldTermStructure> &yts,
                                     const Real s) const {
    Brent b;
    boost::function<Real(Real)> f =
        boost::bind(&Qg1dLocalVolModel::sInvX_helper, this, t, T0, fixedTimes,
                    taus, yts, s, _1);
    return b.solve(f, 1E-7, 0.0, 0.01);
}

inline Real
Qg1dLocalVolModel::varSApprox(const Real T, const Real T0,
                              const std::vector<Real> &fixedTimes,
                              const std::vector<Real> &taus,
                              const Handle<YieldTermStructure> &yts) const {
    return integrator_->operator()(
        boost::bind(&Qg1dLocalVolModel::sigma_r_0_0_dSdx_sqr, this, T0, _1,
                    fixedTimes, taus, yts),
        0.0, T);
}

} // namespace QuantLib

#endif


#include <Engine/MeshEdit/Simulate.h>

#include <math.h>
#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

void Simulate::Clear() {
	this->positions.clear();
	this->velocity.clear();
}

bool Simulate::Init() {
	//Clear();
	g = -9.8;
	stiff = 15000;
	//g = 0;
	isfast =true;
	x.resize(3 * positions.size());
	y_.resize(3 * positions.size());
	xx.resize(3 * positions.size());
	iter = 3;
	SetFix();

	this->velocity.resize(positions.size());
	for (int i = 0; i < positions.size(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			this->velocity[i][j] = 0;
			x[3 * i + j] = positions[i][j];
			y_[3 * i + j] = positions[i][j];
			xx[3 * i + j] = positions[i][j];
		}
	}

	mass.resize(3 * positions.size());
	M.resize(3 * positions.size(), 3 * positions.size());
	M.fill(0);

	force_ext.resize(3 * positions.size());
	force_int.resize(3 * positions.size());
	for (int i = 0; i < 3 * positions.size(); i++)
	{
		force_ext[i] = 0.0;
		force_int[i] = 0.0;
		mass[i] = 1;
		M(i, i) = mass[i];
	}

	//gx_m.resize(3 * positions.size(), 1);

	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		pointf3 v1 = positions[index1];
		pointf3 v2 = positions[index2];

		l.push_back((v1 - v2).norm());
		//l.push_back(0.5);
	}

	SetInitG();
	GetSet();

	CacK();

	CacA();

	return true;
}

void Simulate::SetFast()
{
	isfast = true;
}

bool Simulate::Run() {
	SimulateOnce();

	// half-edge structure -> triangle mesh

	return true;
}

void Ubpa::Simulate::SetLeftFix()
{
	//固定网格x坐标最小点
	fixed_id.clear();
	double x = 100000;
	for (int i = 0; i < positions.size(); i++)
	{
		if (positions[i][0] < x)
		{
			x = positions[i][0];
		}
	}

	for (int i = 0; i < positions.size(); i++)
	{
		if (abs(positions[i][0] - x) < 1e-5)
		{
			fixed_id.push_back(i);
		}
	}

	Init();
}

void Simulate::SimulateOnce() {
	// TODO
	//cout << "WARNING::Simulate::SimulateOnce:" << endl;
//		<< "\t" << "not implemented" << endl;
	//stiff = 50;
	//stiff = 100000;
//SetStiff(100000);

	if (!isfast)
	{
		CacX();
		//void UpdateX();
		CacV();
		UpdatePos();
	}
	else
	{
		std::vector<double> x_=x;
		for (int i = 0; i < x.size(); i++)
			y_ [i]= 2 * x[i]  - xx[i];
		x = y_;
		for (int i = 0; i < iter; i++)
		{
			Local_CacD();
			Global_CacX();
		}
		xx = x_;
		UpdatePos();
	}

}

void Simulate::CacForce()
{
	force_int.clear();
	force_int.resize(3 * positions.size());
	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		//pointf3 v1 = positions[index1];
		//pointf3 v2 = positions[index2];
		pointf3 v1 = pointf3(x[3 * index1 + 0], x[3 * index1 + 1], x[3 * index1 + 2]);
		pointf3 v2 = pointf3(x[3 * index2 + 0], x[3 * index2 + 1], x[3 * index2 + 2]);
		vecf3 r = v1 - v2;

		for (int j = 0; j < 3; j++)
		{
			force_int[3 * index1 + j] += -stiff * (r.norm() - l[i]) * r[j] / r.norm();
			force_int[3 * index2 + j] += stiff * (r.norm() - l[i]) * r[j] / r.norm();
		}

	}
}

void Simulate::SetInitG()
{
	for (int i = 0; i < positions.size(); i++)
		force_ext[i * 3 + 1] = g * mass[i];
}

void Simulate::GetSet()
{
	//fix.insert(10);
	//fix.insert(120);
	//fix.insert(440);
	//fix.insert(20);
	//fix.insert(1*positions.size() / 4);
	//fix.insert(2 * positions.size() / 4);
	fix = std::set<int>(fixed_id.begin(), fixed_id.end());
}

void Simulate::CacK()
{
	K.resize(x.size() - 3 * fix.size(), x.size());
	K.fill(0);

	for (int i = 0, j = 0; i < x.size(); i++)
	{
		if (fix.find(i / 3) == fix.end())
		{
			K(j, i) = 1;
			//std::cout << j << " " << i << endl;
			j++;
		}
	}

	Eigen::MatrixXd xt;
	xt.resize((x.size()), 1);
	b.resize(x.size());

	for (int i = 0; i < x.size(); i++)
		xt(i, 0) = x[i];
	Eigen::MatrixXd t = K.transpose() * K * xt;

	for (int i = 0; i < x.size(); i++)
		b[i] = x[i] - t(i, 0);
}

void Simulate::CacX()
{
	std::vector<double> y(x.size());
	for (int i = 0; i < x.size(); i++)
		y[i] = x[i] + h * velocity[i / 3][i % 3] + h * h / mass[i] * force_ext[i];

	xk = y;
	int i = 0;
	do
	{
		CacForce();
		CacGX();
		CacDiff();

		//Eigen::MatrixXd inverG = diff_.inverse();

		CacGxM();

		Eigen::MatrixXd t = inverG * gx_m;

		xk_1.clear();
		xk_1.resize(gx.size());
		for (int i = 0; i < gx.size(); i++)
		{
			xk_1[i] = xk[i] - t(i, 0);
			//cout << t(i, 0) << endl;
		}
		i++;
		//cout << i++ << endl;
		UpdateX();
		xk = x;
	} while (!isconv() && i <= 10);//sparse 50

}

void Simulate::CacGxM()
{
	gx_m.resize(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		gx_m(i, 0) = gx[i];
}

bool Simulate::isconv()
{
	bool flag = true;
	double delta = 0.01;
	std::vector<double> zero(gx.size(), delta);

	for (int i = 0; i < gx.size() && flag; i++)
	{
		if (abs(gx[i]) > zero[i])
			flag = false;
	}
	return flag;
}

void Simulate::SetFix()
{
	if (fixed_id.empty())
	{

		if (positions.size() > 440)
		{
			fixed_id.push_back(20);
			fixed_id.push_back(440);
		}
		else
		{
			fixed_id.push_back(10);
			fixed_id.push_back(120);
		}
	}
	//fixed_id.push_back(0);

}

void Simulate::CacGX()
{
	std::vector<double> y(xk.size());
	gx.resize(xk.size());

	for (int i = 0; i < xk.size(); i++)
	{
		y[i] = xk[i] + h * velocity[i / 3][i % 3] + h * h / mass[i] * force_ext[i];
	}

	for (int i = 0; i < xk.size(); i++)
	{
		gx[i] = mass[i] * (xk[i] - y[i]) - h * h * force_int[i];
	}

	Eigen::MatrixXd t(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		t(i, 0) = gx[i];

	t = K * t;

	gx.clear();
	gx.resize(t.rows());
	for (int i = 0; i < gx.size(); i++)
		gx[i] = t(i, 0);

	Eigen::MatrixXd xt;
	xt.resize((xk.size()), 1);

	for (int i = 0; i < xk.size(); i++)
		xt(i, 0) = xk[i];

	t = K * xt;
	xk.clear();
	xk.resize(t.rows());
	for (int i = 0; i < xk.size(); i++)
		xk[i] = t(i, 0);
}

void Simulate::CacDiff()
{
	std::vector<Eigen::Triplet<double> > triple;
	//Eigen::MatrixXd I = Eigen::DiagonalMatrix<double,3,3>::DiagonalMatrix(2);
	Eigen::MatrixXd I = MatrixXd::Identity(3, 3);
	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		pointf3 v1 = pointf3(x[3 * index1 + 0], x[3 * index1 + 1], x[3 * index1 + 2]);
		pointf3 v2 = pointf3(x[3 * index2 + 0], x[3 * index2 + 1], x[3 * index2 + 2]);
		vecf3 r = v1 - v2;
		MatrixXd t(3, 1);
		t(0, 0) = r[0]; t(1, 0) = r[1]; t(2, 0) = r[2];

		Eigen::MatrixXd dif;
		dif.resize(3, 3);
		dif = stiff * (l[i] / r.norm() - 1) * I - stiff * l[i]
			/ ((r.norm()) * (r.norm()) * (r.norm())) * t * t.transpose();
		//cout << index1 << " " << index2 << endl;
		//cout << dif << endl;
		for (int j = 0; j < 3; j++)
			for (int k = 0; k < 3; k++)
			{
				triple.push_back(Eigen::Triplet<double>(3 * index1 + j, 3 * index1 + k, -h * h * dif(j, k)));
				//triple.push_back(Eigen::Triplet<double>(3 * index1 + j, 3 * index2 + k, h*h*dif(j, k)));
				//triple.push_back(Eigen::Triplet<double>(3 * index2 + k, 3 * index1 + j, h*h*dif(j, k)));
				triple.push_back(Eigen::Triplet<double>(3 * index2 + k, 3 * index2 + j, -h * h * dif(j, k)));
			}
	}

	for (int i = 0; i < x.size(); i++)
	{
		triple.push_back(Eigen::Triplet<double>(i, i, mass[i]));
	}

	Eigen::MatrixXd diff_;
	Eigen::SparseLU<Eigen::SparseMatrix<double>> LU_;

	diff.setZero();
	diff.resize(x.size(), x.size());
	diff.setFromTriplets(triple.begin(), triple.end());

	diff_ = K * diff * K.transpose();
	//cout << diff_ << endl;
	//cout << K << endl;

	diff = diff_.sparseView();

	I = MatrixXd::Identity(gx.size(), gx.size());
	LU_.analyzePattern(diff);
	LU_.factorize(diff);
	//LU_.compute(diff);
	inverG = LU_.solve(I);
	//cout << inverG << endl;
}

void Simulate::UpdateX()
{
	Eigen::MatrixXd xt;
	xt.resize((xk_1.size()), 1);

	for (int i = 0; i < xk_1.size(); i++)
		xt(i, 0) = xk_1[i];

	Eigen::MatrixXd t = K.transpose() * xt;

	for (int i = 0; i < x.size(); i++)
		x[i] = t(i, 0) + b[i];

}

void Simulate::UpdatePos()
{
	for (int i = 0; i < x.size(); i++)
	{
		positions[i / 3][i % 3] = x[i];
	}
}

void Simulate::CacV()
{
	for (int i = 0; i < x.size(); i++)
	{
		velocity[i / 3][i % 3] = (x[i] - positions[i / 3][i % 3]) / h;
	}
}

void Simulate::CacL()
{
	L = MatrixXd::Zero(positions.size() * 3, positions.size() * 3);

	MatrixXd t = MatrixXd::Zero(positions.size(), positions.size());
	//cout << t << endl;
	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		std::vector<double> Ai(positions.size(),0);
		Ai[index1] = 1;
		Ai[index2] = -1;

		Eigen::MatrixXd At;
		At.resize((Ai.size()), 1);

		for (int j = 0; j < Ai.size(); j++)
			At(j, 0) = Ai[j];
		//cout << At << endl<<endl;
		t += stiff * At * At.transpose();
	}
	//cout << t << endl;
	//cout << M << endl;
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < positions.size(); i++)
	{
		for (int j = 0; j < positions.size(); j++)
			L.block(i * 3, j * 3, 3, 3) = t(i, j) * I3;
	}
}

void Simulate::CacJ()
{
	J = MatrixXd::Zero(positions.size() * 3, edgelist.size() / 2 * 3);

	MatrixXd t = MatrixXd::Zero(positions.size(), edgelist.size() / 2);

	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		std::vector<double> Ai(positions.size(), 0);
		Ai[index1] = 1;
		Ai[index2] = -1;

		std::vector<double> Si(edgelist.size() / 2,0);
		Si[i] = 1;

		Eigen::MatrixXd At;
		At.resize((Ai.size()), 1);

		for (int j = 0; j < Ai.size(); j++)
			At(j, 0) = Ai[j];

		Eigen::MatrixXd St;
		St.resize((Si.size()), 1);

		for (int j = 0; j < Si.size(); j++)
			St(j, 0) = Si[j];
		//cout << stiff * At * St.transpose() << endl;
		t += stiff * At * St.transpose();
	}
	//cout << t << endl;
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < positions.size(); i++)
	{
		for (int j = 0; j < edgelist.size() / 2; j++)
			J.block(i * 3, j * 3, 3, 3) = t(i, j) * I3;
	}
}

void Simulate::CacA()
{
	CacL();
	CacJ();
	MatrixXd A_;
	//cout << L << endl;
	//cout << J << endl;
	A_.resize(K.rows(), K.rows());
	A_ = K * (M + h * h * L) * K.transpose();
	A = A_.sparseView();
	LLT_.compute(A);
}

void Simulate::Global_CacX()
{
	Eigen::MatrixXd dt;
	dt.resize((d.size()), 1);

	for (int j = 0; j < d.size(); j++)
		dt(j, 0) = d[j];

	Eigen::MatrixXd yt;
	yt.resize((y_.size()), 1);

	for (int j = 0; j < y_.size(); j++)
		yt(j, 0) = y_[j];

	Eigen::MatrixXd ft;
	ft.resize((force_ext.size()), 1);

	for (int j = 0; j < force_ext.size(); j++)
		ft(j, 0) = force_ext[j];

	Eigen::MatrixXd bt;
	bt.resize((b.size()), 1);

	for (int j = 0; j < b.size(); j++)
		bt(j, 0) = b[j];

	VectorXd B = K * (h * h * J * dt + M * yt + h * h * ft - (M + h * h * L) * bt);
	//cout << J << endl;
	//cout << L << endl;
	VectorXd xf = LLT_.solve(B);
	//cout << xf << endl;
	xf = K.transpose() * xf + bt;

	for (int j = 0; j < x.size(); j++)
		x[j] = xf[j];
}

void Simulate::Local_CacD()
{
	d.resize(3 * edgelist.size() / 2);

	for (int i = 0; i < edgelist.size() / 2; i++)
	{
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = pointf3(x[3 * index1 + 0], x[3 * index1 + 1], x[3 * index1 + 2]);
		pointf3 v2 = pointf3(x[3 * index2 + 0], x[3 * index2 + 1], x[3 * index2 + 2]);
		vecf3 r = v1 - v2;

		d[3 * i + 0] = l[i] * r[0] / r.norm();
		d[3 * i + 1] = l[i] * r[1] / r.norm();
		d[3 * i + 2] = l[i] * r[2] / r.norm();
	}
}


#include <iostream>
#include <string>
#include <memory>
#include <vector>
#include <random>
#include <cmath>
#include <unordered_map>
#include <Eigen/Dense>
#include <matplotlibcpp.h>
#include "../include/model.h"
#include "../datasets/include/mnist.h"
#include "../include/trainer.h"

using std::vector;
namespace plt = matplotlibcpp;

void search_train(double, double, int, vector<double> &, vector<double> &);
void print_vector(vector<double> &);

int main()
{
    using namespace Eigen;
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::string;
    using std::vector;
    using std::shared_ptr;
    using std::make_shared;
    using std::unordered_map;

    int optimization_trial = 2; //探索回数(テスト用の数値。しっかり確認するなら100回程度やる必要あり)
    // int optimization_trial = 100;
    int trial = 1;

    // 乱数発生のための設定
    std::random_device seed_gen;
    std::default_random_engine engine(seed_gen());
    std::uniform_real_distribution<> lr_dist(-8, -4);
    std::uniform_real_distribution<> lambda_dist(-6, -2);

    // 動作テスト。本来はこれらのパラメータをランダム化してseach_trainに突っ込む
    // 突っ込む内容は、10^(-8) ~ 10^(-4)
    // 10^(-6) ~ 10^(-2)

    unordered_map<string, vector<double>> train_acc_history_dictionary, test_acc_history_dictionary;
    unordered_map<string, double> learning_rate_dictionary, weight_decay_lambda_dictionary;

    while (trial <= optimization_trial )
    {
        vector<double> train_acc_history, test_acc_history;

        double learning_rate = std::pow(10, lr_dist(engine));           // -8 ~ -4
        double weight_decay_lambda = std::pow(10, lambda_dist(engine)); // -6, -2
        int epochs = 5;
        // int epochs = 50;

        cout << "Learning Rate: " << learning_rate << endl;
        cout << "Weight Decay Lambda: " << weight_decay_lambda << endl;

        search_train(learning_rate, weight_decay_lambda, epochs, train_acc_history, test_acc_history);

        cout << "===== Train accuracy =====" << endl;
        print_vector(train_acc_history);
        cout << "===== Test accuracy =====" << endl;
        print_vector(test_acc_history);

        train_acc_history_dictionary["trial" + std::to_string(trial)] = train_acc_history;
        test_acc_history_dictionary["trial" + std::to_string(trial)] = test_acc_history;
        learning_rate_dictionary["trial" + std::to_string(trial)] = learning_rate;
        weight_decay_lambda_dictionary["trial" + std::to_string(trial)] = weight_decay_lambda;

        // train_acc_history, test_acc_historyの中身を消去するコードを作成する
        train_acc_history.clear();
        train_acc_history.shrink_to_fit();
        test_acc_history.clear();
        test_acc_history.shrink_to_fit();

        trial++;
    }

    // 学習結果を描画

    for (auto item : train_acc_history_dictionary)
    {
        string trial_num = item.first;
        string lr_str = std::to_string(learning_rate_dictionary[trial_num]);
        string lambda_str = std::to_string(weight_decay_lambda_dictionary[trial_num]);
        vector<int> x_axis(item.second.size());
        std::iota(x_axis.begin(), x_axis.end(), 1);

        plt::title("accuracy history of " + trial_num);
        plt::plot(x_axis, train_acc_history_dictionary[trial_num], "b");
        plt::plot(x_axis, test_acc_history_dictionary[trial_num], "r");
        plt::grid(true);

        plt::save("Hyperparameter Tuning " + trial_num);
        plt::cla();

        cout << "=== " << trial_num << "===" << endl;
        cout << "Learning Rate: " << lr_str << endl;
        cout << "Weight Decay Lambda: " << lambda_str << endl;
        cout << endl;
    }

    return 0;
}

void search_train(double learning_rate,
                  double weight_decay_lambda,
                  int epochs,
                  vector<double> & train_acc_history,
                  vector<double> & test_acc_history)
{
    using namespace Eigen;
    using namespace MyDL;
    using std::vector;
    using std::string;
    using std::shared_ptr;
    using std::make_shared;

    int input_size = 28 * 28;
    vector<int> hidden_list = {50};
    int output_size = 10;
    int batch_size = 100;
    string activation = "relu";
    string weight_initializer = "he";
    bool use_dropout = false;
    double dropout_ratio = 0.5;
    bool use_batchnorm = false;

    auto model = make_shared<MultiLayerModel>(input_size,
                                              hidden_list,
                                              output_size,
                                              weight_decay_lambda,
                                              activation,
                                              weight_initializer,
                                              use_dropout,
                                              dropout_ratio,
                                              use_batchnorm);
    auto dataset = make_shared<MnistEigenDataset>(batch_size);
    auto optimizer = make_shared<SGD>(learning_rate);
    // Trainer trainer(model, optimizer, dataset, epochs=epochs, false);
    Trainer trainer(model, optimizer, dataset, epochs=epochs, true);

    trainer.train();

    train_acc_history = trainer.train_acc_history;
    test_acc_history  = trainer.test_acc_history;
}

void print_vector(vector<double> & vec)
{
    for (const auto &item : vec)
    {
        std::cout << item << " ";
    }
    std::cout << std::endl;
}


/**
 * @file rbm_network_test.cpp
 * @author Kris Singh
 * @author Shikhar Jaiswal
 *
 * Tests the RBM Network
 *
 * digits dataset source:
 * @misc{Lichman:2013 ,
 * author = "M. Lichman",
 * year = "2013",
 * title = "{UCI} Machine Learning Repository",
 * url = "http://archive.ics.uci.edu/ml",
 * institution = "University of California,
 * Irvine, School of Information and Computer Sciences" }
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license. You should have received a copy of the
 * 3-clause BSD license along with mlpack. If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <mlpack/methods/ann/rbm/rbm.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>
#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace ens;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(RBMNetworkTest);

/*
 * Tests the BinaryRBM implementation on the Digits dataset.
 */
BOOST_AUTO_TEST_CASE(BinaryRBMClassificationTest)
{
  // Normalised dataset.
  int hiddenLayerSize = 100;
  size_t batchSize = 10;
  size_t numEpoches = 30;
  arma::mat trainData, testData, dataset;
  arma::mat trainLabelsTemp, testLabelsTemp;
  trainData.load("digits_train.arm");
  testData.load("digits_test.arm");
  trainLabelsTemp.load("digits_train_label.arm");
  testLabelsTemp.load("digits_test_label.arm");

  arma::Row<size_t> trainLabels = arma::zeros<arma::Row<size_t>>(1,
      trainLabelsTemp.n_cols);
  arma::Row<size_t> testLabels = arma::zeros<arma::Row<size_t>>(1,
      testLabelsTemp.n_cols);

  for (size_t i = 0; i < trainLabelsTemp.n_cols; ++i)
    trainLabels(i) = arma::as_scalar(trainLabelsTemp.col(i));

  for (size_t i = 0; i < testLabelsTemp.n_cols; ++i)
    testLabels(i) = arma::as_scalar(testLabelsTemp.col(i));

  arma::mat output, XRbm(hiddenLayerSize, trainData.n_cols),
      YRbm(hiddenLayerSize, testLabels.n_cols);

  XRbm.zeros();
  YRbm.zeros();

  GaussianInitialization gaussian(0, 0.1);
  RBM<GaussianInitialization> model(trainData,
      gaussian, trainData.n_rows, hiddenLayerSize, batchSize);

  size_t numRBMIterations = trainData.n_cols * numEpoches;
  numRBMIterations /= batchSize;
  ens::StandardSGD msgd(0.03, batchSize, numRBMIterations, 0, true);
  model.Reset();
  model.VisibleBias().ones();
  model.HiddenBias().ones();

  // Test the reset function.
  model.Train(msgd);

  for (size_t i = 0; i < trainData.n_cols; i++)
  {
    model.HiddenMean(std::move(trainData.col(i)), std::move(output));
    XRbm.col(i) = output;
  }

  for (size_t i = 0; i < testData.n_cols; i++)
  {
    model.HiddenMean(std::move(testData.col(i)),
      std::move(output));
    YRbm.col(i) = output;
  }
  const size_t numClasses = 10; // Number of classes.
  const size_t numBasis = 5; // Parameter required for L-BFGS algorithm.
  const size_t numIterations = 100; // Maximum number of iterations.

  // Use an instantiated optimizer for the training.
  L_BFGS optimizer(numBasis, numIterations);
  SoftmaxRegression regressor(trainData, trainLabels,
      numClasses, 0.001, false, optimizer);

  double classificationAccuracy = regressor.ComputeAccuracy(testData,
    testLabels);

  L_BFGS rbmOptimizer(numBasis, numIterations);
  SoftmaxRegression rbmRegressor(XRbm, trainLabels, numClasses,
        0.001, false, rbmOptimizer);
  double rbmClassificationAccuracy = rbmRegressor.ComputeAccuracy(YRbm,
      testLabels);

  // We allow a 6% tolerance because the RBM may not reconstruct samples as
  // well.  (Typically it does, but we have no guarantee.)
  BOOST_REQUIRE_GE(rbmClassificationAccuracy, classificationAccuracy - 6.0);
}

/*
 * Tests the SpikeSlabRBM implementation on the Digits dataset.
 */
BOOST_AUTO_TEST_CASE(ssRBMClassificationTest)
{
  size_t batchSize = 10;
  size_t numEpoches = 3;
  int hiddenLayerSize = 80;
  double radius = 0;
  double tempRadius = 0;
  arma::mat trainData, testData, dataset;
  arma::mat trainLabelsTemp, testLabelsTemp;
  trainData.load("digits_train.arm");
  testData.load("digits_test.arm");
  trainLabelsTemp.load("digits_train_label.arm");
  testLabelsTemp.load("digits_test_label.arm");
  GaussianInitialization gaussian(0, 1);

  arma::Row<size_t> trainLabels = arma::zeros<arma::Row<size_t>>(1,
      trainLabelsTemp.n_cols);
  arma::Row<size_t> testLabels = arma::zeros<arma::Row<size_t>>(1,
      testLabelsTemp.n_cols);

  for (size_t i = 0; i < trainLabelsTemp.n_cols; ++i)
    trainLabels(i) = arma::as_scalar(trainLabelsTemp.col(i));

  for (size_t i = 0; i < testLabelsTemp.n_cols; ++i)
    testLabels(i) = arma::as_scalar(testLabelsTemp.col(i));

  for (size_t i = 0; i < trainData.n_cols; i++)
  {
    tempRadius = arma::norm(trainData.col(i));
    if (radius < tempRadius)
      radius = tempRadius;
  }

  size_t poolSize = 2;
  radius *= 1.3;

  arma::mat output;
  arma::mat XRbm(hiddenLayerSize + poolSize * hiddenLayerSize,
      trainData.n_cols);
  arma::mat YRbm(hiddenLayerSize + poolSize * hiddenLayerSize,
      testLabels.n_cols);

  XRbm.zeros();
  YRbm.zeros();
  double slabPenalty = 8;

  RBM<GaussianInitialization, arma::mat, SpikeSlabRBM> modelssRBM(trainData,
      gaussian, trainData.n_rows, hiddenLayerSize, batchSize, 1, 1, poolSize,
      slabPenalty, radius);

  size_t numRBMIterations = trainData.n_cols * numEpoches;
  numRBMIterations /= batchSize;

  ens::StandardSGD msgd(0.02, batchSize, numRBMIterations, 0, true);
  modelssRBM.Reset();
  modelssRBM.VisiblePenalty().fill(5);
  modelssRBM.SpikeBias().fill(1);

  modelssRBM.Train(msgd);
  for (size_t i = 0; i < trainData.n_cols; i++)
  {
    modelssRBM.HiddenMean(std::move(trainData.col(i)),
        std::move(output));
    XRbm.col(i) = output;
  }

  for (size_t i = 0; i < testData.n_cols; i++)
  {
    modelssRBM.HiddenMean(std::move(testData.col(i)),
      std::move(output));
    YRbm.col(i) = output;
  }
  const size_t numClasses = 10; // Number of classes.
  const size_t numBasis = 5; // Parameter required for L-BFGS algorithm.
  const size_t numIterations = 100; // Maximum number of iterations.

  L_BFGS ssRbmOptimizer(numBasis, numIterations);
  SoftmaxRegression ssRbmRegressor(XRbm, trainLabels, numClasses,
        0.001, false, ssRbmOptimizer);
  double ssRbmClassificationAccuracy = ssRbmRegressor.ComputeAccuracy(
      YRbm, testLabels);

  // 76.18 is the standard accuracy of the Softmax regression classifier,
  // omitted here for speed.  We add a margin of 2% since ssRBM isn't guaranteed
  // to give us better results (we just generally expect it to be about as good
  // or better).
  BOOST_REQUIRE_GE(ssRbmClassificationAccuracy, 76.18 - 2.0);
}

template<typename MatType = arma::mat>
void BuildVanillaNetwork(MatType& trainData,
                         const size_t hiddenLayerSize)
{
  MatType output;
  GaussianInitialization gaussian(0, 0.1);
  RBM<GaussianInitialization, MatType, BinaryRBM> model(trainData, gaussian,
      trainData.n_rows, hiddenLayerSize, 1, 1, 1, 2, 8, 1, true);

  model.Reset();
  // Set the parameters from a learned RBM Sklearn random state 23.
  model.Parameters() = MatType(
      "-0.23224054, -0.23000632, -0.25701271, -0.25122418, -0.20716651,"
      "-0.20962217, -0.59922456, -0.60003836, -0.6, -0.625, -0.475;");

  // Check free energy.
  arma::Mat<float> freeEnergy = MatType(
      "-0.87523715, 0.50615066, 0.46923476, 1.21509084;");
  arma::vec calculatedFreeEnergy(4, arma::fill::zeros);
  for (size_t i = 0; i < trainData.n_cols; i++)
  {
    calculatedFreeEnergy(i) = model.FreeEnergy(std::move(trainData.col(i)));
  }

  for (size_t i = 0; i < freeEnergy.n_elem; i++)
    BOOST_REQUIRE_CLOSE(calculatedFreeEnergy(i), freeEnergy(i), 1e-3);
}

/*
 * Train and evaluate a Vanilla network with the specified structure.
 */
BOOST_AUTO_TEST_CASE(MiscTest)
{
  arma::Mat<float> X = arma::Mat<float>("0.0, 0.0, 0.0;"
                          "0.0, 1.0, 1.0;"
                          "1.0, 0.0, 1.0;"
                          "1.0, 1.0, 1.0;");
  X = X.t();
  BuildVanillaNetwork<arma::Mat<float>>(X, 2);
}

BOOST_AUTO_TEST_SUITE_END();