/*
 * Copyright 2010, 2011, 2012
 * Nicolas Mansard,
 * François Bleibel,
 * Olivier Stasse,
 * Florent Lamiraux
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_KALMAN_H
#define __SOT_KALMAN_H

/* -------------------------------------------------------------------------- */
/* --- INCLUDE -------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

#include <Eigen/LU>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/constraint.hh>

/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(kalman_EXPORTS)
#define SOT_KALMAN_EXPORT __declspec(dllexport)
#else
#define SOT_KALMAN_EXPORT __declspec(dllimport)
#endif
#else
#define SOT_KALMAN_EXPORT
#endif

/* -------------------------------------------------------------------------- */
/* --- CLASSE --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

class SOT_KALMAN_EXPORT Kalman : public Entity {
public:
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }

protected:
  unsigned int size_state;
  unsigned int size_measure;
  double dt;

public:
  SignalPtr<Vector, int> measureSIN;         // y
  SignalPtr<Matrix, int> modelTransitionSIN; // F
  SignalPtr<Matrix, int> modelMeasureSIN;    // H
  SignalPtr<Matrix, int> noiseTransitionSIN; // Q
  SignalPtr<Matrix, int> noiseMeasureSIN;    // R

  SignalPtr<Vector, int> statePredictedSIN;            // x_{k|k-1}
  SignalPtr<Vector, int> observationPredictedSIN;      // y_pred = h (x_{k|k-1})
  SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
  SignalTimeDependent<Vector, int> stateUpdateSOUT;    // X_est

  SignalTimeDependent<Matrix, int> gainSINTERN;       // K
  SignalTimeDependent<Matrix, int> innovationSINTERN; // S

public:
  virtual std::string getDocString() const {
    return "Implementation of extended Kalman filter     \n"
           "\n"
           "  Dynamics of the system:                    \n"
           "\n"
           "    x = f (x   , u   ) + w       (state)      \n"
           "     k      k-1   k-1     k-1                 \n"
           "\n"
           "    y = h (x ) + v               (observation)\n"
           "     k      k     k                           \n"
           "\n"
           "  Prediction:\n"
           "\n"
           "    ^          ^                       \n"
           "    x     = f (x       , u   )     (state) \n"
           "     k|k-1      k-1|k-1   k-1          \n"
           "\n"
           "                           T           \n"
           "    P     = F    P        F    + Q (covariance)\n"
           "     k|k-1   k-1  k-1|k-1  k-1         \n"
           "\n"
           "  with\n"
           "           \\                         \n"
           "           d f  ^                         \n"
           "    F    = --- (x       , u   )           \n"
           "     k-1   \\     k-1|k-1   k-1            \n"
           "           d x                         \n"
           "\n"
           "         \\                             \n"
           "         d h  ^                          \n"
           "    H  = --- (x       )                     \n"
           "     k   \\     k-1|k-1                      \n"
           "         d x                             \n"

           "  Update:\n"
           "\n"
           "                ^                            \n"
           "    z = y  - h (x     )             (innovation)\n"
           "     k   k       k|k-1                       \n"
           "                   T                          \n"
           "    S = H  P      H  + R            (innovation covariance)\n"
           "     k   k  k|k-1  k                          \n"
           "                T  -1                         \n"
           "    K = P      H  S                 (Kalman gain)\n"
           "     k   k|k-1  k  k                          \n"
           "    ^     ^                                   \n"
           "    x   = x      + K  z             (state)   \n"
           "     k|k   k|k-1    k  k                      \n"
           "\n"
           "    P   =(I - K  H ) P                        \n"
           "     k|k       k  k   k|k-1                   \n"
           "\n"
           "  Signals\n"
           "    - input(vector)::x_pred:  state prediction\n"
           "                                                         ^\n"
           "    - input(vector)::y_pred:  observation prediction: h (x     )\n"
           "                                                          k|k-1\n"
           "    - input(matrix)::F:       partial derivative wrt x of f\n"
           "    - input(vector)::y:       measure         \n"
           "    - input(matrix)::H:       partial derivative wrt x of h\n"
           "    - input(matrix)::Q:       variance of noise w\n"
           "                                                 k-1\n"
           "    - input(matrix)::R:       variance of noise v\n"
           "                                                 k\n"
           "    - output(matrix)::P_pred: variance of prediction\n"
           "                                               ^\n"
           "    - output(vector)::x_est:  state estimation x\n"
           "                                                k|k\n";
  }

protected:
  Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
  Vector &computeStateUpdate(Vector &x_est, const int &time);

  void setStateEstimation(const Vector &x0) {
    stateEstimation_ = x0;
    stateUpdateSOUT.recompute(0);
  }

  void setStateVariance(const Matrix &P0) {
    stateVariance_ = P0;
    varianceUpdateSOUT.recompute(0);
  }
  // Current state estimation
  // ^
  // x
  //  k-1|k-1
  Vector stateEstimation_;
  // Variance of current state estimation
  // P
  //  k-1|k-1
  Matrix stateVariance_;

  //                          ^
  // Innovation: z  = y  - H  x
  //              k    k    k  k|k-1
  Vector z_;

  // F    P
  //  k-1  k-1|k-1
  Matrix FP_;

  // Variance prediction
  // P
  //  k|k-1
  Matrix Pk_k_1_;

  // Innovation covariance
  Matrix S_;

  // Kalman Gain
  Matrix K_;

public:
  Kalman(const std::string &name);
  /* --- Entity --- */
  void display(std::ostream &os) const;
};

} // namespace sot
} // namespace dynamicgraph

/*!
  \file Kalman.h
  \brief  Extended kalman filter implementation
*/

#endif


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_
#define VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/shared_ptr.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystem.hpp"
#include "AbstractCellMutationState.hpp"
#include "MathsCustomFunctions.hpp"

/**
 * Represents the van Leeuwen et al. (2007) system of ODEs
 * [doi:10.1016/j.jtbi.2007.01.019]
 * coupled to the Swat et al. cell-cycle model equations.
 * [doi:10.1093/bioinformatics/bth110]
 *
 * The variables are
 *
 *   0. r = pRb
 *   1. e = E2F1 (This is the S-phase indicator)
 *   2. i = CycD (inactive)
 *   3. j = CycD (active)
 *   4. p = pRb-p
 *   5. D = APC destruction complex
 *   6. X = Axin
 *   7. Cu = Beta Cat marked for ubiquitination
 *   8. Co = Open form Beta Cat
 *   9. Cc = Closed form Beta Cat
 *   10. Mo = Open form Mutant Beta Cat
 *   11. Mc = Closed form Mutant Beta Cat
 *   12. A = Free Adhesion molecules
 *   13. Ca = BetaCat/Adhesion
 *   14. Ma = Mutant BetaCat/Adhesion
 *   15. T = free TCF
 *   16. Cot = Open BetaCat/TCF
 *   17. Cct = Closed BetaCat/TCF
 *   18. Mot = Open Mutant BetaCat/TCF
 *   19. Mct = Closed Mutant BetaCat/TCF
 *   20. Y = Wnt Target protein
 *   21. Wnt level
 */
class VanLeeuwen2009WntSwatCellCycleOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Parameters for the Swat et al. (2004) model
     */

    /** Dimensional parameter k_2. */
    double mk2d;
    /** Dimensional parameter k_3. */
    double mk3d;
    /** Dimensional parameter k_34. */
    double mk34d;
    /** Dimensional parameter k_2. */
    double mk43d;
    /** Dimensional parameter k_23. */
    double mk23d;
    /** Dimensional parameter a. */
    double mad;
    /** Dimensional parameter J_11. */
    double mJ11d;
    /** Dimensional parameter J_12. */
    double mJ12d;
    /** Dimensional parameter J_13. */
    double mJ13d;
    /** Dimensional parameter J_13. */
    double mJ61d;
    /** Dimensional parameter J_62. */
    double mJ62d;
    /** Dimensional parameter J_63. */
    double mJ63d;
    /** Dimensional parameter K_m1. */
    double mKm1d;
    /** Dimensional parameter k_p. */
    double mkpd;
    /** Dimensionless parameter phi_r. */
    double mphi_r;
    /** Dimensionless parameter phi_i. */
    double mphi_i;
    /** Dimensionless parameter phi_j. */
    double mphi_j;
    /** Dimensionless parameter phi_p. */
    double mphi_p;
    /** Dimensional parameter k_16. */
    double mk16d;
    /** Dimensional parameter k_61. */
    double mk61d;
    /** Dimensionless parameter phi_E2F1. */
    double mPhiE2F1;

    /**
     * Parameters for the Van Leeuwen et al. (2007) model
     */

    /** Dimensionless parameter s_A. */
    double mSa;
    /** Dimensionless parameter s_CA. */
    double mSca;
    /** Dimensionless parameter s_C. */
    double mSc;
    /** Dimensionless parameter s_CT. */
    double mSct;
    /** Dimensionless parameter s_D. */
    double mSd;
    /** Dimensionless parameter s_T. */
    double mSt;
    /** Dimensionless parameter s_X. */
    double mSx;
    /** Dimensionless parameter s_Y. */
    double mSy;
    /** Dimensionless parameter d_A. */
    double mDa;
    /** Dimensionless parameter d_CA. */
    double mDca;
    /** Dimensionless parameter d_C. */
    double mDc;
    /** Dimensionless parameter d_CT. */
    double mDct;
    /** Dimensionless parameter d_D. */
    double mDd;
    /** Dimensionless parameter d_Dx. */
    double mDdx;
    /** Dimensionless parameter d_T. */
    double mDt;
    /** Dimensionless parameter d_U. */
    double mDu;
    /** Dimensionless parameter d_X. */
    double mDx;
    /** Dimensionless parameter d_Y. */
    double mDy;
    /** Dimensionless parameter K_c. */
    double mKc;
    /** Dimensionless parameter K_D. */
    double mKd;
    /** Dimensionless parameter K_T. */
    double mKt;
    /** Dimensionless parameter p_c. */
    double mPc;
    /** Dimensionless parameter p_u. */
    double mPu;
    /** Dimensionless parameter xi_D. */
    double mXiD;
    /** Dimensionless parameter xi_Dx. */
    double mXiDx;
    /** Dimensionless parameter xi_X. */
    double mXiX;
    /** Dimensionless parameter xi_C. */
    double mXiC;

    /** The mutation state of the cell. */
    boost::shared_ptr<AbstractCellMutationState> mpMutationState;

    /**
     * The hypothesis we are using
     *  = 1u for Van Leeuwen Hypothesis I
     *  = 2u for Van Leeuwen Hypothesis II
     */
    unsigned mHypothesis;

    /** The Wnt level (this affects the ODE system). */
    double mWntLevel;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param hypothesis takes the value 1 or 2 and affects the ODE system.
     * @param wntLevel is a non-dimensional Wnt value between 0 and 1. This sets up the Wnt pathway in its steady state.
     * @param pMutationState cell mutation; some affect the ODE system
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    VanLeeuwen2009WntSwatCellCycleOdeSystem(unsigned hypothesis,
                                            double wntLevel = 0.0,
                                            boost::shared_ptr<AbstractCellMutationState> pMutationState=boost::shared_ptr<AbstractCellMutationState>(),
                                            std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~VanLeeuwen2009WntSwatCellCycleOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Set the mutation state of the cell.
     *
     * This should be called by the relevant cell-cycle model before any solving
     * of the ODE system (as it is used to evaluate the Y derivatives).
     *
     * @param pMutationState the mutation state.
     */
    void SetMutationState(boost::shared_ptr<AbstractCellMutationState> pMutationState);

    /**
     * Called by the archive function on the Wnt cell-cycle model.
     *
     * @return #mpMutationState the mutation state of the cell.
     */
    const boost::shared_ptr<AbstractCellMutationState> GetMutationState() const;

    /**
     * Compute the RHS of the system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using van Leeuwen et al. (2007) system of equations)
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * When using CVODE this function is called instead of CalculateStoppingEvent.
     * It allows the point at which rY[1] reaches 1 to be found to greater precision.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return How close we are to the root of the stopping condition
     */
    double CalculateRootFunction(double time, const std::vector<double>& rY);

    /**
     * @return #mWntLevel.
     */
    double GetWntLevel() const;

    /**
     * @return #mHypothesis.
     */
    unsigned GetHypothesis() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(VanLeeuwen2009WntSwatCellCycleOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a VanLeeuwen2009WntSwatCellCycleOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const VanLeeuwen2009WntSwatCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const unsigned hypothesis = t->GetHypothesis();
    ar & hypothesis;

    const double wnt_level = t->GetWntLevel();
    ar & wnt_level;

    const boost::shared_ptr<AbstractCellMutationState> p_mutation_state = t->GetMutationState();
    ar & p_mutation_state;

    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a VanLeeuwen2009WntSwatCellCycleOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, VanLeeuwen2009WntSwatCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    unsigned hypothesis;
    ar & hypothesis;

    double wnt_level;
    ar & wnt_level;

    boost::shared_ptr<AbstractCellMutationState> p_mutation_state;
    ar & p_mutation_state;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)VanLeeuwen2009WntSwatCellCycleOdeSystem(hypothesis, wnt_level, p_mutation_state, state_variables);
}
}
} // namespace ...

#endif /*VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_*/


//==================================================================================================
/*!
  @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_DETAIL_CONSTANT_MEDIUM_PI_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_MEDIUM_PI_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Constant used in trigonometric reductions

    @par Semantic:

    For type T:

    @code
    T r = Medium_pi<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = Pi<T>()*pow2(18);
    else if T is float
      r = Pi<T>()*pow2(6);
    else
      r =  201
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct medium_pi_ : boost::dispatch::constant_value_<medium_pi_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,medium_pi_,boost::dispatch::constant_value_<medium_pi_>);
      BOOST_SIMD_REGISTER_CONSTANT(201, 0X43490FDB, 0X412921FB54442D18LL); //2^6/pi, //2^{18}/pi;
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,medium_pi_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::medium_pi_,medium_pi);
  }

  template<typename T> BOOST_FORCEINLINE auto Medium_pi()
  BOOST_NOEXCEPT_DECLTYPE(detail::medium_pi( boost::dispatch::as_<T>{}))
  {
    return detail::medium_pi( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff/
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_FUNCTIONS_ESV2007_HH
#define DUNE_STUFF_FUNCTIONS_ESV2007_HH

#include <cmath>

#include <type_traits>

#if HAVE_EIGEN
#include <Eigen/Eigenvalues>
#endif

#include <dune/geometry/referenceelements.hh>

#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/common/debug.hh>
#include <dune/stuff/common/ranges.hh>
#include <dune/stuff/la/container/eigen.hh>

#include "interfaces.hh"

namespace Dune {
namespace Stuff {
namespace Functions {
namespace ESV2007 {

template <class E, class D, size_t d, class R, size_t r, size_t rC = 1>
class Testcase1Force : public LocalizableFunctionInterface<E, D, d, R, r, rC>
{
  Testcase1Force() { static_assert(AlwaysFalse<E>::value, "Not available for these dimensions!"); }
};

template <class EntityImp, class DomainFieldImp, class RangeFieldImp>
class Testcase1Force<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
    : public GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1>
{
  typedef Testcase1Force<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> ThisType;
  typedef GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> BaseType;

public:
  using typename BaseType::DomainFieldType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::JacobianRangeType;

  static const bool available = true;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.testcase1.force"; }

  static Common::Configuration default_config(const std::string sub_name = "")
  {
    Common::Configuration config;
    config["integration_order"] = "3";
    config["name"] = static_id();
    if (sub_name.empty())
      return config;
    else {
      Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr<ThisType> create(const Common::Configuration config = default_config(),
                                          const std::string sub_name = static_id())
  {
    // get correct config
    const Common::Configuration cfg         = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    const Common::Configuration default_cfg = default_config();
    // create
    return Common::make_unique<ThisType>(cfg.get("integration_order", default_cfg.get<size_t>("integration_order")),
                                         cfg.get("name", default_cfg.get<std::string>("name")));
  } // ... create(...)

  Testcase1Force(const size_t ord = default_config().get<size_t>("integration_order"),
                 const std::string nm = static_id())
    : order_(ord), name_(nm)
  {
  }

  Testcase1Force(const ThisType& /*other*/) = default;

  ThisType& operator=(const ThisType& /*other*/) = delete;

  virtual std::string type() const override final { return BaseType::static_id() + ".ESV2007.testcase1.force"; }

  virtual std::string name() const override final { return name_; }

  virtual size_t order() const override final { return order_; }

  /**
   * \brief "0.5 * pi * pi * cos(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   */
  virtual void evaluate(const DomainType& xx, RangeType& ret) const override final
  {
    ret[0] = M_PI_2l * M_PIl * cos(M_PI_2l * xx[0]) * cos(M_PI_2l * xx[1]);
  }

  /**
   * \brief ["-0.25 * pi * pi * pi * sin(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   *         "-0.25 * pi * pi * pi * cos(0.5 * pi * x[0]) * sin(0.5 * pi * x[1])"]
   */
  virtual void jacobian(const DomainType& xx, JacobianRangeType& ret) const override final
  {
    const DomainFieldType pre   = -0.25 * M_PIl * M_PIl * M_PIl;
    const DomainFieldType x_arg = M_PI_2l * xx[0];
    const DomainFieldType y_arg = M_PI_2l * xx[1];
    ret[0][0]                   = pre * sin(x_arg) * cos(y_arg);
    ret[0][1]                   = pre * cos(x_arg) * sin(y_arg);
  } // ... jacobian(...)

private:
  const size_t order_;
  const std::string name_;
}; // class Testcase1Force

template <class E, class D, size_t d, class R, size_t r, size_t rC = 1>
class Testcase1ExactSolution : public LocalizableFunctionInterface<E, D, d, R, r, rC>
{
  Testcase1ExactSolution() { static_assert(AlwaysFalse<E>::value, "Not available for these dimensions!"); }
};

template <class EntityImp, class DomainFieldImp, class RangeFieldImp>
class Testcase1ExactSolution<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
    : public GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1>
{
  typedef Testcase1ExactSolution<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> ThisType;
  typedef GlobalFunctionInterface<EntityImp, DomainFieldImp, 2, RangeFieldImp, 1, 1> BaseType;

public:
  using typename BaseType::DomainFieldType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::JacobianRangeType;

  static const bool available = true;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.testcase1.exactsolution"; }

  static Common::Configuration default_config(const std::string sub_name = "")
  {
    Common::Configuration config;
    config["integration_order"] = "3";
    config["name"] = static_id();
    if (sub_name.empty())
      return config;
    else {
      Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr<ThisType> create(const Common::Configuration config = default_config(),
                                          const std::string sub_name = static_id())
  {
    // get correct config
    const Common::Configuration cfg         = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    const Common::Configuration default_cfg = default_config();
    // create
    return Common::make_unique<ThisType>(cfg.get("integration_order", default_cfg.get<size_t>("integration_order")),
                                         cfg.get("name", default_cfg.get<std::string>("name")));
  } // ... create(...)

  Testcase1ExactSolution(const size_t ord = default_config().get<size_t>("integration_order"),
                         const std::string nm = static_id())
    : order_(ord), name_(nm)
  {
  }

  Testcase1ExactSolution(const ThisType& /*other*/) = default;

  ThisType& operator=(const ThisType& /*other*/) = delete;

  virtual std::string type() const override final { return BaseType::static_id() + ".ESV2007.testcase1.exactsolution"; }

  virtual std::string name() const override final { return name_; }

  virtual size_t order() const override final { return order_; }

  /**
   * \brief "cos(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   */
  virtual void evaluate(const DomainType& xx, RangeType& ret) const override final
  {
    ret[0] = cos(M_PI_2l * xx[0]) * cos(M_PI_2l * xx[1]);
  }

  /**
   * \brief ["-0.5 * pi * sin(0.5 * pi * x[0]) * cos(0.5 * pi * x[1])"
   *         "-0.5 * pi * cos(0.5 * pi * x[0]) * sin(0.5 * pi * x[1])"]
   */
  virtual void jacobian(const DomainType& xx, JacobianRangeType& ret) const override final
  {
    const DomainFieldType pre   = -0.5 * M_PIl;
    const DomainFieldType x_arg = M_PI_2l * xx[0];
    const DomainFieldType y_arg = M_PI_2l * xx[1];
    ret[0][0]                   = pre * sin(x_arg) * cos(y_arg);
    ret[0][1]                   = pre * cos(x_arg) * sin(y_arg);
  } // ... jacobian(...)

private:
  const size_t order_;
  const std::string name_;
}; // class Testcase1ExactSolution

template <class DiffusionFactorType, class DiffusionTensorType = void>
class Cutoff;

template <class DiffusionType>
class Cutoff<DiffusionType, void>
    : public LocalizableFunctionInterface<typename DiffusionType::EntityType, typename DiffusionType::DomainFieldType,
                                          DiffusionType::dimDomain, typename DiffusionType::RangeFieldType, 1, 1>
{
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionType>::value,
                "DiffusionType has to be tagged as a LocalizableFunction!");
  typedef typename DiffusionType::EntityType E_;
  typedef typename DiffusionType::DomainFieldType D_;
  static const size_t d_ = DiffusionType::dimDomain;
  typedef typename DiffusionType::RangeFieldType R_;
  typedef LocalizableFunctionInterface<E_, D_, d_, R_, 1> BaseType;
  typedef Cutoff<DiffusionType> ThisType;

  class Localfunction : public LocalfunctionInterface<E_, D_, d_, R_, 1, 1>
  {
    typedef LocalfunctionInterface<E_, D_, d_, R_, 1, 1> BaseType;

  public:
    typedef typename BaseType::EntityType EntityType;

    typedef typename BaseType::DomainFieldType DomainFieldType;
    static const size_t dimDomain = BaseType::dimDomain;
    typedef typename BaseType::DomainType DomainType;

    typedef typename BaseType::RangeFieldType RangeFieldType;
    static const size_t dimRange     = BaseType::dimRange;
    static const size_t dimRangeCols = BaseType::dimRangeCols;
    typedef typename BaseType::RangeType RangeType;

    typedef typename BaseType::JacobianRangeType JacobianRangeType;

  private:
    template <class D, int r, int rR>
    struct Compute
    {
      static_assert(AlwaysFalse<D>::value, "Not implemented for these dimensions!");
    };

    template <class D>
    struct Compute<D, 1, 1>
    {
      static RangeFieldType min_eigenvalue_of(const D& diffusion, const EntityType& ent)
      {
        const auto local_diffusion = diffusion.local_function(ent);
        assert(local_diffusion->order() == 0);
        const auto& reference_element = ReferenceElements<DomainFieldType, dimDomain>::general(ent.type());
        return local_diffusion->evaluate(reference_element.position(0, 0))[0];
      } // ... min_eigenvalue_of_(...)
    };  // class Compute< ..., 1, 1 >

  public:
    Localfunction(const EntityType& ent, const DiffusionType& diffusion, const RangeFieldType poincare_constant)
      : BaseType(ent), value_(0)
    {
      const RangeFieldType min_eigen_value =
          Compute<DiffusionType, DiffusionType::dimRange, DiffusionType::dimRangeCols>::min_eigenvalue_of(diffusion,
                                                                                ent);
      assert(min_eigen_value > 0.0);
      const DomainFieldType hh = compute_diameter_of_(ent);
      value_                   = (poincare_constant * hh * hh) / min_eigen_value;
    }

    Localfunction(const Localfunction& /*other*/) = delete;

    Localfunction& operator=(const Localfunction& /*other*/) = delete;

    virtual size_t order() const override final { return 0; }

    virtual void evaluate(const DomainType& UNUSED_UNLESS_DEBUG(xx), RangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret[0] = value_;
    }

    virtual void jacobian(const DomainType& UNUSED_UNLESS_DEBUG(xx), JacobianRangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret *= RangeFieldType(0);
    }

  private:
    static DomainFieldType compute_diameter_of_(const EntityType& ent)
    {
      DomainFieldType ret(0);
      for (auto cc : DSC::valueRange(ent.template count<dimDomain>())) {
        const auto vertex = ent.template subEntity<dimDomain>(cc)->geometry().center();
        for (auto dd : DSC::valueRange(cc + 1, ent.template count<dimDomain>())) {
          const auto other_vertex = ent.template subEntity<dimDomain>(dd)->geometry().center();
          const auto diff         = vertex - other_vertex;
          ret                     = std::max(ret, diff.two_norm());
        }
      }
      return ret;
    } // ... compute_diameter_of_(...)

    RangeFieldType value_;
  }; // class Localfunction

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::LocalfunctionType LocalfunctionType;
  typedef typename BaseType::RangeFieldType RangeFieldType;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.cutoff"; }

  Cutoff(const DiffusionType& diffusion, const RangeFieldType poincare_constant = 1.0 / (M_PIl * M_PIl),
         const std::string nm = static_id())
    : diffusion_(diffusion), poincare_constant_(poincare_constant), name_(nm)
  {
  }

  Cutoff(const ThisType& other) = default;

  ThisType& operator=(const ThisType& other) = delete;

  virtual std::string name() const override final { return name_; }

  virtual std::unique_ptr<LocalfunctionType> local_function(const EntityType& entity) const override final
  {
    return std::unique_ptr<Localfunction>(new Localfunction(entity, diffusion_, poincare_constant_));
  }

private:
  const DiffusionType& diffusion_;
  const RangeFieldType poincare_constant_;
  std::string name_;
}; // class Cutoff

template <class DiffusionFactorType, class DiffusionTensorType>
class Cutoff
    : public LocalizableFunctionInterface<typename DiffusionFactorType::EntityType,
                                          typename DiffusionFactorType::DomainFieldType, DiffusionFactorType::dimDomain,
                                          typename DiffusionFactorType::RangeFieldType, 1, 1>
{
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionFactorType>::value,
                "DiffusionFactorType has to be tagged as a LocalizableFunction!");
  static_assert(std::is_base_of<Tags::LocalizableFunction, DiffusionTensorType>::value,
                "DiffusionTensorType has to be tagged as a LocalizableFunction!");
  typedef typename DiffusionFactorType::EntityType E_;
  typedef typename DiffusionFactorType::DomainFieldType D_;
  static const size_t d_ = DiffusionFactorType::dimDomain;
  typedef typename DiffusionFactorType::RangeFieldType R_;
  typedef LocalizableFunctionInterface<E_, D_, d_, R_, 1> BaseType;
  typedef Cutoff<DiffusionFactorType, DiffusionTensorType> ThisType;

  static_assert(DiffusionFactorType::dimRange == 1, "The diffusion factor has to be scalar!");
  static_assert(DiffusionFactorType::dimRangeCols == 1, "The diffusion factor has to be scalar!");

  static_assert(std::is_same<typename DiffusionTensorType::EntityType, E_>::value, "Types do not match!");
  static_assert(std::is_same<typename DiffusionTensorType::DomainFieldType, D_>::value, "Types do not match!");
  static_assert(DiffusionTensorType::dimDomain == d_, "Dimensions do not match!");
  static_assert(std::is_same<typename DiffusionTensorType::RangeFieldType, R_>::value, "Types do not match!");

  static_assert(DiffusionTensorType::dimRange == d_, "The diffusion tensor has to be a matrix!");
  static_assert(DiffusionTensorType::dimRangeCols == d_, "The diffusion tensor has to be a matrix!");

  class Localfunction : public LocalfunctionInterface<E_, D_, d_, R_, 1, 1>
  {
    typedef LocalfunctionInterface<E_, D_, d_, R_, 1, 1> BaseType;

  public:
    typedef typename BaseType::EntityType EntityType;

    typedef typename BaseType::DomainFieldType DomainFieldType;
    static const size_t dimDomain = BaseType::dimDomain;
    typedef typename BaseType::DomainType DomainType;

    typedef typename BaseType::RangeFieldType RangeFieldType;
    static const size_t dimRange     = BaseType::dimRange;
    static const size_t dimRangeCols = BaseType::dimRangeCols;
    typedef typename BaseType::RangeType RangeType;

    typedef typename BaseType::JacobianRangeType JacobianRangeType;

  private:
    template <class DF, size_t r, size_t rR>
    struct ComputeDiffusionFactor
    {
      static_assert(AlwaysFalse<DF>::value, "Not implemented for these dimensions!");
    };

    template <class DF>
    struct ComputeDiffusionFactor<DF, 1, 1>
    {
      /**
       * We try to find the minimum of a polynomial of given order by evaluating it at the points of a quadrature that
       * would integrate this polynomial exactly.
       * \todo These are just some heuristics and should be replaced by something proper.
       */
      static RangeFieldType min_of(const DF& diffusion_factor, const EntityType& ent)
      {
        typename DF::RangeType tmp_value(0);
        RangeFieldType minimum            = std::numeric_limits<RangeFieldType>::max();
        const auto local_diffusion_factor = diffusion_factor.local_function(ent);
        const size_t ord = local_diffusion_factor->order();
        const auto& quadrature =
            QuadratureRules<DomainFieldType, dimDomain>::rule(ent.type(), boost::numeric_cast<int>(ord));
        const auto quad_point_it_end = quadrature.end();
        for (auto quad_point_it = quadrature.begin(); quad_point_it != quad_point_it_end; ++quad_point_it) {
          local_diffusion_factor->evaluate(quad_point_it->position(), tmp_value);
          minimum = std::min(minimum, tmp_value[0]);
        }
        return minimum;
      } // ... min_of(...)
    };  // class ComputeDiffusionFactor< ..., 1, 1 >

    template <class DT, size_t r, size_t rR>
    struct ComputeDiffusionTensor
    {
      static_assert(AlwaysFalse<DT>::value, "Not implemented for these dimensions!");
    };

    template <class DT, size_t d>
    struct ComputeDiffusionTensor<DT, d, d>
    {
      static RangeFieldType min_eigenvalue_of(const DT& diffusion_tensor, const EntityType& ent)
      {
#if !HAVE_EIGEN
        static_assert(AlwaysFalse<DT>::value, "You are missing eigen!");
#else
        const auto local_diffusion_tensor = diffusion_tensor.local_function(ent);
        assert(local_diffusion_tensor->order() == 0);
        const auto& reference_element = ReferenceElements<DomainFieldType, dimDomain>::general(ent.type());
        const Stuff::LA::EigenDenseMatrix<RangeFieldType> tensor =
            local_diffusion_tensor->evaluate(reference_element.position(0, 0));
        ::Eigen::EigenSolver<typename Stuff::LA::EigenDenseMatrix<RangeFieldType>::BackendType> eigen_solver(
            tensor.backend());
        assert(eigen_solver.info() == ::Eigen::Success);
        const auto eigenvalues = eigen_solver.eigenvalues(); // <- this should be an Eigen vector of std::complex
        RangeFieldType min_ev = std::numeric_limits<RangeFieldType>::max();
        for (size_t ii = 0; ii < boost::numeric_cast<size_t>(eigenvalues.size()); ++ii) {
          // assert this is real
          assert(std::abs(eigenvalues[ii].imag()) < 1e-15);
          // assert that this eigenvalue is positive
          const RangeFieldType eigenvalue = eigenvalues[ii].real();
          assert(eigenvalue > 1e-15);
          min_ev = std::min(min_ev, eigenvalue);
        }
        return min_ev;
#endif  // HAVE_EIGEN
      } // ... min_eigenvalue_of_(...)
    };  // class Compute< ..., d, d >

  public:
    Localfunction(const EntityType& ent, const DiffusionFactorType& diffusion_factor,
                  const DiffusionTensorType& diffusion_tensor, const RangeFieldType poincare_constant)
      : BaseType(ent), value_(0)
    {
      const RangeFieldType min_diffusion_factor =
          ComputeDiffusionFactor<DiffusionFactorType,
                                 DiffusionFactorType::dimRange,
                                 DiffusionFactorType::dimRangeCols>::min_of(diffusion_factor, ent);
      const RangeFieldType min_eigen_value_diffusion_tensor =
          ComputeDiffusionTensor<DiffusionTensorType,
                                 DiffusionTensorType::dimRange,
                                 DiffusionTensorType::dimRangeCols>::min_eigenvalue_of(diffusion_tensor, ent);
      assert(min_diffusion_factor > RangeFieldType(0));
      assert(min_eigen_value_diffusion_tensor > RangeFieldType(0));
      const DomainFieldType hh = compute_diameter_of_(ent);
      value_                   = (poincare_constant * hh * hh) / (min_diffusion_factor * min_eigen_value_diffusion_tensor);
    } // Localfunction(...)

    Localfunction(const Localfunction& /*other*/) = delete;

    Localfunction& operator=(const Localfunction& /*other*/) = delete;

    virtual size_t order() const override final { return 0; }

    virtual void evaluate(const DomainType& UNUSED_UNLESS_DEBUG(xx), RangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret[0] = value_;
    }

    virtual void jacobian(const DomainType& UNUSED_UNLESS_DEBUG(xx), JacobianRangeType& ret) const override final
    {
      assert(this->is_a_valid_point(xx));
      ret *= RangeFieldType(0);
    }

  private:
    static DomainFieldType compute_diameter_of_(const EntityType& ent)
    {
      DomainFieldType ret(0);
      for (auto cc : DSC::valueRange(ent.template count<dimDomain>())) {
        const auto vertex = ent.template subEntity<dimDomain>(cc)->geometry().center();
        for (auto dd : DSC::valueRange(cc + 1, ent.template count<dimDomain>())) {
          const auto other_vertex = ent.template subEntity<dimDomain>(dd)->geometry().center();
          const auto diff         = vertex - other_vertex;
          ret                     = std::max(ret, diff.two_norm());
        }
      }
      return ret;
    } // ... compute_diameter_of_(...)

    RangeFieldType value_;
  }; // class Localfunction

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::LocalfunctionType LocalfunctionType;

  typedef typename BaseType::DomainFieldType DomainFieldType;
  static const size_t dimDomain = BaseType::dimDomain;
  typedef typename BaseType::DomainType DomainType;

  typedef typename BaseType::RangeFieldType RangeFieldType;
  static const size_t dimRange     = BaseType::dimRange;
  static const size_t dimRangeCols = BaseType::dimRangeCols;
  typedef typename BaseType::RangeType RangeType;

  static std::string static_id() { return BaseType::static_id() + ".ESV2007.cutoff"; }

  Cutoff(const DiffusionFactorType& diffusion_factor, const DiffusionTensorType& diffusion_tensor,
         const RangeFieldType poincare_constant = 1.0 / (M_PIl * M_PIl), const std::string nm = static_id())
    : diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
    , poincare_constant_(poincare_constant)
    , name_(nm)
  {
  }

  Cutoff(const ThisType& other) = default;

  ThisType& operator=(const ThisType& other) = delete;

  virtual std::string name() const override final { return name_; }

  virtual std::unique_ptr<LocalfunctionType> local_function(const EntityType& entity) const override final
  {
    return std::unique_ptr<Localfunction>(
        new Localfunction(entity, diffusion_factor_, diffusion_tensor_, poincare_constant_));
  }

private:
  const DiffusionFactorType& diffusion_factor_;
  const DiffusionTensorType& diffusion_tensor_;
  const RangeFieldType poincare_constant_;
  std::string name_;
}; // class Cutoff

} // namespace ESV2007
} // namespace Functions
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_FUNCTIONS_ESV2007_HH


#include "LinearMuscleConstraint.h"
#include <Eigen/SVD>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <iostream>
using namespace FEM;

LinearMuscleConstraint::
LinearMuscleConstraint(const double& stiffness,
		const Eigen::Vector3d& fiber_direction,
		const double& activation_level,
		int i0,int i1,int i2,int i3,
		double vol,const Eigen::Matrix3d& invDm,
		// const Eigen::Vector4d& barycentric1,const Eigen::Vector4d& barycentric2,
		double weight)
	:Constraint(stiffness),
	mi0(i0),mi1(i1),mi2(i2),mi3(i3),
	mStiffness(stiffness),mFiberDirection(fiber_direction),
	mVol(vol),mInvDm(invDm),mDs(Eigen::Matrix3d::Zero()),
	mActivationLevel(activation_level),mWeight(weight)
{
	mF.setZero();
	// mBarycentric.clear();
	// mBarycentric.push_back(barycentric1);
	// mBarycentric.push_back(barycentric2);

	mStiffness *= mWeight;
}
void
LinearMuscleConstraint::
ComputeF
(const Eigen::VectorXd& x)
{
	Eigen::Vector3d x0(x.block<3,1>(mi0*3,0));

	Eigen::Matrix3d Ds;

	Ds.block<3,1>(0,0) = x.block<3,1>(mi1*3,0)-x0;
	Ds.block<3,1>(0,1) = x.block<3,1>(mi2*3,0)-x0;
	Ds.block<3,1>(0,2) = x.block<3,1>(mi3*3,0)-x0;

	mDs = Ds;
	mF = mDs * mInvDm;
}
void
LinearMuscleConstraint::
ComputeP
(Eigen::Matrix3d& P)
{
	P = mStiffness*(mF*mFiberDirection*mFiberDirection.transpose() - mp0*mFiberDirection.transpose());
}
void
LinearMuscleConstraint::
ComputedPdF
(Tensor3333& dPdF)
{
	Tensor3333 dFdF;
	dFdF.SetIdentity();

	for(int i=0; i<3;i++)
		for(int j=0;j<3;j++)
			dPdF(i,j) = mStiffness*dFdF(i,j)*mFiberDirection*mFiberDirection.transpose();
}
void
LinearMuscleConstraint::
EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
{
	Eigen::MatrixXd Ai(3,12);

	Eigen::Vector3d v = mInvDm*mFiberDirection;

	double a,b,c;
	a = v[0];
	b = v[1];
	c = v[2];

	Ai<<
		-(a+b+c),0,0,a,0,0,b,0,0,c,0,0,
		0,-(a+b+c),0,0,a,0,0,b,0,0,c,0,
		0,0,-(a+b+c),0,0,a,0,0,b,0,0,c;

	Eigen::MatrixXd MuAiT = mVol*mStiffness*Ai.transpose();

	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+0,MuAiT(3*0+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+1,MuAiT(3*0+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+0,3*index+2,MuAiT(3*0+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+0,MuAiT(3*0+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+1,MuAiT(3*0+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+1,3*index+2,MuAiT(3*0+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+0,MuAiT(3*0+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+1,MuAiT(3*0+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi0+2,3*index+2,MuAiT(3*0+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+0,MuAiT(3*1+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+1,MuAiT(3*1+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+0,3*index+2,MuAiT(3*1+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+0,MuAiT(3*1+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+1,MuAiT(3*1+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+1,3*index+2,MuAiT(3*1+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+0,MuAiT(3*1+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+1,MuAiT(3*1+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi1+2,3*index+2,MuAiT(3*1+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+0,MuAiT(3*2+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+1,MuAiT(3*2+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+0,3*index+2,MuAiT(3*2+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+0,MuAiT(3*2+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+1,MuAiT(3*2+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+1,3*index+2,MuAiT(3*2+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+0,MuAiT(3*2+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+1,MuAiT(3*2+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi2+2,3*index+2,MuAiT(3*2+2,3*0+2)));

	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+0,MuAiT(3*3+0,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+1,MuAiT(3*3+0,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+0,3*index+2,MuAiT(3*3+0,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+0,MuAiT(3*3+1,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+1,MuAiT(3*3+1,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+1,3*index+2,MuAiT(3*3+1,3*0+2)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+0,MuAiT(3*3+2,3*0+0)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+1,MuAiT(3*3+2,3*0+1)));
	J_triplets.push_back(Eigen::Triplet<double>(3*mi3+2,3*index+2,MuAiT(3*3+2,3*0+2)));
}
void
LinearMuscleConstraint::
EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
{
	Eigen::MatrixXd Ai(3,12);
	Eigen::Vector3d v = mInvDm*mFiberDirection;

	double a,b,c;
	a = v[0];
	b = v[1];
	c = v[2];

	Ai<<
		-(a+b+c),0,0,a,0,0,b,0,0,c,0,0,
		0,-(a+b+c),0,0,a,0,0,b,0,0,c,0,
		0,0,-(a+b+c),0,0,a,0,0,b,0,0,c;

	auto MuAiTAi = mVol*mStiffness*((Ai.transpose())*Ai);

	int idx[4] = {mi0,mi1,mi2,mi3};

	for(int i =0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+0,MuAiTAi(3*i+0,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+1,MuAiTAi(3*i+0,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0,3*idx[j]+2,MuAiTAi(3*i+0,3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+0,MuAiTAi(3*i+1,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+1,MuAiTAi(3*i+1,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1,3*idx[j]+2,MuAiTAi(3*i+1,3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+0,MuAiTAi(3*i+2,3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+1,MuAiTAi(3*i+2,3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2,3*idx[j]+2,MuAiTAi(3*i+2,3*j+2)));
		}
	}
}
void
LinearMuscleConstraint::
EvaluateDVector(const Eigen::VectorXd& x)
{
	ComputeF(x);
	Computep0();
}
void
LinearMuscleConstraint::
GetDVector(int& index,Eigen::VectorXd& d)
{
	d.block<3,1>(3*index,0) = mp0;
	index++;
}
void
LinearMuscleConstraint::
Computep0()
{
	mp0 = (1.0-mActivationLevel)*mF*mFiberDirection;
}
int
LinearMuscleConstraint::
GetDof()
{
	return 1;
}
ConstraintType
LinearMuscleConstraint::
GetType()
{
	return ConstraintType::LINEAR_MUSCLE;
}
void
LinearMuscleConstraint::
SetActivationLevel(const double& a)
{
	mActivationLevel = a;
}
const double&
LinearMuscleConstraint::
GetActivationLevel()
{
	return mActivationLevel;
}
const Eigen::Vector3d&
LinearMuscleConstraint::
GetFiberDirection()
{
	return mFiberDirection;
}
void
LinearMuscleConstraint::
SetActivationIndex(const int& i)
{
	mActivationIndex = i;
}


#include <iostream>
#include <vector>
#include <random>
#include <fstream>
#include <omp.h>
#include <boost/numeric/odeint.hpp>
#include "cahnhilliard_thermal_nodiffusion.h"
#include "utils_ch.h"

  /*
  Cahn-Hilliard:

  dc/dt = laplacian( u*c^3 - b*c ) - eps_2*biharm(c) - sigma*(c - m) + sigma_noise * N(0,1^2)

  expanding out RHS into individual differentials:
  D*laplacian( u*c^3 - b*c) - D*eps_2*biharm(c)
  assuming constant eps_2.

  need a d^4 and a d^2 operator.
  */

CahnHilliard2DRHS_thermal_nodiffusion::CahnHilliard2DRHS_thermal_nodiffusion(CHparamsScalar& chp , SimInfo& info)
  : noise_dist_(0.0,1.0) , info_(info)
  {
    chpV_.eps_2    = std::vector<double>( info_.nx*info_.ny , chp.eps_2     );
    chpV_.b        = std::vector<double>( info_.nx*info_.ny , chp.b         );
    chpV_.u        = std::vector<double>( info_.nx*info_.ny , chp.u         );
    chpV_.sigma    = std::vector<double>( info_.nx*info_.ny , chp.sigma     );
    chpV_.m        = std::vector<double>( info_.nx*info_.ny , chp.m  );
    chpV_.DT       = std::vector<double>( info_.nx*info_.ny , chp.DT  );
    chpV_.f_T      = std::vector<double>( info_.nx*info_.ny , chp.f_T  );
    chpV_.sigma_noise    = chp.sigma_noise;
    chpV_.T_const        = std::vector<double>( info_.nx*info_.ny , chp.T_const  );

    if ( info.bc.compare("dirichlet") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_stationary_boundaries;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, dirichlet BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else if ( info.bc.compare("neumann") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_neumannBC;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, neumann BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else {
      ch_rhs_ = &compute_ch_nonlocal;
      std::cout << "Initialized Cahn-Hilliard equation: scalar parameters, periodic BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }

  }

CahnHilliard2DRHS_thermal_nodiffusion::CahnHilliard2DRHS_thermal_nodiffusion(CHparamsVector& chp , SimInfo& info)
  : noise_dist_(0.0,1.0) , chpV_(chp) , info_(info)
  {

    if ( info.bc.compare("dirichlet") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_stationary_boundaries;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, dirichlet BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else if ( info.bc.compare("neumann") == 0) {
      ch_rhs_ = &compute_ch_nonlocal_neumannBC;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, neumann BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }
    else {
      ch_rhs_ = &compute_ch_nonlocal;
      std::cout << "Initialized Cahn-Hilliard equation: spatial-field parameters, periodic BCs, thermal coefficient dependence, no thermal diffusion" << std::endl;
    }

  }

CahnHilliard2DRHS_thermal_nodiffusion::~CahnHilliard2DRHS_thermal_nodiffusion() { };

void CahnHilliard2DRHS_thermal_nodiffusion::rhs(const std::vector<double> &c, std::vector<double> &dcdt, const double t)
  {
    dcdt.resize(info_.nx*info_.ny);

    // evaluate CH parameter dependencies on temperature
    //chpV_ = compute_chparams_using_temperature( chpV_ , info_ , chpV_.T_const );
    chpV_ = compute_eps2_and_sigma_from_polymer_params( chpV_ , info_ , chpV_.T_const );

    // evaluate deterministic nonlocal dynamics
    compute_ch_nonlocal(c, dcdt, t, chpV_, info_);

  }

void CahnHilliard2DRHS_thermal_nodiffusion::setInitialConditions(std::vector<double> &x)
  {
    x.resize(info_.nx * info_.ny);

    std::default_random_engine generator;
    std::uniform_real_distribution<double> distribution(-1.0,1.0);

    for (int i = 0; i < info_.ny; ++i) {
      for (int j = 0; j < info_.nx; ++j) {
        x[info_.idx2d(i,j)]   = distribution(generator) * 0.005;
      }
    }

    // Set BCs if needed
    if ( info_.bc.compare("dirichlet") == 0) {
      x = apply_dirichlet_bc( x , info_ );
    }
    else if ( info_.bc.compare("neumann") == 0 ) {
      x = apply_neumann_bc( x , info_ );
    }

  }

void CahnHilliard2DRHS_thermal_nodiffusion::write_state(const std::vector<double> &x , const int idx , const int nx , const int ny , std::string& outdir)
{
  if ( outdir.back() != '/' )
    outdir += '/';
  std::ofstream outC;
  outC.open( outdir + "C_" + std::to_string(idx) + ".out" );
  outC.precision(16);

  for (int i = 0; i < ny; ++i){
    for (int j = 0; j < nx; ++j){
      outC << x[i * ny + j] << " ";
    }
  }

  outC.close();
};


// Copyright PinaPL
//
// functions.cpp
// PinaPL
//
#include <stdlib.h>
#include <math.h>
#include <Eigen/Dense>
#include <string>
#include <sstream>
#include <iostream>
#include <map>

double sigmoid(double x) {
    return (1/(1+exp(-x)));
}

double sigmoid_derivative(double x) {
    return x*(1-x);
}

double tanh_derivative(double x) {
    return 1-tanh(x)*tanh(x);
}

double tanhyp(double x) {
    return tanh(x);
}


#include <iostream>
using namespace std;

#include <mtl/mtl.h>
#include <mtl/matrix.h>
#include <mtl/dense1D.h>
#include <mtl/utils.h>

namespace mtl {

template <class MatA>
inline void
simple_print(const MatA& A)
{
  typedef typename matrix_traits<MatA>::size_type Int;
  typename MatA::const_iterator A_k;
  typename MatA::OneD::const_iterator A_ki;

  A_k = A.begin();
  while (not_at(A_k, A.end())) {
    const typename MatA::OneDRef A_k_ = *A_k;
    A_ki = A_k_.begin();
    while (not_at(A_ki, A_k_.end())) {
      Int k = A_ki.column();
      Int i = A_ki.row();
      std::cout << "A(" << i << "," << k << ")" << *A_ki << std::endl;
      ++A_ki;
    }
    ++A_k;
  }
}

}

int
main()
{
  typedef mtl::matrix<double,
                  mtl::diagonal<>,
                  mtl::banded<mtl::external>,
                   mtl::column_major>::type DiagMatE;

  typedef mtl::matrix<double,
                 mtl::rectangle<>,
                 mtl::dense<mtl::external>,
                 mtl::column_major>::type MatrixE;

  typedef mtl::matrix<double,
                 mtl::rectangle<>,
                 mtl::dense<mtl::internal>,
                 mtl::column_major>::type MatrixI;

  int N = 3;
  double da [] = { 1, 3, 2, 2, 1, 2, 2, 2, 1 };
  double dc [] = { 1, 3, 2};

  MatrixE A(da, N, N);
  DiagMatE C(dc, N, N, 0, 0);
  MatrixI AxC(N,N);

  cout << "Full A:" << endl;
  mtl::print_all_matrix(A);
  cout << "Diag C:" << endl;
  mtl::print_all_banded(C,0,0);
  mtl::simple_print(C);
  cout << "Output AxC:" << endl;

  //put C as the first parameter in mult
  //since C is a banded matrix
  mtl::mult(C,A,AxC);
  mtl::print_all_matrix(AxC);

  return 0;
}


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

#include <Eigen/Cholesky>

#include "lama/nlls/gauss_newton.h"

lama::GaussNewton::Options::Options()
{
    eps1 = 1e-4;
    eps2 = 1e-4;
}

lama::GaussNewton::GaussNewton(const Options& options)
    : opt_(options)
{}

void lama::GaussNewton::reset()
{
    stop_ = false;
}

Eigen::VectorXd lama::GaussNewton::step(const VectorXd& residuals, const MatrixXd& J)
{
    VectorXd g = J.transpose() * residuals;
    chi2_ = residuals.squaredNorm();

    double max_abs_g = g.lpNorm<Eigen::Infinity>();
    if (max_abs_g < opt_.eps1){
        stop_ = true;
        return VectorXd::Zero(J.cols());
    }

    MatrixXd A = J.transpose() * J;
    // Solve the system
    VectorXd h = A.selfadjointView<Eigen::Lower>().ldlt().solve(-g);

    double max_abs_h = h.lpNorm<Eigen::Infinity>();
    if (max_abs_h < opt_.eps2)
        stop_ = true;

    return h;
}

bool lama::GaussNewton::valid(const VectorXd& residuals)
{
    if (stop_) return true;

    double dF  = chi2_ - residuals.squaredNorm();
    if (dF > 0){
        return true;
    }

    stop_ = true;
    return false;
}

bool lama::GaussNewton::stop()
{
    return stop_;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2018 - 2020 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.
 *
 * ---------------------------------------------------------------------
 */

// Solve Laplacian using SIPG + mesh_loop + ScratchData

#include <deal.II/numerics/data_out.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/patterns.h>
#include <deal.II/base/thread_management.h>

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

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

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/grid_tools_cache.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/sparse_direct.h>

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/mesh_loop.h>
#include <deal.II/meshworker/scratch_data.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <unordered_map>

using namespace dealii;
using namespace MeshWorker;

template <int dim, int spacedim>
void test()
{
  Triangulation<dim, spacedim> tria;
  FE_DGQ<dim, spacedim> fe(1);
  DoFHandler<dim, spacedim> dh(tria);

  FunctionParser<spacedim> rhs_function("1");
  FunctionParser<spacedim> boundary_function("0");

  AffineConstraints<double> constraints;
  constraints.close();

  GridGenerator::hyper_cube(tria);
  tria.refine_global(4);
  tria.execute_coarsening_and_refinement();
  dh.distribute_dofs(fe);

  SparsityPattern sparsity;

  {
    DynamicSparsityPattern dsp(dh.n_dofs(), dh.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dh, dsp);
    sparsity.copy_from(dsp);
  }

  SparseMatrix<double> matrix;
  matrix.reinit(sparsity);

  Vector<double> solution(dh.n_dofs());
  Vector<double> rhs(dh.n_dofs());

  QGauss<dim> quad(3);
  QGauss<dim - 1> face_quad(3);

  UpdateFlags cell_flags = update_values | update_gradients |
                           update_quadrature_points | update_JxW_values;
  UpdateFlags face_flags = update_values | update_gradients |
                           update_quadrature_points |
                           update_face_normal_vectors | update_JxW_values;

  // Stabilization for SIPG
  double gamma = 1;

  using ScratchData = MeshWorker::ScratchData<dim, spacedim>;
  using CopyData = MeshWorker::CopyData<1 + GeometryInfo<dim>::faces_per_cell,
                                        1,
                                        1 + GeometryInfo<dim>::faces_per_cell>;

  ScratchData scratch(fe, quad, cell_flags, face_quad, face_flags);
  CopyData copy(fe.dofs_per_cell);

  auto cell = dh.begin_active();
  auto endc = dh.end();

  typedef decltype(cell) Iterator;

  auto cell_worker =
      [&rhs_function](const Iterator &cell, ScratchData &s, CopyData &c) {
        const auto &fev = s.reinit(cell);
        const auto &JxW = s.get_JxW_values();
        const auto &p = s.get_quadrature_points();

        c.local_dof_indices[0] = s.get_local_dof_indices();

        for (unsigned int q = 0; q < p.size(); ++q)
          for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
            {
              c.matrices[0](i, j) +=
                  fev.shape_grad(i, q) * fev.shape_grad(j, q) * JxW[q];
            }
            c.vectors[0](i) +=
                fev.shape_value(i, q) * rhs_function.value(p[q]) * JxW[q];
          }
      };

  auto boundary_worker = [gamma, &boundary_function](const Iterator &cell,
                                                     const unsigned int &f,
                                                     ScratchData &s,
                                                     CopyData &c) {
    const auto &fev = s.reinit(cell, f);
    const auto &JxW = s.get_JxW_values();
    const auto &p = s.get_quadrature_points();
    const auto &n = s.get_normal_vectors();

    for (unsigned int q = 0; q < p.size(); ++q)
      for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
      {
        for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
        {
          c.matrices[0](i, j) +=
              (-fev.shape_grad(i, q) * n[q] * fev.shape_value(j, q) +
               -fev.shape_grad(j, q) * n[q] * fev.shape_value(i, q) +
               gamma / cell->face(f)->diameter() * fev.shape_value(i, q) *
                   fev.shape_value(j, q)) *
              JxW[q];
        }
        c.vectors[0](i) +=
            ((gamma / cell->face(f)->diameter() * fev.shape_value(i, q) -
              fev.shape_grad(i, q) * n[q]) *
             boundary_function.value(p[q])) *
            JxW[q];
      }
  };

  auto face_worker = [gamma](const Iterator &cell,
                             const unsigned int &f,
                             const unsigned int &sf,
                             const Iterator &ncell,
                             const unsigned int &nf,
                             const unsigned int &nsf,
                             ScratchData &s,
                             CopyData &c) {
    const auto &fev = s.reinit(cell, f, sf);
    const auto &JxW = s.get_JxW_values();
    const auto &nfev = s.reinit_neighbor(ncell, nf, nsf);

    c.local_dof_indices[f + 1] = s.get_neighbor_dof_indices();

    const auto &p = s.get_quadrature_points();
    const auto &n = s.get_normal_vectors();
    const auto &nn = s.get_neighbor_normal_vectors();

    const double gh = gamma / cell->face(f)->diameter();

    for (unsigned int q = 0; q < p.size(); ++q)
      for (unsigned int i = 0; i < fev.dofs_per_cell; ++i)
        for (unsigned int j = 0; j < fev.dofs_per_cell; ++j)
        {
          c.matrices[0](i, j) +=
              (-.5 * fev.shape_grad(i, q) * n[q] * fev.shape_value(j, q) +
               -.5 * fev.shape_value(i, q) * n[q] * fev.shape_grad(j, q) +
               gh * fev.shape_value(i, q) * fev.shape_value(j, q)) *
              JxW[q];

          c.matrices[f + 1](i, j) +=
              (-.5 * fev.shape_grad(i, q) * nn[q] * nfev.shape_value(j, q) +
               -.5 * fev.shape_value(i, q) * n[q] * nfev.shape_grad(j, q) -
               gh * fev.shape_value(i, q) * nfev.shape_value(j, q)) *
              JxW[q];
        }
  };

  auto copier = [&constraints, &matrix, &rhs](const CopyData &c) {
    constraints.distribute_local_to_global(
        c.matrices[0], c.vectors[0], c.local_dof_indices[0], matrix, rhs);

    for (unsigned int f = 0; f < GeometryInfo<dim>::faces_per_cell; ++f)
      constraints.distribute_local_to_global(c.matrices[1 + f],
                                             c.local_dof_indices[0],
                                             c.local_dof_indices[1 + f],
                                             matrix);
  };

  mesh_loop(cell,
            endc,
            cell_worker,
            copier,
            scratch,
            copy,
            assemble_own_cells | assemble_boundary_faces |
                assemble_own_interior_faces_both,
            boundary_worker,
            face_worker);

  SparseDirectUMFPACK inv;
  inv.initialize(matrix);

  inv.vmult(solution, rhs);
  constraints.distribute(solution);

  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dh);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches();
    std::ofstream output("solution.vtu");
    data_out.write_vtu(output);
  }
  deallog << "Linfty norm of solution " << solution.linfty_norm() << std::endl;
}

int main()
{
  test<2, 2>();
}


#ifndef DEMO
#define DEMO

#include <Eigen/Dense>
#include <algorithm>
#include <iostream>

#include "MiniDnn/layer.h"
#include "MiniDnn/layer/conv.h"
#include "MiniDnn/layer/identity_block.h"
#include "MiniDnn/layer/conv_block.h"
#include "MiniDnn/layer/fully_connected.h"
#include "MiniDnn/layer/ave_pooling.h"
#include "MiniDnn/layer/max_pooling.h"
#include "MiniDnn/layer/relu.h"
#include "MiniDnn/layer/sigmoid.h"
#include "MiniDnn/layer/softmax.h"
#include "MiniDnn/loss.h"
#include "MiniDnn/loss/mse_loss.h"
#include "MiniDnn/loss/cross_entropy_loss.h"
#include "MiniDnn/mnist.h"
#include "MiniDnn/network.h"
#include "MiniDnn/optimizer.h"
#include "MiniDnn/optimizer/sgd.h"
#include "Enclave.h"
#include "Enclave_t.h"  /* print_string */
//#include <time.h>

using namespace std;

class RandData {
public:
    Matrix train_data;
    Matrix train_labels;
    Matrix test_data;
    Matrix test_labels;

    RandData(int size) {
        train_data = Matrix::Ones(224 * 224 * 3, size);
        train_labels = Matrix::Ones(1, size);
        test_data = Matrix::Ones(224 * 224 * 3, size);
        test_labels = Matrix::Ones(1, size);
    }
};

void ecall_ml_resnet50() {
    // data
//    MNIST dataset("/Users/rc/Study/Projects/mini-dnn/data/mnist/");
//    dataset.read();
//    int n_train = dataset.train_data.cols();
//    int dim_in = dataset.train_data.rows();
//    std::cout << "mnist train number: " << n_train << std::endl;
//    std::cout << "mnist test number: " << dataset.test_labels.cols() << std::endl;
    RandData dataset(100);
//    dataset.read();
    int n_train = dataset.train_data.cols();
    int dim_in = dataset.train_data.rows();
    // dnn
    Network dnn;

//    // Conv
//    // int channel_in, int height_in, int width_in, int channel_out, int height_kernel,
//    // int width_kernel, int stride = 1, int pad_w = 0, int pad_h = 0

    Layer *input_conv1 = new Conv(3, 224, 224, 64, 7, 7, 2, 3, 3);
    Layer *input_relu1 = new ReLU;

    Layer *b1_pool = new MaxPooling(64, 112, 112, 2, 2, 2);

    dnn.add_layer(input_conv1);
    dnn.add_layer(input_relu1);
    dnn.add_layer(b1_pool);

    //------------------------------------------------------------
    Layer *s1_conv_block = new ConvBlock(64, 56, 56, 64,
                                         64, 256, 3, 3, 1);
    Layer *s1_id_block1 = new IdentityBlock(256, 56, 56, 64, 64, 3, 3);
    Layer *s1_id_block2 = new IdentityBlock(256, 56, 56, 64, 64, 3, 3);

    dnn.add_layer(s1_conv_block);
    dnn.add_layer(s1_id_block1);
    dnn.add_layer(s1_id_block2);

    //------------------------------------------------------------

    Layer *s2_conv_block = new ConvBlock(256, 56, 56, 128,
                                         128, 512, 3, 3, 2);
    Layer *s2_id_block1 = new IdentityBlock(512, 28, 28, 128, 128, 3, 3);
    Layer *s2_id_block2 = new IdentityBlock(512, 28, 28, 128, 128, 3, 3);

    dnn.add_layer(s2_conv_block);
    dnn.add_layer(s2_id_block1);
    dnn.add_layer(s2_id_block2);

    //------------------------------------------------------------

    Layer *s3_conv_block = new ConvBlock(512, 28, 28, 256,
                                         256, 1024, 3, 3, 2);
    Layer *s3_id_block1 = new IdentityBlock(1024, 14, 14, 256, 256, 3, 3);
    Layer *s3_id_block2 = new IdentityBlock(1024, 14, 14, 256, 256, 3, 3);

    dnn.add_layer(s3_conv_block);
    dnn.add_layer(s3_id_block1);
    dnn.add_layer(s3_id_block2);
    //------------------------------------------------------------

    Layer *s4_conv_block = new ConvBlock(1024, 14, 14, 512,
                                         512, 2048, 3, 3, 2);
    Layer *s4_id_block1 = new IdentityBlock(2048, 7, 7, 512, 512, 3, 3);
    Layer *s4_id_block2 = new IdentityBlock(2048, 7, 7, 512, 512, 3, 3);

    dnn.add_layer(s4_conv_block);
    dnn.add_layer(s4_id_block1);
    dnn.add_layer(s4_id_block2);
    //------------------------------------------------------------

    Layer *out_pool = new MaxPooling(2048, 7, 7, 7, 7, 1);

//    Layer *b1_pool = new MaxPooling(64, 28, 28, 2, 2, 2);
    Layer *fc_fc1 = new FullyConnected(out_pool->output_dim(), 1000);
    Layer *softmax = new Softmax;

    dnn.add_layer(out_pool);
    dnn.add_layer(fc_fc1);
    dnn.add_layer(softmax);

    // loss
    Loss *loss = new CrossEntropy;
    dnn.add_loss(loss);
    // train & test
    SGD opt(0.001, 5e-4, 0.9, true);
    // SGD opt(0.001);
    // const int n_epoch = 5;
    const int total_rounds[4] = {1, 2, 3, 4};
    const int batch_size = 20;
    for (int idx = 0; idx < 4; idx++) {
        int n_epoch = total_rounds[idx];
        printf("total_rounds: %d\n", 5 * n_epoch);
        ocall_start_clock();
        for (int epoch = 0; epoch < n_epoch; epoch++) {
            shuffle_data(dataset.train_data, dataset.train_labels);
            for (int start_idx = 0; start_idx < n_train; start_idx += batch_size) {
            // for(int start_idx=0, rounds=0; rounds < total_rounds; start_idx += batch_size){
                int ith_batch = start_idx / batch_size;
                Matrix x_batch = dataset.train_data.block(0, start_idx, dim_in,
                                                          std::min(batch_size, n_train - start_idx));
                Matrix label_batch = dataset.train_labels.block(0, start_idx, 1,
                                                                std::min(batch_size, n_train - start_idx));
                Matrix target_batch = one_hot_encode(label_batch, 1000);
                // if (false && ith_batch % 10 == 1) {
                //     std::cout << ith_batch << "-th grad: " << std::endl;
                //     dnn.check_gradient(x_batch, target_batch, 10);
                // }

                dnn.forward(x_batch);
                // ocall_end_clock("Forward: %f   ");

                // ocall_start_clock();
                dnn.backward(x_batch, target_batch);

    //            // display
    //            if (ith_batch % 2 == 0) {
    //                //std::cout << ith_batch << "-th batch, loss: " << dnn.get_loss() << std::endl;
    //                printf("%d-th batch, loss: %f\n", ith_batch, dnn.get_loss());
    //            }
                // optimize
                dnn.update(opt);
                // rounds++;
            }
        }
        ocall_end_clock("total: %f\n");

// //        // test
//         dnn.forward(dataset.test_data);
//         float acc = compute_accuracy(dnn.output(), dataset.test_labels);
        // std::cout << std::endl;
        // std::cout << epoch + 1 << "-th epoch, test acc: " << acc << std::endl;
        // std::cout << std::endl;
    }
}

#endif