// inverting A
// using getrf() & getri()

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmpx;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmpx, ublas::row_major> cm_t;
#endif

int main() {

  cout << endl;
  cout << "real matrix:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);
  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]
  print_m (a, "A");
  cout << endl;

  m_t aa (a);  // copy of a, for later use

  std::vector<int> ipiv (n);   // pivot vector
  atlas::lu_factor (a, ipiv);  // alias for getrf()
  atlas::lu_invert (a, ipiv);  // alias for getri()

  m_t i1 (n, n), i2 (n, n);
  atlas::gemm (a, aa, i1);   // i1 should be (almost) identity matrix
  atlas::gemm (aa, a, i2);   // i2 should be (almost) identity matrix

  print_m (i1, "I = A^(-1) * A");
  cout << endl;
  print_m (i2, "I = A * A^(-1)");
  cout << endl;

  cout << endl;

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

  cout << "complex matrix:" << endl << endl;
  cm_t ca (3, 3);

  ca (0, 0) = cmpx (3, 0);
  ca (0, 1) = cmpx (4, 2);
  ca (0, 2) = cmpx (-7, 5);
  ca (1, 0) = cmpx (4, -2);
  ca (1, 1) = cmpx (-5, 0);
  ca (1, 2) = cmpx (0, -3);
  ca (2, 0) = cmpx (-7, -5);
  ca (2, 1) = cmpx (0, 3);
  ca (2, 2) = cmpx (2, 0);
  print_m (ca, "CA");
  cout << endl;

  cm_t caa (ca);

  std::vector<int> ipiv2 (3);

  int ierr = atlas::getrf (ca, ipiv2);
  if (ierr == 0) {
    atlas::getri (ca, ipiv2);
    cm_t ii (3, 3);
    atlas::gemm (ca, caa, ii);
    print_m (ii, "I = CA^(-1) * CA");
    cout << endl;
    atlas::gemm (caa, ca, ii);
    print_m (ii, "I = CA * CA^(-1)");
    cout << endl;
  }
  else
    cout << "matrix is singular" << endl;

  cout << endl;

}


#ifndef __CAD_UTIL_HH__
#define __CAD_UTIL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (CAD Utility functions from Rocket6G)
LIBRARY DEPENDENCY:
      ((../src/cad_utility.cpp))
*******************************************************************************/
#include <tuple>
#include "math_utility.hh"
#include <armadillo>

namespace cad {
    /**
     * @return great circle distance between two point on a spherical Earth
     * @brief Refrence: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p. 310
     *
     * @param[in] lon1 longitude of first  point - rad
     * @param[in] lat1 latitude  of first  point - rad
     * @param[in] lon2 longitude of second point - rad
     * @param[in] lat2 latitude  of second point - rad
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     */

    double distance(const double &lon1,
                    const double &lat1,
                    const double &lon2,
                    const double &lat2);

    /**
     * @brief Calculates geodetic longitude, latitude, and altitude from inertial
     *        displacement vector
     *        using the WGS 84 reference ellipsoid
     *        Reference: Britting,K.R."Inertial Navigation Systems Analysis", Wiley. 1971
     *
     * @return std::tuple(lon, lat, alt)
     *                    lon geodetic longitude - rad
     *                    lat geodetic latitude - rad
     *                    alt altitude above ellipsoid - m
     *
     * @param[in] SBII(3x1) = Inertial position - m
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    std::tuple<double, double, double> geo84_in(arma::vec3 SBII,
                                                arma::mat33 TEI);

    /**
     * @return geodetic velocity vector information from inertial postion and
     * velocity
     * using the WGS 84 reference ellipsoid
     *
     * @brief Calls utilities
     *    geo84_in(...), tdi84(...)
     *
     * @param[out] dvbe geodetic velocity - m/s
     * @param[out] psivdx geodetic heading angle - deg
     * @param[out] thtvdx geodetic flight path angle - deg
     *
     * @param[in] SBII(3x1) Inertial position - m
     * @param[in] VBII(3x1) Inertial velocity - m
     *
     * @author 040710 created by Peter H Zipfel
     */
    std::tuple<double, double, double> geo84vel_in(arma::vec3 SBII,
                                                   arma::vec3 VBII,
                                                   arma::mat33 TEI);
    /**
     * @return geocentric lon, lat, alt from inertial displacement vector
     * for spherical Earth
     *
     * @param[out] lonc = geocentric longitude - rad
     * @param[out] latc = geocentric latitude - rad
     * @param[out] altc = geocentric latitude - m
     *
     * @param[in] SBII = Inertial position - m
     * @param[in] time = simulation time - sec
     *
     * @author 010628 Created by Peter H Zipfel
     * @author 030416 Modified for SBII (not SBIE) input, PZi
     */
    std::tuple<double, double, double> geoc_in(arma::vec3 SBII,
                                               const double &time);

    /**
     * @return lon, lat, alt from displacement vector in Earth coord for spherical
     * earth
     * RETURN[0]=lon
     * RETURN[1]=lat
     * RETURN[2]=alt
     *
     * @param[in] SBIE = displacement of vehicle wrt Earth center in Earth coordinates
     *
     * @author 010628 Created by Peter H Zipfel
     */
    std::tuple<double, double, double>  geoc_ine(arma::vec3 SBIE);

    /**
     * @brief Earth gravitational acceleration, using the WGS 84 ellipsoid
     *      Ref: Chatfield, A.B.,"Fundamentals of High Accuracy Inertial
     *      Navigation",p.10, Prog.Astro and Aeronautics, Vol 174, AIAA, 1997.
     *
     * @return GRAVG(3x1) = gravitational acceleration in geocentric coord - m/s^2
     *
     * @param[in] SBII = inertial displacement vector - m
     * @param[in] time = simulation time - sec
     *
     * @author 030417 Created from FORTRAN by Peter H Zipfel
     */
    arma::vec3 grav84(arma::vec3 SBII, const double &time);

    /**
     * @brief Returns the inertial displacement vector from longitude, latitude and
     *      altitude
     *      using the WGS 84 reference ellipsoid
     *      Reference: Britting,K.R."Inertial Navigation Systems Analysis"
     *      pp.45-49, Wiley, 1971
     *
     * @return SBII(3x1) = Inertial vehicle position - m
     *
     * @param[in] lon = geodetic longitude - rad
     * @param[in] lat = geodetic latitude - rad
     * @param[in] alt = altitude above ellipsoid - m
     * @param[in] time = simulation time - sec
     *
     * @author 030411 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyanga
     */
    arma::vec3 in_geo84(const double lon,
                        const double lat,
                        const double alt,
                        arma::mat33 TEI);

    /**
     * @brief Returns the inertial displacement vector from geocentric longitude, latitude
     *      and altitude
     *      for spherical Earth
     *
     * @return SBII = position of vehicle wrt center of Earth, in inertial coord
     *
     * @param[in] lon = geographic longitude - rad
     * @param[in] lat = geocentric latitude - rad
     * @param[in] alt = altitude above spherical Earth = m
     *
     * @author 010405 Created by Peter H Zipfel
     */
    arma::vec3 in_geoc(const double &lon,
                       const double &lat,
                       const double &alt,
                       const double &time);

    /**
     * @brief Calculates inertial displacement and velocity vectors from orbital elements
     *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.71
     *
     * @return parabola_flag = 0 ok
     *                       = 1 not suitable (divide by zero), because parabolic
     *                           trajectory
     *
     * @param[out] SBII = Inertial position - m
     * @param[out] SBII = Inertial velocity - m/s
     *
     * @param[in] semi = semi-major axis of orbital ellipsoid - m
     * @param[in] ecc = eccentricity of elliptical orbit - ND
     * @param[in] inclx = inclination of orbital wrt equatorial plane - deg
     * @param[in] lon_anodex = celestial longitude of the ascending node - deg
     * @param[in] arg_perix = argument of periapsis (ascending node to periapsis) - deg
     * @param[in] true_anomx = true anomaly (periapsis to satellite) - deg
     *
     * @author 040510 Created by Peter H Zipfel
     */
    int in_orb(arma::vec3 &SBII,
               arma::vec3 &VBII,
               const double &semi,
               const double &ecc,
               const double &inclx,
               const double &lon_anodex,
               const double &arg_perix,
               const double &true_anomx);

    /**
     * @brief Projects initial state through 'tgo' to final state along a Keplerian
     *      trajectory
     *      Based on Ray Morth, unpublished utility
     *
     * @return kepler_flan = 0: good Kepler projection;
     *                     = 1: bad (# of iterations>20, or neg. sqrt), no new proj cal,
     *                          use prev value;  - ND
     * @param[out] SPII = projected inertial position after tgo - m
     * @param[out] VPII = projected inertial velocity after tgo - m/s
     *
     * @param[in] SBII = current inertial position - m
     * @param[in] VBII = current inertial velocity - m/s
     * @param[in] tgo = time-to-go to projected point - sec
     *
     * @author 040319 Created from FORTRAN by Peter H Zipfel
     */
    int kepler(arma::vec3 &SPII,
               arma::vec3 &VPII,
               arma::vec3 SBII,
               arma::vec3 VBII,
               const double &tgo);

    /**
     * @brief Projects initial state through 'tgo' to final state along a Keplerian
     *      trajectory
     *      Based on: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971
     *
     * @return iter_flan = 0: # of iterations < 20;
     *                   = 1: # of iterations > 20;  - ND
     *
     * @param[out] SPII = projected inertial position after tgo - m
     * @param[out] VPII = projected inertial velocity after tgo - m/s
     *
     * @param[in] SBII = current inertial position - m
     * @param[in] VBII = current inertial velocity - m/s
     * @param[in] tgo = time-to-go to projected point - sec
     *
     * @author 040318 Created from ASTRO_KEP by Peter H Zipfel
     */
    int kepler1(arma::vec3 &SPII,
                arma::vec3 &VPII,
                arma::vec3 SBII,
                arma::vec3 VBII,
                const double &tgo);

    /**
     * @brief Calculates utility functions c(z) and s(z) for kepler(...)
     *      Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971,
     *      p.196
     *
     * @param[out] c = c(z) utility function
     * @param[out] s = s(z) utility function
     *
     * @param[in] z = z-variable
     *
     * @author 040318 Created from ASTRO_UCS by Peter H Zipfel
     */
    std::tuple<double, double> kepler1_ucs(const double &z);

    /**
     * @brief Calculates the orbital elements from inertial displacement and velocity
     *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.58
     *
     * @return cadorbin_flag = 0 ok
     *                         1 'true_anomx' not calculated, because of circular orbit
     *                         2 'semi' not calculated, because parabolic orbit
     *                         3 'lon_anodex' not calculated, because equatorial orbit
     *                         13 'arg_perix' not calculated, because equatorialand/or
     *                            circular orbit
     * @param[out] semi = semi-major axis of orbital ellipsoid - m
     * @param[out] ecc = eccentricity of elliptical orbit - ND
     * @param[out] inclx = inclination of orbital wrt equatorial plane - deg
     * @param[out] lon_anodex = celestial longitude of the ascending node - deg
     * @param[out] arg_perix = argument of periapsis (ascending node to periapsis) - deg
     * @param[out] true_anomx = true anomaly (periapsis to satellite) - deg
     *
     * @param[in] SBII = Inertial position - m
     * @param[in] VBII = Inertial velocity - m/s
     *
     * @author 040510 Created by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    int orb_in(double &semi,
                       double &ecc,
                       double &inclx,
                       double &lon_anodex,
                       double &arg_perix,
                       double &true_anomx,
                       arma::vec3 &SBII,
                       arma::vec3 &VBII);

    /**
     * @brief Returns the T.M. of geodetic wrt inertial coordinates
     *        using the WGS 84 reference ellipsoid
     *
     * @return TDI(3x3) = T.M.of geosetic wrt inertial coord - ND
     *
     * @param[out] lon = geodetic longitude - rad
     * @param[out] lat = geodetic latitude - rad
     * @param[out] alt = altitude above ellipsoid - m
     *
     * @author 030424 Created by Peter H Zipfel
     * @author 170121 Create Armadillo Version by sonicyang
     */
    arma::mat33 tdi84(const double &lon,
                               const double &lat,
                               const double &alt,
                               arma::mat33 TEI);

    arma::mat33 tde84(const double &lon,
                               const double &lat,
                               const double &alt);

    /**
     * @brief Returns the T.M. of earth wrt inertial coordinates
     *
     * @return TEI = T.M. of Earthy wrt inertial coordinates
     *
     * @param[in] time = since start of simulation - s
     *
     * @author 010628 Created by Peter H Zipfel
     */
    arma::mat33 tei(const double &time);

    /**
     * @return TGE = the T.M. of geographic wrt earth coordinates, TGE
     *               spherical Earth only
     *
     * @param[in] lon = geographic longitude - rad
     * @param[in] lat = geographic latitude - rad
     *
     * @author 010628 Created by Peter H Zipfel
     */
    arma::mat33 tge(const double &lon, const double &lat);

    /**
     * @brief Returns the T.M. of geographic (geocentric) wrt inertial
     *        using the WGS 84 reference ellipsoid
     *        Reference: Britting,K.R."Inertial Navigation Systems Analysis",
     *        pp.45-49, Wiley, 1971
     *
     * @return TGI(3x3) = T.M.of geographic wrt inertial coord - ND
     *
     * @param[in] lon = geodetic longitude - rad
     * @param[in] lat = geodetic latitude - rad
     * @param[in] alt = altitude above ellipsoid - m
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    arma::mat33 tgi84(const double &lon,
                      const double &lat,
                      const double &alt,
                      arma::mat33 TEI);

    /**
     * @brief Returns the transformation matrix of inertial wrt perifocal coordinates
     *
     * @return TIP = TM of inertial wrt perifocal
     *
     * @param[in] incl = inclination of orbital wrt equatorial plane - rad
     * @param[in] lon_anode = celestial longitude of the ascending node - rad
     * @param[in] arg_peri = argument of periapsis (ascending node to periapsis) - rad
     *
     * @author 040510 Created by Peter H Zipfel
     */
    arma::mat33 tip(const double &incl,
                    const double &lon_anode,
                    const double &arg_peri);
}  // namespace cad
#endif  // __CAD_UTIL_HH__


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

#include <Eigen/Core>
#include <Eigen/Geometry> //Eigen Geometry module

//----------------------------use Eigen Geometry module-------------------------
// Eigen/Geometry 提供了各种旋转和平移的表示

int main(int argc, char const *argv[])
{
   // set a 3x3 indentity matrix
   Eigen::Matrix3d R = Eigen::Matrix3d::Identity();

   //-------------------get Rotation Matrix given theta,n-----------------------
   // 创建rotation动作
   //   rot(theta,n) rot 45 degree about z-axis
   //   类型 Eigen::AngleAxisd rot(),
   //     括号里面可以是（theta, n）, 也可以是quaternion q，应该还能是很多其他类型的
   //   旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
   Eigen::AngleAxisd rot(M_PI / 4, Eigen::Vector3d(0, 0, 1));
   //   Eigen::AngleAxisd rot_2(q); //q在下面定义这里用不了
   // two ways to see the rotation matrix from rot，都可以返回一个matrix
   // 1. rot.matrix()
   // 2. rot.toRotationMatrix()
   R = rot.matrix();           //等价
   R = rot.toRotationMatrix(); //等价
   cout << "rotation matrix =\n" << R << "\n" << endl;

   // 坐标转换
   // 初始vector p
   Eigen::Vector3d p(1, 0, 0);        //这里用了括号初始化，没有赋值操作
   // p' = rot * p -> p在旋转后的坐标里面的值
   Eigen::Vector3d p_prime = rot * p; //rot.matrix()*p, rot可以直接进行运算（因为重载了运算符）
                                      //也可以用 p_prime = R * p   line23
   cout << "vector p (1,0,0) after rotation = " << p_prime.transpose() << "\n" << endl;

   //-----------------------------Euler Angle-----------------------------------
   // 可以将旋转矩阵直接转换成欧拉角，欧拉角用3个量表示3个自由度
   // 欧拉角将一个一次性的rotation matrix分解成三次旋转，返回每次旋转对应的角度
   // 所有3维matrix都可以使用.eulerAngles(0,1,2)，类型为AngleAxisd的，比如rot不能使用(line19)
   Eigen::Vector3d euler_angles // 返回一个3x1的向量
      = R.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
   cout << "yaw pitch roll = " << euler_angles.transpose() << "\n" << endl;

   //------------------------Transformation Matrix------------------------------
   // T = | R | t |
   //     | 0 | 1 |
   // given R，t，create transformation matrix（欧式变换矩阵）
   Eigen::Vector3d   t(1, 2, 4);                        //初始化t，没有赋值操作
   Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //虽然是3d，但是是4x4的矩阵
   T.rotate(R);                                         //把 R 放到 T 里面
   T.pretranslate(t);                                   //把 t 放到 T 里面
   cout << "Transform matrix = \n" << T.matrix() << "\n" << endl;

   // 坐标转换
   p_prime = T * p; //这里T是4x4，而p是3x1。实际操作是 p'=Rp+t
   cout << "p' after transformation matrix is "
        << p_prime.transpose() << "\n" << endl;

   //-------------------------------Quaternion---------------------------------
   // 将 rotation matrix R 转变为 Quaternion q
   // 用Eigen::Quaternion()
   // 括号里的参数类型可以是Matrix3d,比如R，也可以是AngleAxisd，比如rot
   Eigen::Quaterniond q = Eigen::Quaterniond(rot);
   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
   cout << "quaternion = \n" << q.coeffs() << "\n" << endl;

   // 坐标转换
   p_prime = q * p; //这里的实际操作是 p'=qpq^(-1)
   cout << "p' after Quaternion transformation is "
        << p_prime.transpose() << "\n" << endl;

   //----对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略-----

   return 0;
}


/**
 * license is free
 * \file    ontheline.cpp
 * \author  hiratake26to@gmail.com
 * \date    2017-10-11 JST
 */
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>

/**
 * \brief mouse pointer is on the line
 * \param xa  x of equipment A
 * \param ya  y of equipment A
 * \param xb  x of equipment B
 * \param yb  y of equipment B
 * \param xc  x of mouse pointer
 * \param yc  y of mouse pointer
 * \param r   line width
 */
bool isOnTheLine(int xa, int ya, int xb, int yb, int xc, int yc, int r);

int main() {
  /*    0 1 2 3 4
   * 0  _ _ _ _ _
   * 1  _ _ _ B _
   * 2  _ _ @ _ _
   * 3  _ @ _ _ _
   * 4  A _ _ _ _
   */
  bool result;
  result = isOnTheLine(0, 4, 3, 1, 2, 2, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 3, 2, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 4, 3, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 4, 0, 0);
  std::cout << "result : " << result << std::endl;
  return 0;
}

bool isOnTheLine(int xa, int ya, int xb, int yb, int xc, int yc, int r)
{
  namespace ublas = boost::numeric::ublas;

  // point C is mouse pointer
  // point X is point at the intersection of AB with CX
  // vector CX is perpendicular line from C to line AB
  ublas::vector<double> v_AB(2);
  ublas::vector<double> v_nAB(2); // nAB is unit vector of AB
  ublas::vector<double> v_AC(2);
  ublas::vector<double> v_AX(2);
  ublas::vector<double> v_CX(2);
  double len_AB;                  // length of vector AB
  double len_AX;                  // length of vector AX
  double len_CX;                  // length of vector CX

  // vector AB
  v_AB[0] = xb - xa; // AB.x
  v_AB[1] = yb - ya; // AB.y
  // length of AB
  len_AB = ublas::norm_2(v_AB);

  // unit vector nAB
  v_nAB = v_AB / len_AB;

  // vector AC
  v_AC[0] = xc - xa; // AC.x
  v_AC[1] = yc - ya; // AC.y

  // length of AX
  len_AX = ublas::inner_prod(v_AC, v_nAB); // (AC,nAB)
  // vector AX
  v_AX = v_nAB * len_AX; // AX = nAB*|AX|

  // vector CX
  v_CX = v_AX - v_AC;
  // length of CX
  len_CX = ublas::norm_2(v_CX);

  // debug log
  std::cout << "length of AB    : " << len_AB << std::endl;
  std::cout << "length of AX    : " << len_AX << std::endl;
  std::cout << "dist from C to X: " << len_CX << std::endl;
  std::cout << "check length CX : "
    << ((len_CX < r)?"true":"false") << std::endl;
  std::cout << "check length AX : "
    << ((len_AX < len_AB)?"true":"false") << std::endl;

  bool pointer_isOnLine = (len_CX < r) && (len_AX <= len_AB);

  return pointer_isOnLine;
}


#include <iostream>
#include <cmath>
#include <math.h>
#include <iostream>
#include <vector>
#include <array>
#include "heatEquation.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void HeatEquation::SetSpaceTimeMesh( SpaceMesh smesh, TimeMesh tmesh, const std::string outputFileName )
{
    mpsmesh = smesh;
    mptmesh = tmesh;
    mpcurrenTimeStep = 0;
}

void HeatEquation::Solve()
{
AnalyticSolutionVec();
mpPreviousSolution = mpAnalyticSolution;

int m = mptmesh.NumberOfTimeSteps();
for(int j = 0; j<m; j++)
{
mpcurrenTimeStep = j+1;
mpcurrentMeshIndex = j;
stiff.BuildStiffnessMatrix( mpsmesh );
stiff.MultiplyByScalar( mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
mass.BuildMassMatrix(mpsmesh);
LHS.AddTwoMatrices( mass, stiff );

mass.MatrixVectorMultiplier( mpPreviousSolution, mpRHS );

LHS.MatrixSolver( mpRHS, mpx );

mpPreviousSolution = mpx;

}
}

void HeatEquation::AnalyticSolutionVec( )
{
    mpAnalyticSolution.clear();
     for (int i = 0; i<mpsmesh.meshsize()-1; i++)
{

    mpAnalyticSolution.push_back(ContinuousAnalyticSolution( mpsmesh.ReadSpaceNode(i+1),
                                                                        mptmesh.ReadTimeStep(mpcurrenTimeStep)));
}
}

    //the if statements deal with what happens if you are in the first interval
    //in this case the first value of u is given by the boundary condition
    //all the other elements can be fixed for x = 0
    //this function only works for the static case if the mesh changes everything is fucked
double HeatEquation::PiecewiseU( double x, SpaceMesh currentsmesh, std::vector<double> U )
{
    std::array<double, 2> firstpoint;
    std::array<double, 2> secondpoint;

    int upperindex = currentsmesh.IndexAbove( x );

    auto boundaryconditionU0 = 0;
    auto boundarycondition1Un = 0;

    if((upperindex==1)||(upperindex==0))
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(0);
    firstpoint.at(1) = boundaryconditionU0;

    secondpoint[0] = currentsmesh.ReadSpaceNode(1);
    secondpoint.at(1) = U.at(0);
    }
    else if (upperindex == currentsmesh.meshsize())
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = U.at(upperindex-2);

    secondpoint[0] = currentsmesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = boundarycondition1Un;
    }
    else
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = U.at(upperindex-2);

    secondpoint[0] = currentsmesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = U.at(upperindex-1);
    }

    long double m = (firstpoint[1]-secondpoint[1])/(firstpoint[0]-secondpoint[0]);

    return m*(x - firstpoint[0])+firstpoint[1];
}

double HeatEquation::ContinuousAnalyticSolution( double x, double t )
{
     return 6*exp(-t)*sin(M_PI*x);
}

void HeatEquation::BuildErrorMesh()
{
mpErrorMesh.clear();

auto SquaredError = [&](double x)
    { return pow(PiecewiseU(x, mpsmesh, mpx)-ContinuousAnalyticSolution(x, mptmesh.ReadTimeStep(mpcurrenTimeStep)), 2); };

double Q;
for(int i=0; i<mpsmesh.meshsize(); i++)
{
Q = gauss<double, 7>::integrate(SquaredError, mpsmesh.ReadSpaceNode(i), mpsmesh.ReadSpaceNode(i+1));
mpErrorMesh.push_back( Q );
}

}

double HeatEquation::GlobalSpaceError()
{
    BuildErrorMesh();
    double globalError=0;
    for(auto k: mpErrorMesh)
        globalError = globalError + k;

    std::cout << sqrt(globalError);
    std::cout << " \n";
    return sqrt(globalError);
}

void HeatEquation::PrintErrorMesh()
{
    BuildErrorMesh();
    PrintVector(mpErrorMesh);
}

void HeatEquation::PrintSolution( )
{
        BuildErrorMesh();
        AnalyticSolutionVec();

        std::cout << "FEM Approximation:     ";
        PrintVector(mpx);
        std::cout << "Analytic Solution:     ";
        PrintVector(mpAnalyticSolution);
        std::cout << "Error Mesh:            ";
        PrintVector(mpErrorMesh);
        std::cout << "Global Error           ";
        GlobalSpaceError();
}

void HeatEquation::PrintVector( std::vector<double> aVector)
{
        for (auto k: aVector)
        std::cout << k << ", ";
        std::cout << " \n";
}

void HeatEquation::AddVectors(std::vector<double>func1, std::vector<double> func2, std::vector<double>& result)
{
    if(func1.size()==func2.size())
    {
    result.clear();
     for ( int i=0; i<func1.size(); i++ )
     {
         result.push_back(func1.at(i)+func2.at(i));
     }
    }
    else
    {
        std::cout<< " your vectors are different sizes";
        std::cout<<"\n";
    }
}

void HeatEquation::VectorTimesScalar( std::vector<double>& func1, double scalar)
{
         for ( int i=0; i<func1.size(); i++ )
     {
         func1.at(i)= scalar*func1.at(i);
     }
}

Simple Heat Equation solver class/heatEquation.cpp


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>, Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "rotation_matrix_from_directions.h"
#include <Eigen/Geometry>
#include <iostream>

template <typename Scalar>
IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions(
  const Eigen::Matrix<Scalar, 3, 1> v0,
  const Eigen::Matrix<Scalar, 3, 1> v1)
{
  Eigen::Matrix<Scalar, 3, 3> rotM;
  const double epsilon=1e-8;
  Scalar dot=v0.normalized().dot(v1.normalized());
  ///control if there is no rotation
  if ((v0-v1).norm()<epsilon)
  {
    rotM = Eigen::Matrix<Scalar, 3, 3>::Identity();
    return rotM;
  }
  if ((v0+v1).norm()<epsilon)
  {
    rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity();
    rotM(0,0) = 1.;
    std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl;
    return rotM;
  }
  ///find the axis of rotation
  Eigen::Matrix<Scalar, 3, 1> axis;
  axis=v0.cross(v1);
  axis.normalize();

  ///construct rotation matrix
  Scalar u=axis(0);
  Scalar v=axis(1);
  Scalar w=axis(2);
  Scalar phi=acos(dot);
  Scalar rcos = cos(phi);
  Scalar rsin = sin(phi);

  rotM(0,0) =      rcos + u*u*(1-rcos);
  rotM(1,0) =  w * rsin + v*u*(1-rcos);
  rotM(2,0) = -v * rsin + w*u*(1-rcos);
  rotM(0,1) = -w * rsin + u*v*(1-rcos);
  rotM(1,1) =      rcos + v*v*(1-rcos);
  rotM(2,1) =  u * rsin + w*v*(1-rcos);
  rotM(0,2) =  v * rsin + u*w*(1-rcos);
  rotM(1,2) = -u * rsin + v*w*(1-rcos);
  rotM(2,2) =      rcos + w*w*(1-rcos);

  return rotM;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template Eigen::Matrix<double, 3, 3, 0, 3, 3> igl::rotation_matrix_from_directions<double>(const Eigen::Matrix<double, 3, 1, 0, 3, 1>, const Eigen::Matrix<double, 3, 1, 0, 3, 1>);
#endif


#ifndef BURKARDT_ODE_HPP
#define BURKARDT_ODE_HPP

// ODE test problem set from
//   https://people.sc.fsu.edu/~burkardt/f_src/test_ode/test_ode.html
//   https://people.sc.fsu.edu/~burkardt/f_src/test_ode/test_ode.f90

#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

class P01 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P02 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - x(0)* x(0)*x(0) / 2.;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P03 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - cos(t) * x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P04 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * (20. - x(0)) / 80.;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P05 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = (x(0) - t) / (x(0) + t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 4.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P06 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = 2. * x(0) * (1. - x(1));
		y(1) = - x(1) * (1. - x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 1.;
		x(1) = 3.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P07 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = -x(0) + x(1);
		y(1) = x(0) - 2. * x(1) + x(2);
		y(2) = x(1) - x(2);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 2.;
		x(1) = 0.;
		x(2) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P08 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = -x(0);
		y(1) = x(0) - x(1) * x(1);
		y(2) = x(1) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 1.;
		x(1) = 0.;
		x(2) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P09 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		T norm;

		norm = sqrt(x(0)*x(0) + x(1)*x(1));

		y(0) = - x(1) - x(0) * x(2) / norm;
		y(1) = x(0) - x(1) * x(2) / norm;
		y(2) = x(0) / norm;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 3.;
		x(1) = 0.;
		x(2) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P10 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = x(1) * x(2);
		y(1) = - x(0) * x(2);
		y(2) = -0.51 * x(0) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 0.;
		x(1) = 1.;
		x(2) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P11 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - x(0);
		for (i=1; i<9; i++) {
			y(i) = x(i-1) - x(i);
		}
		y(9) = - x(8);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P12 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - x(0);
		for (i=1; i<9; i++) {
			y(i) = (double)i * x(i-1) - (double)(i+1) * x(i);
		}
		y(9) = - 9. * x(8);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P13 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - 2. * x(0) + x(1);
		for (i=1; i<9; i++) {
			y(i) = x(i-1) - 2. * x(i) + x(i+1);
		}
		y(9) = x(8) - 2. * x(9);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P14 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(51);
		int i;

		y(0) = - 2. * x(0) + x(1);
		for (i=1; i<50; i++) {
			y(i) = x(i-1) - 2. * x(i) + x(i+1);
		}
		y(50) = x(49) - 2. * x(50);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(51);
		x(0) = 1.;
		for (i=1; i<51; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P15 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(30);
		const double k2 = 2.95912208286;
		const double m[6] = {
			1.00000597682,
			0.954786104043e-03,
			0.285583733151e-03,
			0.437273164546e-04,
			0.517759138449e-04,
			0.277777777778e-05
		};
		int i, i3, j, l, ll, mm;
		T p;
		T q[5][5];
		T r[5];

		i = 0;
		for (l=3; l<=15; l+=3) {
			i++;
			p = x(l-3)*x(l-3) + x(l-2)*x(l-2) + x(l-1)*x(l-1);
			r[i-1] = 1. / (p * sqrt(p));
			j = 0;
			for (ll=3; ll<=15; ll+=3) {
				j++;
				if (ll != l) {
					p = (x(l-3) - x(ll-3)) * (x(l-3) - x(ll-3))
					+ (x(l-2) - x(ll-2)) * (x(l-2) - x(ll-2))
					+ (x(l-1) - x(ll-1)) * (x(l-1) - x(ll-1));
					q[i-1][j-1] = 1. / (p * sqrt(p));
					q[j-1][i-1] = q[i-1][j-1];
				}
			}
		}

		i3 = 0;
		for (i=1; i<=5; i++) {
			i3 += 3;
			for (ll = i3-2; ll <= i3; ll++) {
				mm = ll - i3;
				y(ll-1) = x(ll+14);
				p = 0.;
				for (j=1; j<=5; j++) {
					mm += 3;
					if (j != i) {
						p += m[j-1] * (x(mm-1) * (q[i-1][j-1] - r[j-1]) - x(ll-1) * q[i-1][j-1]);
					}
				}
				y(ll+14) = k2 * ( - (m[0] + m[i]) * x(ll-1) * r[i-1] + p);
			}
		}

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		x.resize(30);

		x(0) = 3.42947415189;
		x(1) = 3.35386959711;
		x(2) = 1.35494901715;
		x(3) = 6.64145542550;
		x(4) = 5.97156957878;
		x(5) = 2.18231499728;
		x(6) = 11.2630437207;
		x(7) = 14.6952576794;
		x(8) = 6.27960525067;
		x(9) = -30.1552268759;
		x(10) = 1.65699966404;
		x(11) = 1.43785752721;
		x(12) = -21.1238353380;
		x(13) = 28.4465098142;
		x(14) = 15.3882659679;
		x(15) = -0.557160570446;
		x(16) = 0.505696783289;
		x(17) = 0.230578543901;
		x(18) = -0.415570776342;
		x(19) = 0.365682722812;
		x(20) = 0.169143213293;
		x(21) = -0.325325669158;
		x(22) = 0.189706021964;
		x(23) = 0.0877265322780;
		x(24) = -0.0240476254170;
		x(25) = -0.287659532608;
		x(26) = -0.117219543175;
		x(27) = -0.176860753121;
		x(28) = -0.216393453025;
		x(29) = -0.0148647893090;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P16 {
	public:
	double delta;

	P16(double _delta = 0.1) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(4);
		T tmp;

		tmp = sqrt(x(0) * x(0) + x(1) * x(1));
		y(0) = x(2);
		y(1) = x(3);
		y(2) = - x(0) / (tmp * tmp * tmp);
		y(3) = - x(1) / (tmp * tmp * tmp);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		x.resize(4);
		x(0) = 1. - delta;
		x(1) = 0.;
		x(2) = 0.;
		x(3) = sqrt((1. + delta)/(1. - delta));
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// P17-P20はP16の初期値違い。
// P16, P17は解けるが、P18-P20は厳しい。

class P17 : public P16 {
	public:
	P17(double _delta = 0.3) {delta = _delta;}
};

class P18 : public P16 {
	public:
	P18(double _delta = 0.5) {delta = _delta;}
};

class P19 : public P16 {
	public:
	P19(double _delta = 0.7) {delta = _delta;}
};

class P20 : public P16 {
	public:
	P20(double _delta = 0.9) {delta = _delta;}
};

class P21 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = - ( 1. - 0.25 / ((t + 1.) * (t + 1.))) * x(0) - 1. / (t + 1.) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.6713967071418030;
		x(1) = 0.09540051444747446;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P22 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = (1. - x(0) * x(0)) * x(1) - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P23 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) * x(0) * x(0) / 6. - x(0) + 2. * sin(2.78535 * t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P24 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = 0.032 - 0.4 * x(1) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 30.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P25 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = sqrt(1. + x(1) * x(1)) / (25. - t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// Lotka-Volterra Predator-Prey Equations
class P31 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double a = 5.;
		const double b = 1.;
		const double c = 0.5;
		const double d = 2.;

		y(0) = (a - b * x(1)) * x(0);
		y(1) = (c * x(0) - d) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 10.;
	}
};

// The Lorenz System
class P32 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);
		const double beta = 8./3.;
		const double rho = 28.;
		const double sigma = 10.;

		y(0) = sigma * (x(1) - x(0));
		y(1) = rho * x(0) - x(1) - x(0) * x(2);
		y(2) = - beta * x(2) + x(0) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 2.;
		x(1) = 2.;
		x(2) = 21.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Van der Pol equation
class P33 {
	public:

	double delta;

	P33(double _delta = 1.) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = delta * (1. - x(0) * x(0)) * x(1) - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Linearized Damped Pendulum
class P34 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double g = 32.;
		const double k = 1.;
		const double l = 1.;
		const double m = 1.;

		y(0) = x(1);
		y(1) = - (g / l) * x(0) - (k / m) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Nonlinear Damped Pendulum
class P35 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double g = 32.;
		const double k = 1.;
		const double l = 1.;
		const double m = 1.;

		y(0) = x(1);
		y(1) = - (g / l) * sin(x(0)) - (k / m) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// Duffing's Equation
class P36 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) * (1. - x(0) * x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.5;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 100.;
	}
};

// Duffing's Equation with Damping and Forcing
class P37 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double a = 0.3;
		const double k = 0.2;
		const double w = 1.;

		y(0) = x(1);
		y(1) = x(0) * (1. - x(0) * x(0)) - k * x(1) + a * cos(w * t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.5;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 100.;
	}
};

// Shampine's Ball of Flame
class P38 {
	public:
	double delta;

	P38(double _delta = 0.01) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * x(0) * (1. - x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = delta;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 2./delta;
	}
};

// Polking's first order ODE
class P39 {
	public:

	double a;
	double b;

	P39(double _a = 1., double _b = 0.) : a(_a), b(_b) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * x(0) - a * t + b;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 0.5;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = (b + 9.) / a;
	}
};

// the Kee problem
class P40 {
	public:

	double eps;

	P40(double _eps = 0.01) : eps(_eps) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * (x(0) - t) / eps;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = -1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = -1.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 1.;
	}
};

#endif // BURKARDT_ODE_HPP


//
// Copyright (c) 2019-2020 CNRS INRIA
//

#include <boost/python.hpp>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"
#include "pinocchio/math/rpy.hpp"

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    Eigen::Matrix3d rpyToMatrix_proxy(const Eigen::Vector3d & rpy)
    {
      return pinocchio::rpy::rpyToMatrix(rpy);
    }

    Eigen::Vector3d matrixToRpy_proxy(const Eigen::Matrix3d & R)
    {
      return pinocchio::rpy::matrixToRpy(R);
    }

    Eigen::Matrix3d rotate(const std::string & axis, const double ang)
    {
      if(axis.length() != 1U)
          throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
      Eigen::Vector3d u;
      u.setZero();
      const char axis_ = axis[0];
      switch(axis_)
      {
        case 'x': u[0] = 1.; break;
        case 'y': u[1] = 1.; break;
        case 'z': u[2] = 1.; break;
        default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_));
      }

      return Eigen::AngleAxisd(ang, u).matrix();
    }

    void exposeRpy()
    {
      using namespace Eigen;
      using namespace pinocchio::rpy;

      {
        // using the rpy scope
        bp::scope current_scope = getOrCreatePythonNamespace("rpy");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const double, const double, const double)>(&rpyToMatrix),
                bp::args("roll", "pitch", "yaw"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("rpyToMatrix",
                &rpyToMatrix_proxy,
                bp::arg("rpy"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("matrixToRpy",
                &matrixToRpy_proxy,
                bp::arg("R"),
                "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a."
                " The angles are guaranteed to be in the ranges: r in [-pi,pi],"
                " p in[-pi/2,pi/2], y in [-pi,pi]");

        bp::def("rotate",
                &rotate,
                bp::args("axis", "ang"),
                "Rotation matrix corresponding to a rotation about x, y or z"
                " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");
      }

    }

  } // namespace python
} // namespace pinocchio


#pragma once
#include <cmath>
#include <autoppl/math/math.hpp>
#include <autoppl/util/traits/dist_expr_traits.hpp>
#include <fastad_bits/util/type_traits.hpp>
#include <Eigen/Dense>

// MSVC does not seem to support M_PI
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace ppl {
namespace math {

using dist_value_t = util::dist_value_t;

/////////////////////////////////
// Compile-time Constants
/////////////////////////////////

inline constexpr double SQRT_TWO_PI =
    2.506628274631000502415765284811045;
inline constexpr double LOG_SQRT_TWO_PI =
    0.918938533204672741780329736405617;

/////////////////////////////////
// Normal Density
/////////////////////////////////

// sss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const XType& x,
                               const MeanType& mean,
                               const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z = (x - mean) / sigma;
    return std::exp(-0.5 * z * z) / (sigma * SQRT_TWO_PI);
}

// vss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const MeanType& mean,
                               const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean).matrix().squaredNorm() / (sigma * sigma);
    return std::exp(-0.5 * z_sq) / std::pow(sigma * SQRT_TWO_PI, x.size());
}

// vvs
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const Eigen::MatrixBase<MeanType>& mean,
                               const SigmaType& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);
    assert(x.size() == mean.size());
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean.array()).matrix().squaredNorm() / (sigma * sigma);
    return std::exp(-0.5 * z_sq) / std::pow(sigma * SQRT_TWO_PI, x.size());
}

// vsv, vsm
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const MeanType& mean,
                               const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean)/sigma.array()).matrix().squaredNorm();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * sigma.array().prod());

    } else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * det);
    }
}

// vvv, vvm
template <class XType
        , class MeanType
        , class SigmaType>
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const Eigen::MatrixBase<MeanType>& mean,
                               const Eigen::MatrixBase<SigmaType>& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);

    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        assert(x.size() == mean.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean.array())/sigma.array()).matrix().squaredNorm();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        assert(x.size() == mean.size());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean.array()).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * det);
    }
}

// sss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const XType& x,
                                   const MeanType& mean,
                                   const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z = (x - mean) / sigma;
    return -0.5 * z * z - std::log(sigma) - LOG_SQRT_TWO_PI;
}

// vss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                           const MeanType& mean,
                           const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean).matrix().squaredNorm() / (sigma * sigma);
    return -0.5 * z_sq - (x.size() * (std::log(sigma) + LOG_SQRT_TWO_PI));
}

// vvs
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                           const Eigen::MatrixBase<MeanType>& mean,
                           const SigmaType& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);
    assert(x.size() == mean.size());
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean.array()).matrix().squaredNorm() / (sigma * sigma);
    return -0.5 * z_sq - (x.size() * (std::log(sigma) + LOG_SQRT_TWO_PI));
}

// vsv, vsm
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const MeanType& mean,
                                   const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean)/sigma.array()).matrix().squaredNorm();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(det);
    }
}

// vvv, vvm
template <class XType
        , class MeanType
        , class SigmaType>
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const Eigen::MatrixBase<MeanType>& mean,
                                   const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        assert(x.size() == mean.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean.array())/sigma.array()).matrix().squaredNorm();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        assert(x.size() == mean.size());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean.array()).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(det);
    }
}

/////////////////////////////////
// Cauchy Density
/////////////////////////////////

// sss
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<LocType> &&
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const XType& x,
                                   const LocType& loc,
                                   const ScaleType& scale)
{
    auto diff = x - loc;
    return (scale > 0) ? -std::log(scale + diff * diff / scale) : neg_inf<double>;
}

// vss
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<LocType> &&
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const LocType& loc,
                                   const ScaleType& scale)
{
    bool cond = scale > 0.;
    auto diff = x.array() - loc;
    return cond ? -(scale + (1./scale) * diff * diff).log().sum() :
                  neg_inf<double>;
}

// vvs
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<LocType>& loc,
                                    const ScaleType& scale)
{
    bool cond = scale > 0.;
    auto diff = x.array() - loc.array();
    return cond ? -(scale + (1./scale) * diff * diff).log().sum() : neg_inf<double>;
}

// vsv
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<LocType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const LocType& loc,
                                    const Eigen::MatrixBase<ScaleType>& scale)
{
    bool cond = (scale.array() > 0.).all();
    auto diff = x.array() - loc;
    auto gamma = scale.array();
    return cond ? -(gamma + (1./gamma) * diff * diff).log().sum() : neg_inf<double>;
}

// vvv
template <class XType
        , class LocType
        , class ScaleType>
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<LocType>& loc,
                                    const Eigen::MatrixBase<ScaleType>& scale)
{
    bool cond = (scale.array() > 0.).all();
    auto diff = x.array() - loc.array();
    auto gamma = scale.array();
    return cond ? -(gamma + (1./gamma) * diff * diff).log().sum() : neg_inf<double>;
}

/////////////////////////////////
// Uniform Density
/////////////////////////////////

// sss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const XType& x,
                                const MinType& min,
                                const MaxType& max)
{
    return (min < x && x < max) ? 1. / (max - min) : 0;
}

// vss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const MinType& min,
                                const MaxType& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max).all();
    return cond ? std::pow(1./(max-min), x.size()) : 0;
}

// vvs
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const Eigen::MatrixBase<MinType>& min,
                                const MaxType& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max).all();
    return cond ? (1./(max-min.array())).prod() : 0;
}

// vsv
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const MinType& min,
                                const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max.array()).all();
    return cond ? (1./(max.array()-min)).prod() : 0;
}

// vvv
template <class XType
        , class MinType
        , class MaxType>
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const Eigen::MatrixBase<MinType>& min,
                                const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max.array()).all();
    return cond ? (1./(max.array()-min.array())).prod() : 0;
}

// sss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const XType& x,
                                    const MinType& min,
                                    const MaxType& max)
{
    return (min < x && x < max) ? -std::log(max - min) : neg_inf<double>;
}

// vss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const MinType& min,
                                    const MaxType& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max).all();
    return cond ?
        static_cast<dist_value_t>(x.size())*(-std::log(max-min)) :
        neg_inf<double>;
}

// vvs
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<MinType>& min,
                                    const MaxType& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max).all();
    return cond ? -(max-min.array()).log().sum() : neg_inf<double>;
}

// vsv
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const MinType& min,
                                    const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max.array()).all();
    return cond ? -(max.array()-min).log().sum() : neg_inf<double>;
}

// vvv
template <class XType
        , class MinType
        , class MaxType>
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<MinType>& min,
                                    const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max.array()).all();
    return cond ? -(max.array()-min.array()).log().sum() : neg_inf<double>;
}

/////////////////////////////////
// Bernoulli Density
/////////////////////////////////

/**
 * Bernoulli pdf and log pdf (pmf actually).
 * It is defined to clip when p is out of the range [0,1],
 * i.e. if p < 0, then we take p = 0 and
 * if p > 1, then we take p = 1.
 */

// ss
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_pdf(const XType& x,
                                  const PType& p)
{
    if (p <= 0) return (x == 0) + 0.;
    else if (p >= 1) return (x == 1) + 0.;

    if (x == 1) return p;
    else if (x == 0) return 1. - p;
    else return 0.0;
}

// vs
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_pdf(const Eigen::MatrixBase<XType>& x,
                                  const PType& p)
{
    double pdf = 1.;
    for (int i = 0; i < x.size(); ++i) {
        pdf *= bernoulli_pdf(x(i), p);
    }
    return pdf;
}

// vv
template <class XType
        , class PType>
inline dist_value_t bernoulli_pdf(const Eigen::MatrixBase<XType>& x,
                                  const Eigen::MatrixBase<PType>& p)
{
    assert(x.size() == p.size());
    double pdf = 1.;
    for (int i = 0; i < x.size(); ++i) {
        pdf *= bernoulli_pdf(x(i), p(i));
    }
    return pdf;
}

// ss
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_log_pdf(const XType& x,
                                      const PType& p)
{
    if (p <= 0) {
        if (x == 0) return 0.;
        else return neg_inf<PType>;
    }
    else if (p >= 1) {
        if (x == 1) return 0.;
        else return neg_inf<PType>;
    }

    if (x == 1) return std::log(p);
    else if (x == 0) return std::log(1. - p);
    else return neg_inf<PType>;
}

// vs
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_log_pdf(const Eigen::MatrixBase<XType>& x,
                                      const PType& p)
{
    double logpdf = 0.;
    for (int i = 0; i < x.size(); ++i) {
        logpdf += bernoulli_log_pdf(x(i), p);
    }
    return logpdf;
}

// vv
template <class XType
        , class PType>
inline dist_value_t bernoulli_log_pdf(const Eigen::MatrixBase<XType>& x,
                                      const Eigen::MatrixBase<PType>& p)
{
    assert(x.size() == p.size());
    double logpdf = 0.;
    for (int i = 0; i < x.size(); ++i) {
        logpdf += bernoulli_log_pdf(x(i), p(i));
    }
    return logpdf;
}

/////////////////////////////////
// Wishart Density
// Note: drops unnecessary constant terms
/////////////////////////////////
template <class XType
        , class VType
        , class NType>
inline dist_value_t wishart_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<VType>& v,
                                    const NType& n)
{
    Eigen::LLT<Eigen::MatrixXd> x_llt(x);
    Eigen::LLT<Eigen::MatrixXd> v_llt(v);
    auto log_det_x = std::log(x_llt.matrixL().determinant());
    auto log_det_v = std::log(v_llt.matrixL().determinant());
    auto tr = v_llt.solve(x).trace();
    dist_value_t p = v.rows();
    return (n - p - 1.) * log_det_x - 0.5 * tr - n * log_det_v;
}

} // namespace math
} // namespace ppl


// Copyright Paul Bristow 2012.
// Copyright John Maddock 2010.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

/*! \brief Examples of using the enhanced math constants.
    \details This allows for access to constants via functions like @c pi(),
    and also via namespaces, @c using @c namespace boost::math::double_constants;
    called simply @c pi.
*/

#include <boost/math/constants/constants.hpp>

#include <iostream>
using std::cout;
using std::endl;

#include <limits>
using std::numeric_limits;

/*! \brief Examples of a template function using constants.
    \details This example shows using of constants from function calls like @c pi(),
    rather than the 'cute' plain @c pi use in non-template applications.

    \tparam Real radius parameter that can be a built-in like float, double,
      or a user-defined type like multiprecision.
    \returns Area = pi * radius ^ 2
*/

//[math_constants_eg1
template<class Real>
Real area(Real r)
{
  using namespace boost::math::constants;

  return pi<Real>() * r * r;
}
//] [/math_constants_eg1]

int main()
{

  { // Boost.Math constants using function calls like pi().
    // using namespace boost::math::constants;

   using boost::math::constants::pi;
   using boost::math::constants::one_div_two_pi;

  cout.precision(numeric_limits<double>::max_digits10);
  cout << "double pi =  boost::math::double_constants::pi = " << pi<double>() << endl;
  //   double pi =  boost::math::double_constants::pi = 3.1415926535897931
  double r = 1.234567890123456789;
  double d = pi<double>() * r * r;

  cout << "d = " << d << ", r = " << r << endl;

  float rf = 0.987654321987654321f;

  float pif = boost::math::constants::pi<float>();
  cout << "pidf =  boost::math::constants::pi() = " << pif << endl;
  //   pidf =  boost::math::float_constants::pi = 3.1415927410125732

    //float df = pi * rf * rf; // conversion from 'const double' to 'float', possible loss of data
  float df = pif * rf * rf;

  cout << "df = " << df << ", rf = " << rf << endl;

  cout << "one_div_two_pi " << one_div_two_pi<double>() << endl;

  using boost::math::constants::one_div_two_pi;

  cout << "one_div_root_two_pi " << one_div_two_pi<double>() << endl;
  }

  { // Boost math new constants using namespace selected values, like pi.

    //using namespace boost::math::float_constants;
    using namespace boost::math::double_constants;

    double my2pi = two_pi; // Uses boost::math::double_constants::two_pi;

    cout << "double my2pi = " << my2pi << endl;

    using boost::math::float_constants::e;
    float my_e = e;
    cout << "float my_e  " << my_e << endl;

    double my_pi = boost::math::double_constants::pi;
    cout << "double my_pi = boost::math::double_constants::pi =  " << my_pi << endl;

    // If you try to use two namespaces, this may, of course, create ambiguity:
    // it is not too difficult to do this inadvertently.
    using namespace boost::math::float_constants;
    //cout << pi << endl; // error C2872: 'pi' : ambiguous symbol

  }
  {

//[math_constants_ambiguity
     // If you use more than one namespace, this will, of course, create ambiguity:
     using namespace boost::math::double_constants;
     using namespace boost::math::constants;

      //double my_pi = pi(); // error C2872: 'pi' : ambiguous symbol
      //double my_pi2 = pi; // Context does not allow for disambiguation of overloaded function

     // It is also possible to create ambiguity inadvertently,
     // perhaps in other peoples code,
     // by making the scope of a namespace declaration wider than necessary,
     // therefore is it prudent to avoid this risk by localising the scope of such definitions.
//] [/math_constants_ambiguity]

  }

  { // You can, of course, use both methods of access if both are fully qualified, for examples:

    //cout.precision(std::numeric_limits<double>::max_digits10);// Ideally.
    cout.precision(2 + std::numeric_limits<double>::digits * 3010/10000); // If no max_digits10.

    double my_pi1 = boost::math::constants::pi<double>();
    double my_pid = boost::math::double_constants::pi;
    cout << "boost::math::constants::pi<double>() = " << my_pi1 << endl
         << "boost::math::double_constants::pi = " << my_pid << endl;

    // cout.precision(std::numeric_limits<float>::max_digits10); // Ideally.
    cout.precision(2 + std::numeric_limits<double>::digits * 3010/10000); // If no max_digits10.
    float my_pif = boost::math::float_constants::pi;
    cout << "boost::math::float_constants::pi = " << my_pif << endl;

  }

  { // Use with templates

    // \warning it is important to be very careful with the type provided as parameter.
    // For example, naively providing an @b integer instead of a floating-point type can be disastrous.
    // cout << "Area = " << area(2) << endl; // warning : 'return' : conversion from 'double' to 'int', possible loss of data
    // Failure to heed this warning can lead to very wrong answers!
    //  Area = 12 !!  = 3 * 2 * 2
//[math_constants_template_integer_type
    //cout << "Area = " << area(2) << endl; //   Area = 12!
    cout << "Area = " << area(2.) << endl;  //   Area = 12.566371

    // You can also avoid this by being explicit about the type of @c area.
    cout << "Area = " << area<double>(2) << endl;

//] [/math_constants_template_integer_type]

  }
  {
    using  boost::math::constants::pi;
    double my_pi3 = pi<double>();
    //double my_pi4 = pi<>(); cannot find template type.
    //double my_pi4 = pi();

  }

} // int main()

/*[constants_eq1_output

Output:

  double pi =  boost::math::double_constants::pi = 3.1415926535897931
  d = 4.7882831840285398, r = 1.2345678901234567
  pidf =  boost::math::constants::pi() = 3.1415927410125732
  df = 3.0645015239715576, rf = 0.98765432834625244
  one_div_two_pi 0.15915494309189535
  one_div_root_two_pi 0.15915494309189535
  double my2pi = 6.2831853071795862
  float my_e  2.7182817459106445
  double my_pi = boost::math::double_constants::pi =  3.1415926535897931
  boost::math::constants::pi<double>() = 3.1415926535897931
  boost::math::double_constants::pi = 3.1415926535897931
  boost::math::float_constants::pi = 3.1415927410125732
  Area = 12.566370614359172
  Area = 12.566370614359172

] [/constants_eq1_output]
*/


/**
 * @file propagator.h
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 04.05.2020
 * @copyright Developed at ETH Zurich
 */

#include "propagator.h"

#include <cmath>
#include <complex>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

namespace NonLinSchroedingerEquation {

// KineticPropagator
/* SAM_LISTING_BEGIN_1 */
KineticPropagator::KineticPropagator(const SparseMatrixXd &A,
                                     const SparseMatrixXcd &M, double tau) {
  // Defeats the rationale of expression templates, but acceptable here, because
  // executed only once in the constructor.
  B_plus_ = M + 0.5 * tau * A.cast<std::complex<double>>();
  SparseMatrixXcd B_minus = M - 0.5 * tau * A.cast<std::complex<double>>();
  // This is the expensive step: LU-factorization of a big sparse matrix.
  // Precomputation is essential
  solver_.compute(B_minus);
}

Eigen::VectorXcd KineticPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  // Cheap elimination steps operating on the LU-factors. Effort is almost O(N)
  // thanks to sophisticated fill-in avoiding techniques employed by the sparse
  // solvers.
  return solver_.solve(B_plus_ * mu);
}
/* SAM_LISTING_END_1 */

// InteractionPropagator
/* SAM_LISTING_BEGIN_2 */
InteractionPropagator::InteractionPropagator(double tau) {
  // We know that the discrete evolution operator for the non-linear part of the
  // method-of-lines ODE boils down to componentwise multiplication of the
  // vector with a single phase shift. Thus, this phase shift can be
  // precomputed, here by means of a lambda function
  phase_multiplier_ = [tau](std::complex<double> z) {
    const std::complex<double> i(0, 1);
    return std::exp(-i * tau * std::norm(z)) * z;
  };
}

Eigen::VectorXcd InteractionPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  // Eigen's way of applying a function to all components of a vector.
  return mu.unaryExpr(phase_multiplier_);
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
SplitStepPropagator::SplitStepPropagator(const SparseMatrixXd &A,
                                         const SparseMatrixXcd &M, double tau)
    : kineticPropagator_(A, M, 0.5 * tau), interactionPropagator_(tau) {}

Eigen::VectorXcd SplitStepPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  Eigen::VectorXcd nu(mu.size());
  nu = kineticPropagator_(mu);
  nu = interactionPropagator_(nu);
  nu = kineticPropagator_(nu);
  return nu;
}
/* SAM_LISTING_END_3 */

}  // namespace NonLinSchroedingerEquation


#include "problemes.h"
#include "arithmetique.h"

#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::pair<long double, long double> coordonnee;

namespace {
    long double norme(const coordonnee &v) {
        return std::hypot(v.first, v.second);
    }

    std::vector<coordonnee> convertion(const std::vector<coordonnee> &v) {
        if (v.empty())
            return v;

        std::vector<coordonnee> resultat{
                std::make_pair(v.front().first - v.back().first, v.front().second - v.back().second)};
        for (size_t k = 1; k < v.size(); ++k) {
            auto p = std::make_pair(v.at(k).first - v.at(k - 1).first, v.at(k).second - v.at(k - 1).second);
            auto n = norme(p);
            p.first /= n;
            p.second /= n;
            resultat.push_back(p);
        }

        return resultat;
    }

    std::vector<coordonnee> S(size_t n) {
        std::vector<coordonnee> s;
        for (size_t k = 0; k < n; ++k) {
            const long double x = (2.0L * k - 1L) / n + M_PIl;
            s.emplace_back(std::cos(x), std::sin(x));
        }

        return s;
    }

    long double produit_scalaire(const coordonnee &v1, const coordonnee &v2) {
        return (v1.first * v2.first + v1.second * v2.second) / (norme(v1) * norme(v2));
    }
}

ENREGISTRER_PROBLEME(228, "Minkowski Sums") {
    // Let Sn be the regular n-sided polygon – or shape – whose vertices vk (k = 1,2,…,n) have coordinates:
    //
    // 			x_k = cos( 2*(k-1)/n × 180° )
    // 			y_k = sin( 2*(k-1)/n × 180° )
    //
    // Each Sn is to be interpreted as a filled shape consisting of all points on the perimeter and in the interior.
    //
    // The Minkowski sum, S+T, of two shapes S and T is the result of adding every point in S to every point in T, where
    // point addition is performed coordinate-wise: (u, v) + (x, y) = (u+x, v+y).
    //
    // For example, the sum of S3 and S4 is the six-sided shape shown in pink below:
    //
    //							picture showing S_3 + S_4
    //
    // How many sides does S1864 + S1865 + … + S1909 have?
    std::set<coordonnee, std::function<bool(const coordonnee &v1, const coordonnee &v2)>> minkowski(
            [](const coordonnee &v1, const coordonnee &v2) {
                if (std::abs(1.0L - produit_scalaire(v1, v2)) < 0.0000000000001L)
                    return false;

                return v1 < v2;
            });

    for (size_t n = 1864; n <= 1909; ++n) {
        auto Sn = convertion(S(n));
        minkowski.insert(Sn.begin(), Sn.end());
    }

    nombre resultat = minkowski.size();
    return std::to_string(resultat);
}


#ifndef NEURALNET_HPP
#define NEURALNET_HPP

#include <eigen3/Eigen/Eigen>
#include <iostream>
#include <vector>
#include <string>
#include <boost/range/combine.hpp>
#include <fstream>

class LayerDense {
    public:
        int n_inputs; // number of inputs
        int n_neurons; // number of neurons

        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd weights;
        Eigen::MatrixXd weight_momentums;
        Eigen::VectorXd biases;
        Eigen::VectorXd bias_momentums;
        Eigen::MatrixXd output;

        Eigen::MatrixXd dweights; // derivative wrt weights
        Eigen::VectorXd dbiases; // derivative wrt biases
        Eigen::MatrixXd dinputs; // derivative wrt inputs

        // constructor
        LayerDense(int n_inputs, int n_neurons) {
            this->weights = Eigen::MatrixXd::Random(n_inputs,n_neurons) * 0.01; // initialize weights
            this->weights = weights;
            this->biases = Eigen::VectorXd::Zero(n_neurons); // initialize biases
            this->n_inputs = n_inputs;
            this->n_neurons = n_neurons;
            this->weight_momentums = Eigen::MatrixXd::Zero(n_inputs,n_neurons); // weight momentum
            this->bias_momentums = Eigen::VectorXd::Zero(n_neurons); // bias momentum
        }

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class ActivationRelu {
    public:
        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd dinputs; // derivative wrt inputs
        Eigen::MatrixXd output;

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class ActivationSoftmax {
    public:
        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd dinputs; // derivative wrt inputs
        Eigen::MatrixXd output;

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class CrossEntropyLoss {
    public:
        Eigen::MatrixXd dinputs;

        // Member functions declaration
        Eigen::VectorXd forward(const Eigen::MatrixXd &y_pred, const Eigen::VectorXi &y_true);
        double calculate(const Eigen::MatrixXd &output, const Eigen::VectorXi &y);
        void backward(const Eigen::MatrixXd &dvalues, const Eigen::VectorXi &y_true);
};

class StochasticGradientDescent {
    public:
        double learning_rate; // learning rate
        double decay; // decay
        double momentum; // momentum
        double iterations; // initialize number of iterations to 0

        // constructor
        StochasticGradientDescent(double learning_rate, double decay, double momentum) {
            this->learning_rate = learning_rate;
            this->decay = decay;
            this->momentum = momentum;
            this->iterations = 0.0;
        }

        // Member functions declaration
        void pre_update_params(double start_learning_rate);
        void update_params(LayerDense &layer);
        void post_update_params();
};

Eigen::MatrixXd load_matrix_data(std::string fileToOpen);
Eigen::VectorXi load_vector_data(std::string fileToOpen);

#endif // NEURALNET_HPP


/***************************************************************************

The MIT License (MIT)

Copyright (c) 2013-2017 Joel E. Merritt

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

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

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

***************************************************************************/

//  This module defines the GNSolver (Gauss-Newton Solver) class whose
//  objects solve the camera resection (calibration) described in
//  main.cpp.

#include "g_n_solver.h"

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

//
//  Member Function: GNSolver::GetErr2
//
//  Return the length squared of input vector.
//
//  Input parameters:
//  a -- vector to calculate length squared
//

double GNSolver::GetErr2(Eigen::VectorXd const &a) const
{
  double ret = 0.0;
  for(int i = 0;i < a.size();i++)
  {
    ret += a[i] * a[i];
  }

  return ret;
}

//
//  Member Function: GNSolver::EvalRFunctionVector
//
//  Return the vector containing the outputs of the r functions.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::EvalRFunctionVector(FunctionObjectList const &r,
                                              Eigen::VectorXd const &beta)
                                              const
{
  int num_conds = r.size();
  Eigen::VectorXd ret(num_conds);

  for(int i = 0;i < num_conds;i++)
  {
    ret[i] = r[i](beta);
  }

  return ret;
}

//
//  Member Function: GNSolver::GetPartialD
//
//  Return the partial derivative (partial d)r[i] / (partial d)beta[j].
//
//  Input parameters:
//  i    -- row index
//  j    -- column index
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

double GNSolver::GetPartialD(int i, int j, FunctionObjectList const &r,
                             Eigen::VectorXd const &beta) const
{
  double epsilon = 0.001;

  Eigen::VectorXd beta_epsilon = beta;
  beta_epsilon[j] += epsilon;

  double ret = r[i](beta);
  double ret_epsilon = r[i](beta_epsilon);

  double partd = (ret - ret_epsilon) / epsilon;

  return partd;
}

//
//  Member Function: GNSolver::Jacobian
//
//  Return the Jacobian matrix of vector function r.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::MatrixXd GNSolver::Jacobian(FunctionObjectList const &r,
                                   Eigen::VectorXd const &beta) const
{
  int num_conds = r.size();
  Eigen::MatrixXd ret(num_conds, beta.size());

  for(int i = 0;i < num_conds;i++)
  {
    for(int j = 0;j < beta.size();j++)
    {
      ret(i, j) = GetPartialD(i, j, r, beta);
    }
  }

  return ret;
}

//
//  Member Function: GNSolver::CalcErr2
//
//  Return the error of vector function r.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

double GNSolver::CalcErr2(FunctionObjectList const &r,
                          Eigen::VectorXd const &beta) const
{
  Eigen::VectorXd vcol = EvalRFunctionVector(r, beta);

  return GetErr2(vcol);
}

//
//  Member Function: GNSolver::CalcDiff
//
//  Return the calculated difference to adjust the beta vector.
//
//  The Gauss-Newton algorithm calculates successive approximations of
//  the beta vector by the following equation:
//
//  beta_next = beta_current - (J^T*J)^-1*J^T * r(beta_current)
//
//  where:
//  beta_next is the next iteration of the beta vector
//  beta_current is the current iteration of the beta vector
//  J is the Jacobian matrix
//  J^T is the transpose of the Jacobian matrix
//  r is the r vector of functions to be minimized
//
//  CalcDiff calculates and returns this part:
//
//  (J^T*J)^-1*J^T * r(beta_current)
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::CalcDiff(FunctionObjectList const &r,
                                   Eigen::VectorXd const &beta) const
{
  Eigen::MatrixXd ejf = Jacobian(r, beta);

  Eigen::MatrixXd ejft = ejf.transpose();
  Eigen::MatrixXd ejf2 = ejft * ejf;
  Eigen::MatrixXd ejf3 = ejf2.inverse() * ejft;

  Eigen::MatrixXd vcol = EvalRFunctionVector(r, beta);

  return ejf3 * vcol;
}

//
//  Member Function: GNSolver::operator()
//
//  Return the optimized beta vector.
//
//  The main solver, iterates to improve the beta vector, including
//  "dialing back" the next iteration of the beta vector until it
//  produces less error than the current beta vector.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::operator()(FunctionObjectList const &r,
                                     Eigen::VectorXd &beta) const
{
  std::cout << "Entering Solve" << std::endl;

  double err2_min;
  // iterate loop
  for(int j = 0;j < 100;j++)
  {
    std::cout << "iteration " << j << std::endl;

    std::cout << "BETA" << std::endl;
    std::cout << beta << std::endl;
    std::cout << std::endl;

    double err2 = CalcErr2(r, beta);
    Eigen::VectorXd ediff = CalcDiff(r, beta);

    std::cout << "err2 (orig) " << err2 << std::endl;

    double atten = 1.0;
    err2_min = err2;
    Eigen::VectorXd beta_min = beta;
    for(int j1 = 0;j1 < 20;j1++)
    {
      Eigen::VectorXd beta_next = atten * ediff + beta;

      double err2_next = CalcErr2(r, beta_next);
      std::cout << "j1 " << j1 << " atten " << atten << " err2_next " <<
          err2_next << std::endl;
      if(err2_next < err2_min)
      {
        err2_min = err2_next;
        beta_min = beta_next;
        atten *= 0.7;
      }
      else
      {
        atten *= 0.7;
      }
    }

    beta = beta_min;

    // stop iterating if the error is almost zero
    if(err2_min < 0.0000001)
    {
      std::cout << "solved" << std::endl;

      break;
    }

    // stop iterating if the error has decreased minimally
    if(err2_min / err2 > 0.99999)
    {
      std::cout << "converged" << std::endl;

      break;
    }
  } // iterate

  std::cout << "**********OUTPUT************" << std::endl;
  std::cout << "BETA" << std::endl;
  std::cout << beta << std::endl;
  std::cout << std::endl;
  std::cout << "err2_min " << err2_min << std::endl;

  return beta;
}


#include <iostream>
#include <boost/numeric/odeint.hpp>
#include "gnuplot-iostream.h"
#include "sysfunc_exercise2_2_b.h"

namespace odeint = boost::numeric::odeint;

int main(int argc, char const *argv[])
{
    // Odeint
    std::vector<double> position;
    {
        system_function::state_type x; // a vector with size 3. [x; x_dot; x_dot_dot]
        //x = {1,1,1, 1};                    // start at x=1, 1, 1, 1;
        x = {1, -1, 1, -1};
        size_t steps = odeint::integrate(system_function::harmonic_oscillator,
                                         x, 0.0, 10.0, 0.1,
                                         system_function::push_back_state_and_time);

        for (size_t i = 0; i <= steps; i++)
        {
            position.push_back(system_function::m_states[i][0]);
        }
    }

    // GNU Plot
    {
        /* output */
        Gnuplot gp;
        gp << "plot '-' using 1:2 with linespoint" << std::endl;
        gp.send1d(std::make_tuple(system_function::m_times, position));
    }
    return 0;
}


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

template<class T>
class func {
public:
  typedef T real_t;
  func(real_t a) : a_(a) {}
  real_t operator()(real_t x) const {
    using std::abs; using std::cos; using std::log;
    auto s = abs(a_ * a_ - 2 * a_ * cos(x) + 1);
    return (s > 0) ? log(s) : real_t(0);
  }
  real_t result() const {
    using std::abs; using std::log;
    using boost::math::constants::pi;
    return (abs(a_) < 1) ? real_t(0) : (2 * pi<real_t>() * log(abs(a_)));
  }
private:
  real_t a_;
};

int main() {
  using std::abs; using std::sqrt;
  typedef boost::multiprecision::cpp_dec_float_50 real_t;
  const real_t pi = boost::math::constants::pi<real_t>();

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());
  real_t error, L1;
  size_t levels;

  real_t values[5] = { 0.5, 1 - 1e-6, 1, 1 + 1e-6, 1.5 };
  for (auto a : values) {
    func<real_t> f(a);
    real_t q = integrator.integrate(f, 0, pi, termination, &error, &L1, &levels);

    std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
              << "value of a: " << a << std::endl
              << "result: " << q << std::endl
              << "estimated error: " << error << std::endl
              << "real error: " << abs(q - f.result()) << std::endl
              << "L1 * error: " << L1 * error << std::endl
              << "levels: " << levels << std::endl;
  }
}