#include "custom_libraries/convert_to_NED.hpp"
#include <vector>
#include <cmath>
//#include <tf2_ros/transform_listener.h>
//#include <tf2_ros/transform_broadcaster.h>
//#include <geometry_msgs/TransformStamped.h>
//#include <Eigen/Dense>

//tf2_ros::Buffer tfBuffer;
//tf2_ros::TransformListener tfListener(tfBuffer);

/*
using namespace Eigen;
//Roll pitch and yaw in Radians
Quaternionf euler2quaternions(double roll, double pitch, double yaw){
  Quaternionf q;
  q = AngleAxisf(roll, Vector3f::UnitX())
      * AngleAxisf(pitch, Vector3f::UnitY())
      * AngleAxisf(yaw, Vector3f::UnitZ());
  return q;
}*/
/*
geometry_msgs::TransformStamped broadcast_tf(std:string parent, std:string child, std::vector<double> trans, double[] rot){
  geometry_msgs::TransformStamped t;
  t.header.frame_id = parent;
  t.child_frame_id = child;
  t.transform.translation.x = x;
  t.transform.translation.y = y;
  t.transform.translation.z = z;
  tf2::Quaternion q;
  q.setRPY(rot[0], rot[1], rot[2]);
  q.normalize();
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();

  return t;
}*/

std::vector<double> lla2nedPiren(double lat_deg, double long_deg, double altitude){
    double lat0_deg  = 63.4389029083;
    double long0_deg = 10.39908278;
    double altitude0 = 39.923;

    double deg2rad = M_PI/180;
    double rad2deg = 180/M_PI;

    double lat_rad   = lat_deg * deg2rad;
    double long_rad  = long_deg * deg2rad;
    double lat0_rad  = lat0_deg * deg2rad;
    double long0_rad = long0_deg * deg2rad;

    //From vik 2012 - page 4 - table 1.1
    double re        = 6378137.0;  // [m]
    double rp        = 6356752.0;  // [m]
    double rad_curve = pow(re,2) / (sqrt(pow(re,2) * pow(cos(lat_rad),2) + pow(rp,2) * pow(sin(lat_rad),2)));  //N in vik

    // LLA to ECEF
    double xe = (rad_curve + altitude) * cos(lat_rad) * cos(long_rad);
    double ye = (rad_curve + altitude) * cos(lat_rad) * sin(long_rad);
    double ze = ((pow(rp, 2) / pow(re, 2)) * rad_curve + altitude) * sin(lat_rad);

    double xe0 = (rad_curve + altitude0) * cos(lat0_rad) * cos(long0_rad);
    double ye0 = (rad_curve + altitude0) * cos(lat0_rad) * sin(long0_rad);
    double ze0 = ((pow(rp, 2) / pow(re, 2)) * rad_curve + altitude0) * sin(lat0_rad);

    // ECEF to NED
    double cosPhi    = cos(lat0_rad);
    double sinPhi    = sin(lat0_rad);
    double cosLambda = cos(long0_rad);
    double sinLambda = sin(long0_rad);

    double dist   = cosLambda * xe + sinLambda * ye;
    double uNorth = -sinPhi * dist + cosPhi * ze;
    double vEast  = -sinLambda * xe + cosLambda * ye;
    double wDown  = -(cosPhi * dist + sinPhi * ze);

    // ECEF0 to NED0
    double cosPhi0    = cosPhi;
    double sinPhi0    = sinPhi;
    double cosLambda0 = cosLambda;
    double sinLambda0 = sinLambda;

    double dist0   = cosLambda0 * xe0 + sinLambda0 * ye0;
    double vEast0  = -sinLambda0 * xe0 + cosLambda0 * ye0;
    double wDown0  = -(cosPhi0 * dist0 + sinPhi0 * ze0);
    double uNorth0 = -sinPhi0 * dist0 + cosPhi0 * ze0;

    // Compute difference
    double N = uNorth - uNorth0;  // [m]
    double E = vEast - vEast0;   // [m]
    double D = wDown - wDown0;   // [m]

    std::vector<double> NED;
    NED.push_back(N);
    NED.push_back(E);
    NED.push_back(D);

    return NED;

}

std::vector<double> lla2ned(double lat_deg, double long_deg, double altitude, double lat0_deg, double long0_deg, double altitude0){
    double deg2rad = M_PI/180;
    double rad2deg = 180/M_PI;

    double lat_rad   = lat_deg * deg2rad;
    double long_rad  = long_deg * deg2rad;
    double lat0_rad  = lat0_deg * deg2rad;
    double long0_rad = long0_deg * deg2rad;

    //From vik 2012 - page 4 - table 1.1
    double re        = 6378137.0;  // [m]
    double rp        = 6356752.0;  // [m]
    double rad_curve = pow(re,2) / (sqrt(pow(re,2) * pow(cos(lat_rad),2) + pow(rp,2) * pow(sin(lat_rad),2)));  //N in vik

    // LLA to ECEF
    double xe = (rad_curve + altitude) * cos(lat_rad) * cos(long_rad);
    double ye = (rad_curve + altitude) * cos(lat_rad) * sin(long_rad);
    double ze = ((pow(rp, 2) / pow(re, 2)) * rad_curve + altitude) * sin(lat_rad);

    double xe0 = (rad_curve + altitude0) * cos(lat0_rad) * cos(long0_rad);
    double ye0 = (rad_curve + altitude0) * cos(lat0_rad) * sin(long0_rad);
    double ze0 = ((pow(rp, 2) / pow(re, 2)) * rad_curve + altitude0) * sin(lat0_rad);

    // ECEF to NED
    double cosPhi    = cos(lat0_rad);
    double sinPhi    = sin(lat0_rad);
    double cosLambda = cos(long0_rad);
    double sinLambda = sin(long0_rad);

    double dist   = cosLambda * xe + sinLambda * ye;
    double uNorth = -sinPhi * dist + cosPhi * ze;
    double vEast  = -sinLambda * xe + cosLambda * ye;
    double wDown  = -(cosPhi * dist + sinPhi * ze);

    // ECEF0 to NED0
    double cosPhi0    = cosPhi;
    double sinPhi0    = sinPhi;
    double cosLambda0 = cosLambda;
    double sinLambda0 = sinLambda;

    double dist0   = cosLambda0 * xe0 + sinLambda0 * ye0;
    double vEast0  = -sinLambda0 * xe0 + cosLambda0 * ye0;
    double wDown0  = -(cosPhi0 * dist0 + sinPhi0 * ze0);
    double uNorth0 = -sinPhi0 * dist0 + cosPhi0 * ze0;

    // Compute difference
    double N = uNorth - uNorth0;  // [m]
    double E = vEast - vEast0;   // [m]
    double D = wDown - wDown0;   // [m]

    std::vector<double> NED;
    NED.push_back(N);
    NED.push_back(E);
    NED.push_back(D);

    return NED;

}

std::vector<double> objectCoord2NED(double x, double y, double z, double n_MA, double e_MA, double d_MA, double heading_MA){
  double E = (x*cos(heading_MA) - sin(heading_MA)*z)+e_MA;
  double N = (x*sin(heading_MA) + cos(heading_MA)*z)+n_MA;
  double D = d_MA;
/*
  double N = (x+n_MA)*cos(heading_MA) - sin(heading_MA)*(y+e_MA);
  double E = (x+n_MA)*sin(heading_MA) + cos(heading_MA)*(y+e_MA);
  double D = d_MA;
*/
  std::vector<double> NED;
  NED.push_back(N);
  NED.push_back(E);
  NED.push_back(D);

  return NED;
}

/*
std::vector<double> objectCoord_to_NED(double[] object_coord, std::vector<double> ma_NED, double ma_heading){
  //Parent=piren, child=gps_antenna, translation = NED posisjon nå, orientation
  //transform fra origio til nåværende posisjon

  geometry_msgs::TransformStamped t = broadcast_tf("piren", "camera_frame", ma_NED, [0, 0, heading]);
  //NED i forhold til camera frame
  geometry_msgs::TransformStamped tarns = t.lookup_transform("camera_frame", "camera_frame", 2);

  double N_cam = trans.transform.translation.x + object_coord[0];
  double E_cam = trans.transform.translation.y + object_coord[1];
  double D_cam = MA_d;

  std::vector<double> NED_cam;
  NED_cam.push_back(N_cam);
  NED_cam.push_back(E_cam);
  NED_cam.push_back(D_cam);

  geometry_msgs::TransformStamped t1 = broadcast_tf("piren", "camera_frame", ma_NED, [0, 0, heading]);

  return NED;

}*/


#ifndef EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_CHAIN_HPP
#define EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_CHAIN_HPP

#include <cmath>
#include <string>
#include <boost/math/special_functions/bessel.hpp>
#include <functional>

namespace exactsolution{

class PartitionFunctionClassicalXYChain{
public :
  static std::string name() { return "Exact partition function , classical XY, open boundary"; }
  PartitionFunctionClassicalXYChain(double J,unsigned int num) : J_(J), num_(num) {}

  double operator()(double T) const {
    double value = 0.0;
    value = 2.0 * M_PI * boost::math::cyl_bessel_i(0,J_/T);
    value = 2.0 * M_PI * std::pow(value , num_-1);
    value += std::pow( std::sqrt(2.0 * M_PI * T), num_);
    return value;
  }

  double logZ(double T) const {
    double value = std::log(2.0 * M_PI);
    value += (double)(num_ - 1) * std::log(2.0 * M_PI * boost::math::cyl_bessel_i(0,J_/T));
    value += (double)(num_) * 0.5 * std::log(2.0 * M_PI * T );
    return value;
  }

private :
  double J_;
  unsigned int num_;
};

} //end namespce

#endif //EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_CHAIN_HPP


/**
 * @file crossprod.cc
 * @brief NPDE homework CrossProd code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "crossprod.h"

#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <vector>

namespace CrossProd {

/* SAM_LISTING_BEGIN_0 */
void tab_crossprod() {
  // Solve the cross-product ODE with the implicit RK method
  // defined in solve_imp_mid. Tabulate the norms of the results at all steps.
  //====================
  // Your code goes here

  // set data

  double T =10;
  int M =128;
  int c=1;
  Eigen::Vector3d y0(1.0, 1.0, 1.0);
  Eigen::Vector3d a(1,0,0);

  // define right hand side function fy
  auto f [a,c] (Eigen::Vector3d y) -> Eigen::Vector3d {
    return a.cross(y)+c*y.cross(a.cross(y));
  }

  auto Jf [a,c] (Eigen::Vector3d y) -> Eigen::Matrix3d{
    Eigen::Matrix3d Jf;
    Jf << -c * (a(1) * y(1) + a(2) * y(2)),
        c * (2 * a(0) * y(1) - a(1) * y(0)) - a(2),
        a(1) + c * (2 * a(0) * y(2) - a(2) * y(0)),
        a(2) - c * (a(0) * y(1) - 2 * a(1) * y(0)),
        -c * (a(0) * y(0) + a(2) * y(2)),
        c * (2 * a(1) * y(2) - a(2) * y(1)) - a(0),
        -a(1) - c * (a(0) * y(2) - 2 * a(2) * y(0)),
        a(0) - c * (a(1) * y(2) - 2 * a(2) * y(1)),
        -c * (a(0) * y(0) + a(1) * y(1));
    return Jf;
  }

  // define the Jacobian of rhs

  std::vector<Eigen::VectorXd> sol_imp = solve_imp_mid(f, Jf, T, y0, M);

  std::cout << "1. Implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))" << std::endl;

  for (int i=0; i <M+1; ++i){
    std::cout << std::setw(10) << T*i/M << std::stew(15) << sol_imp[i].norm() << std::end;
  }

  std::vector<Eigen::VectorXd> sol_lin = sol_lin_mid(f, Jf, T, y0, M);

  std::cout << "\n2. Linear implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))" << std::endl;

  for (int i =0; i<M+1;i++){
    // tabulate the ||yk||2 for the sequence of approcimate states generated by the linear implicit
    //midpoint method
    std::cout <<std::setw(10) << T*i/M << std::setw(15) << sol_lin[i].norm() << std::endl;

  }

  //====================
  /* SAM_LISTING_END_0 */

  /* SAM_LISTING_BEGIN_1 */
  // Solve the cross-product ODE with the implicit RK method
  // defined in solve_lin_mid. Tabulate the norms of the results at all steps.
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_1 */
}

}  // namespace CrossProd


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
/// Function interpolation stuff.

#include <unsupported/Eigen/Splines>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace alma {
/// Perform spline interpolation of a tabulated function Y(X) at
/// Xtarget.
inline Eigen::VectorXd splineInterpolation(
    const Eigen::Ref<const Eigen::VectorXd>& X,
    const Eigen::Ref<const Eigen::VectorXd>& Y,
    const Eigen::Ref<const Eigen::VectorXd>& Xtarget) {
    Eigen::VectorXd result(Xtarget.size());

    // map X values to 1D chord lengths ranging from 0 to 1
    Eigen::VectorXd X_norm =
        (X.array() - X.minCoeff()) / (X.maxCoeff() - X.minCoeff());

    // construct cubic spline over the base points
    typedef Eigen::Spline<double, 1> Spline1D;
    Spline1D S(Eigen::SplineFitting<Spline1D>::Interpolate(
        Y.transpose(), 3, X_norm.transpose()));

    // convert Xtarget values to their equivalent chord length
    Eigen::VectorXd Xtarget_norm =
        (Xtarget.array() - X.minCoeff()) / (X.maxCoeff() - X.minCoeff());

    // obtain spline value at each of these chord lengths

    for (int nx = 0; nx < Xtarget.size(); nx++) {
        result(nx) = S(Xtarget_norm(nx))(0);
    }

    return result;
}

/// Perform linear interpolation of a tabulated function Y(X) at
/// Xtarget.
/// X must be strictly ascending (no duplicate entries).
inline Eigen::VectorXd linearInterpolation(
    const Eigen::Ref<const Eigen::VectorXd>& X,
    const Eigen::Ref<const Eigen::VectorXd>& Y,
    const Eigen::Ref<const Eigen::VectorXd>& Xtarget) {
    Eigen::VectorXd result(Xtarget.size());
    int N = X.size();

    // perform linear interpolation for each of the specified targets

    for (int nx = 0; nx < Xtarget.size(); nx++) {
        if (Xtarget(nx) < X(0)) {
            std::cout << "alma::linearInterpolation > " << std::endl;
            std::cout << "WARNING: encountered target element out of bounds."
                      << std::endl;
            std::cout << "Interpolated result will be clipped." << std::endl;

            result(nx) = X(0);
        }

        else if (Xtarget(nx) > X(N - 1)) {
            std::cout << "alma::linearInterpolation > " << std::endl;
            std::cout << "WARNING: encountered target element out of bounds."
                      << std::endl;
            std::cout << "Interpolated result will be clipped." << std::endl;

            result(nx) = X(N - 1);
        }

        else {
            // use bisection algorithm to determine base array bin the target
            // sits
            // in.

            int leftindex = 0;
            int rightindex = N - 1;

            while (rightindex - leftindex > 1) {
                int midindex = (leftindex + rightindex) / 2;

                if (X(midindex) > Xtarget(nx)) {
                    rightindex = midindex;
                }
                else {
                    leftindex = midindex;
                }
            }

            double yleft = Y(leftindex);
            double yright = Y(rightindex);
            double xleft = X(leftindex);
            double xright = X(rightindex);

            result(nx) = yleft + (Xtarget(nx) - xleft) * (yright - yleft) /
                                     (xright - xleft);
        }
    }

    return result;
}

class CubicSpline1D {
private:
    Eigen::MatrixXd splines;
    std::vector<double> x;

public:
    /// Default constructors
    CubicSpline1D() = default;
    CubicSpline1D(const CubicSpline1D& A) = default;

    /// Constructor from data
    CubicSpline1D(const Eigen::Ref<const Eigen::VectorXd>& X,
                  const Eigen::Ref<const Eigen::VectorXd>& Y);

    /// Build interpolation from given data
    /// it discards the data
    void BuildCubicSpline1D(const Eigen::Ref<const Eigen::VectorXd>& X,
                            const Eigen::Ref<const Eigen::VectorXd>& Y);

    /// Interpolates Y for newX
    double Interpolate(double newX);
};

} // namespace alma


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

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef long long nombre;
typedef std::vector<nombre> vecteur;
typedef boost::rational<nombre> fraction;
typedef std::pair<nombre, nombre> paire;

namespace {
    fraction aire(const paire &A, const paire &B, const paire &C) {
        nombre abc = (B.first - A.first) * (C.second - A.second) - (C.first - A.first) * (B.second - A.second);
        if (abc < 0)
            abc = -abc;
        return fraction(abc, 2);
    }

    bool contient(const paire &A, const paire &B, const paire &C) {
        static const paire O = std::make_pair(0, 0);
        const fraction ABC = aire(A, B, C);
        const fraction OBC = aire(O, B, C);
        const fraction AOC = aire(A, O, C);
        const fraction ABO = aire(A, B, O);

        return (ABC == OBC + AOC + ABO);
    }
}

ENREGISTRER_PROBLEME(102, "Triangle containment") {
    // Three distinct points are plotted at random on a Cartesian plane, for which -1000 ≤ x, y ≤ 1000, such that a
    // triangle is formed.
    //
    // Consider the following two triangles:
    //
    //                                  A(-340,495), B(-153,-910), C(835,-947)
    //
    //                                  X(-175,41), Y(-421,-714), Z(574,-645)
    //
    // It can be verified that triangle ABC contains the origin, whereas triangle XYZ does not.
    //
    // Using triangles.txt (right click and 'Save Link/Target As...'), a 27K text file containing the co-ordinates of
    // one thousand "random" triangles, find the number of triangles for which the interior contains the origin.
    //
    // NOTE: The first two examples in the file represent the triangles in the example given above.
    std::ifstream ifs("data/p102_triangles.txt");
    nombre resultat = 0;
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        const paire A(stoll(v[0]), stoll(v[1]));
        const paire B(stoll(v[2]), stoll(v[3]));
        const paire C(stoll(v[4]), stoll(v[5]));
        if (contient(A, B, C))
            ++resultat;
    }
    return std::to_string(resultat);
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/SVD>

namespace fluid {
namespace algorithm {

class NNDSVD
{

public:
  using MatrixXd = Eigen::MatrixXd;

  index process(RealMatrixView X, RealMatrixView W, RealMatrixView H,
                index minRank = 0, index maxRank = 200, double amount = 0.8,
                index method = 0) // 0 - NMF-SVD, 1 NNDSVDar, 2 NNDSVDa 3 NNDSVD
  {
    using namespace _impl;
    using namespace Eigen;
    MatrixXd XT = asEigen<Matrix>(X).transpose();
    MatrixXd WT = asEigen<Matrix>(W).transpose();
    MatrixXd HT = asEigen<Matrix>(H).transpose();

    assert(amount > 0 || minRank > 0);

    BDCSVD<MatrixXd> svd(XT, ComputeThinV | ComputeThinU);

    MatrixXd U = svd.matrixU();
    MatrixXd V = svd.matrixV().transpose();
    VectorXd s = svd.singularValues();
    MatrixXd S = svd.singularValues().asDiagonal();
    index    k = 0;
    if (amount == 0)
      k = minRank;
    else
    {
      double current = 0;
      double total = s.sum();
      while ((current / total) < amount) current += s[k++];
    }
    if (k < minRank) k = minRank;
    if (k > maxRank) k = maxRank;

    if (method == 0)
    {
      WT.block(0, 0, WT.rows(), k) = U.block(0, 0, U.rows(), k).array().abs();
      HT.block(0, 0, k, HT.cols()) =
          (S.block(0, 0, k, S.cols()) * V).array().abs();
    }
    else
    {
      // avoid scaling for NMF with normalized W
      WT.col(0) = U.col(0).array().abs();
      HT.row(0) = sqrt(s(0)) * V.row(0).array().abs();

      for (index j = 1; j < k; j++)
      {
        VectorXd x = U.col(j);
        VectorXd y = V.row(j);
        VectorXd xP = x.array().max(0.0);
        VectorXd yP = y.array().max(0.0);
        VectorXd xN = x.array().min(0.0).abs();
        VectorXd yN = y.array().min(0.0).abs();
        double   xPNorm = xP.norm();
        double   yPNorm = yP.norm();
        double   xNNorm = xN.norm();
        double   yNNorm = xN.norm();
        double   mP = xPNorm * yPNorm;
        double   mN = xNNorm * yNNorm;
        ArrayXd  u;
        ArrayXd  v;
        double   sigma;
        if (mP > mN)
        {
          u = xP / xPNorm;
          v = yP / yPNorm;
          sigma = mP;
        }
        else
        {
          u = xN / xNNorm;
          v = yN / yNNorm;
          sigma = mN;
        }
        auto lbd = std::sqrt(s[j] * sigma);
        WT.col(j) = u; // avoid scaling for NMF with normalized W
        HT.row(j) = lbd * v;
      }
      WT = WT.array().max(epsilon);
      HT = HT.array().max(epsilon);
      if (method == 1)
      {
        auto Wrand =
            MatrixXd::Random(WT.rows(), WT.cols()).array().abs() / 100.0;
        auto Hrand =
            MatrixXd::Random(HT.rows(), HT.cols()).array().abs() / 100.0;
        WT = (WT.array() < epsilon).select(Wrand, WT);
        HT = (HT.array() < epsilon).select(Hrand, HT);
      }
      else if (method == 2)
      {
        double mean = XT.mean();
        WT = (WT.array() < epsilon)
                 .select(MatrixXd::Constant(WT.rows(), WT.cols(), mean), WT);
        HT = (HT.array() < epsilon)
                 .select(MatrixXd::Constant(HT.rows(), HT.cols(), mean), HT);
      }
    }
    MatrixXd W1 = WT.transpose();
    W <<= asFluid(W1);
    MatrixXd H1 = HT.transpose();
    H <<= asFluid(H1);
    return k;
  }
};
} // namespace algorithm
} // namespace fluid


// Copyright (c) 2017 Evan S Weinberg
// A reference piece of code which computes matrix elements
// of a reference sparse first derivative operator to fill a dense
// Eigen matrix, then computes the spectrum and prints
// the Eigenvalues.

// This code is for real, indefinite matrices.
// This code lives on github at github.com/weinbe2/eigens-with-eigen/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// This is just for convenience. By default, all matrices
// in Eigen are column major. You can replace "Dynamic"
// with a specific number to template just one size.
// You can also ust use "MatrixXd".
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;
// Likewise, we can use "MatrixXcd".
typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;
// We need both real and complex because, while the matrix itself is real,
// the eigenvalues and eigenvectors can generically be complex.

// Reference 1-D central difference operator
void central_diff_1d(double* out, double* in, const int L, const double m2);

int main(int argc, char** argv)
{
  double *in_real;
  double *out_real;
  complex<double> *out_cplx; // needed for eigenvectors

  // Set output precision to be long.
  cout << setprecision(10);

  // Basic information about the lattice.
  const int length = 8;
  const double mass = 0.001;

  // Print the basic info.
  std::cout << "1D central difference operator, length " << length << ", mass " << mass << ", periodic boundary conditions.\n";
  std::cout << "Change the length and mass by modifying the source.\n";

  // Allocate.
  in_real = new double[length];
  out_real = new double[length];
  out_cplx = new complex<double>[length];

  // Zero out.
  for (int i = 0; i < length; i++)
  {
    in_real[i] = out_real[i] = 0.0;
  }

  ///////////////////////////
  // REAL, INDEFINITE CASE //
  ///////////////////////////

  std::cout << "Real, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(length, length);

  // Form matrix elements. This is where it's important that
  // dMatrix is column major.
  for (int i = 0; i < length; i++)
  {
    // Set a point on the rhs for a matrix element.
    // If appropriate, zero out the previous point.
    if (i > 0)
    {
      in_real[i-1] = 0.0;
    }
    in_real[i] = 1.0;

    // Zero out the "out" vector. I defined "laplace_1d" to
    // not require this, but I put this here for generality.
    for (int j = 0; j < length; j++)
      out_real[j] = 0.0;

    // PUT YOUR MAT-VEC HERE.
    central_diff_1d(out_real, in_real, length, mass);

    // Copy your output into the right memory location.
    // If your data layout supports it, you can also pass
    // "mptr" directly as your "output vector" when you call
    // your mat-vec.
    double* mptr = &(mat_real(i*length));

    for (int j = 0; j < length; j++)
    {
      mptr[j] = out_real[j];
    }
  }

  // We've now formed the dense matrix. We print it here
  // as a sanity check if it's small enough.
  if (length <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues and eigenvectors.
  EigenSolver< dMatrix > eigsolve_real_indef(length);
  eigsolve_real_indef.compute(mat_real);

  // Remark: if you only want the eigenvalues, you can call
  // eigsolve_real.compute(mat_real, EigenvaluesOnly);

  // Print the eigenvalues.
  // Note that the eigenvalues are of type "cMatrix" as typedef'd above or
  // MatrixXcd generically.
  cMatrix evals = eigsolve_real_indef.eigenvalues();
  std::cout << "The eigenvalues are:\n" << evals << "\n\n";
  // You can also index individual eigenvalues as "evals(i)", where
  // "i" zero-indexes the eigenvalues.

  // Print the eigenvectors if the matrix is small enough.
  // As a remark, this also shows you how to access the eigenvectors.
  if (length <= 16)
  {
    for (int i = 0; i < length; i++)
    {
      // You can use "VectorXcd" as the type instead.
      cMatrix evec = eigsolve_real_indef.eigenvectors().col(i);

      // Print the eigenvector.
      std::cout << "Eigenvector " << i << " equals:\n" << evec << "\n\n";

      // You can also copy the eigenvector into another array as such:
      for (int j = 0; j < length; j++)
      {
        out_cplx[j] = evec(j);
      }
    }
  }

  // Clean up.
  delete[] in_real;
  delete[] out_real;
  delete[] out_cplx;

  return 0;
}

// Reference 1-D central difference function.
void central_diff_1d(double* out, double* in, const int L, const double mass)
{
  // Central difference operator with periodic boundary conditions.
  for (int i = 0; i < L; i++)
  {
    out[i] = mass*in[i] + 0.5*(in[(i+1)%L] - in[(i-1+L)%L]);
  }

  // Done.
  return;
}


#include <iostream>
#include <string>
#include <map>
#include <Eigen/Dense>
#include "../simple_lib/include/two_layer_net.h"

using namespace Eigen;
using namespace MyDL;

int main(){
    using std::cout;
    using std::endl;
    using std::string;
    using std::map;

    int batch_size = 2;
    int input_size = 2;
    int hidden_size = 3;
    int output_size = 2;

    // int batch_size = 3;
    // int input_size = 2;
    // int hidden_size = 3;
    // int output_size = 5;

//     int batch_size = 5;
//     int input_size = 28 * 28;
//     int hidden_size = 100;
//     int output_size = 10;

    TwoLayerNet net(input_size, hidden_size, output_size, 0.01);

    MatrixXd X = MatrixXd::Random(batch_size, input_size);
    MatrixXd y; // (batch_size, output_size)
    MatrixXd t = MatrixXd::Zero(batch_size, output_size);
//     MatrixXd t = MatrixXd::Random(batch_size, output_size);
    double loss;
    double accuracy;
    map<string, MatrixXd> grads, numerical_grads;

    X = -2 * MatrixXd::Identity(2, 2) + MatrixXd::Ones(2, 2);
    t = MatrixXd::Identity(2, 2);

    // X << 1, 2,
    //      3, 4,
    //      5, 6;
    // t << 0, 1, 0, 0, 0,
    //      0, 0, 0, 1, 0,
    //      0, 0, 0, 1, 0;

    y = net.predict(X);
    cout << "y = " << y << endl; // predictの結果は、行方向に総和を取った場合、「1」となっていることを確認

    loss = net.loss(X, t);
    cout << "loss: " << loss << endl;

    accuracy = net.accuracy(X, t);
    cout << "accuracy: " << accuracy << endl; // この入力の場合、accuracyは0.666..が正解

    grads = net.gradient(X, t);
    cout << "gradient of W1: " << grads["W1"] << endl;
    cout << "gradient of W2: " << grads["W2"] << endl;
    cout << "gradient of b1: " << grads["b1"] << endl;
    cout << "gradient of b2: " << grads["b2"] << endl;

    cout << "numerical gradient" << endl;
    numerical_grads = net.numerical_gradient(X, t);
    cout << "gradient of W1: " << numerical_grads["W1"] << endl;
    cout << "gradient of W2: " << numerical_grads["W2"] << endl;
    cout << "gradient of b1: " << numerical_grads["b1"] << endl;
    cout << "gradient of b2: " << numerical_grads["b2"] << endl;

    return 0;
}


#ifndef AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP
#define AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP

#include <armadillo>
#include <functional>

namespace amt::regression{

    struct default_opt{

        auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const {
            // x_t = x^T
            auto x_t = x.t();

            // xt_y = x^T * y
            auto xt_y = x_t * y;

            if( lm == 0 ){
                // xt_x = x^T * x
                auto xt_x = x_t * x;

                // (X * X^T) * B = X^T * Y
                beta = arma::solve(xt_x,xt_y,arma::solve_opts::fast);
            }else{

                arma::Mat<double> l_I = arma::eye(x.n_cols, x.n_cols) * lm;

                // xt_x = x^T * x + l_I
                auto xt_x_lI = ( x_t * x ) + l_I;

                // (X * X^T) * B = X^T * Y
                beta = arma::solve(xt_x_lI,xt_y,arma::solve_opts::fast);
            }

        }

    };

    struct gradient_descent{

        constexpr gradient_descent() noexcept = default;
        constexpr gradient_descent(gradient_descent const&  other) noexcept = default;
        constexpr gradient_descent(gradient_descent &&  other) noexcept = default;
        constexpr gradient_descent& operator=(gradient_descent const&  other) noexcept = default;
        constexpr gradient_descent& operator=(gradient_descent &&  other) noexcept = default;
        ~gradient_descent() = default;

        constexpr gradient_descent(double alpha, std::size_t iter)
            : alpha(alpha)
            , iteration(iter)
        {}

        auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const {
            beta.resize(x.n_cols, 1ul);
            beta.randu();
            if( lm == 0.0 )
                eval(beta,x,y,lm);
            else
                eval(beta,x,y);
        }

        // double compute_cost(arma::Mat<double> const& beta, arma::Mat<double> const& x, arma::Mat<double> const& y){
        //     auto temp = ( x * beta ) - y;
        //     auto m = 2 * x.n_rows;
        //     auto J = ( temp * temp.t() ) / static_cast<double>(m);
        //     return J[0];
        // }

        void eval(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const{
            auto m = alpha / static_cast<double>(x.n_rows);
            auto reg_coef = m * lm;

            auto x_t = x.t();
            for( auto i = 0u; i < iteration; ++i ){
                auto p = ( x * beta ) - y;
                auto temp = m * (x_t * p);
                beta -= ( temp + ( reg_coef * beta ) );
            }
        }

        void eval(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y) const{
            auto m = alpha / static_cast<double>(x.n_rows);
            auto x_t = x.t();
            for( auto i = 0u; i < iteration; ++i ){
                auto p = ( x * beta ) - y;
                auto temp = m * (x_t * p);
                beta -= temp;
            }
        }

        double alpha{0.01};
        std::size_t iteration{1500};
    };

    template<typename T>
    inline static constexpr bool is_default_opt_v = std::is_same_v<T,default_opt>;

    template<typename T>
    inline static constexpr bool is_gradient_descent_v = std::is_same_v<T,gradient_descent>;

} // namespace amt::regression

#endif


/**
 * @file eigen.cpp
 * @author Wei Du
 *
 * @version 0.1
 * @date 2021-08-18
 */

// STL include
#include <array>
#include <cassert>
#include <iostream>

// external include
#include <Eigen/Dense>
#include <Eigen/Geometry>

/*
 * int main() {
 *   //  x, y, z, w
 *   std::array<double, 4> arr{0.3, 0.27, 0, 1.0};
 *   Eigen::Vector4d vct{0.3, 0.27, 0, 1};
 *
 *   // Eigen::Quaterniond quat1(vct(0), vct(1), vct(2), vct(3));
 *   // Eigen::Quaterniond quat2(arr[0], arr[1], arr[2], arr[3]);
 *   Eigen::Quaterniond quat1(vct.data());
 *   Eigen::Quaterniond quat2(arr.data());
 *
 *   printf("should be: %.2f %.2f %.2f %.2f\n", arr[0], arr[1], arr[2], arr[3]);
 *   printf("%.2f %.2f %.2f %.2f\n", quat1.x(), quat1.y(), quat1.z(),
 * quat1.w()); printf("%.2f %.2f %.2f %.2f\n", quat2.x(), quat2.y(), quat2.z(),
 * quat2.w()); return 0;
 * }
 */

/*
 * int main() {
 *   Eigen::VectorXd v1(4);
 *   Eigen::Vector3d v2{0.1, 0.2, 0.3};
 *
 *   v1.head(3) = v2;
 *   v1(3) = 0.8;
 *   std::cout << v1 << std::endl;
 *   return 0;
 * }
 */

/*
int main() {
  Eigen::Vector2d tran{0.2, 0.3};
  Eigen::Rotation2Dd ori(0.785398);

  Eigen::Affine2d pose;
  pose.setIdentity();
  pose.translate(tran);
  pose.rotate(ori);

  Eigen::Vector2d origin{0.1, 0.0};
  Eigen::Vector2d newpt = pose * origin;
  std::cout << newpt << std::endl;
  return 0;
}
*/

/*
int main() {
  Eigen::Vector3d vct1;
  Eigen::VectorXd vct2;
  if (vct1.isApprox(vct2)) {
    printf("they are the same\n");
  } else {
    printf("they are not the same\n");
  }
  std::cout << vct1 << std::endl;

  return 0;
}
*/

/*
 * int main() {
 *   Eigen::MatrixXd mt;
 *   mt.resize(1000, 1000);
 *   mt.fill(-1);
 *   std::cout << mt;
 *   return 0;
 * }
 */

/*
 * int main() {
 *   Eigen::MatrixXd mat(5, 4);
 *   mat << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
 * 20;
 *
 *   std::cout << mat.rows() << std::endl;
 *   mat.conservativeResize(mat.rows() - 2, 3);
 *   std::cout << mat << std::endl;
 *   std::cout << mat.rows() << std::endl;
 *
 *   return 0;
 * }
 */

/*
 * int main() {
 *   const Eigen::Vector3d vec{0, 1, 0};
 *   std::cout << vec << std::endl;
 *   return 0;
 * }
 */

int main() {
  Eigen::MatrixXd mt(2, 2);
  mt << 0, 1, 2, 3;

  Eigen::MatrixXd new_mt = mt;
  mt(1, 1) = 8;
  std::cout << mt << std::endl;
  std::cout << new_mt << std::endl;

  return 0;
}


//============================================================================
// Daniel J. Greenhoe
// normed linear space R^2
//============================================================================
//=====================================
// headers
//=====================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>
#include "r1.h"
#include "r3.h"

//=====================================
// VectR3
//=====================================*/
//-----------------------------------------------------------------------------
//! \brief Calculate magnitude of vector
//-----------------------------------------------------------------------------
double vectR3::mag(void) const
{
  const double x = getx();
  const double y = gety();
  const double z = getz();
  const double magSq  = x*x + y*y + z*z;
  const double magVal = sqrt( magSq );
  return magVal;
}

//-----------------------------------------------------------------------------
//! \brief Convert polar (r, theta, phi) coordinate to rectangular (x,y,z) coordinate
//-----------------------------------------------------------------------------
void vectR3::polartoxyz(const double r, const double theta, const double phi)
{
  const double x = r*cos(phi)*cos(theta);
  const double y = r*cos(phi)*sin(theta);
  const double z = r*sin(phi);
  otriple::put( x, y, z );
}

//-----------------------------------------------------------------------------
//! \brief Add vector q to vectR3 vector
//-----------------------------------------------------------------------------
void vectR3::operator+=(const vectR3 q)
{
  Eigen::Map< Eigen::Vector3d > a(   getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdata() );
  a += b;
}

//-----------------------------------------------------------------------------
//! \brief Subtract vector q from vectR3 vector
//-----------------------------------------------------------------------------
void vectR3::operator-=(const vectR3 q)
{
  Eigen::Map< Eigen::Vector3d > a(   getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdata() );
  a -= b;
}

//=====================================
// seqR3
//=====================================*/
//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR3 to 0
//-----------------------------------------------------------------------------
seqR3::seqR3(long M)
{
  N=M;
  seqr3 = new vectR3[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR3 to <u>
//-----------------------------------------------------------------------------
seqR3::seqR3(long M, double u)
{
  N = M;
  seqr3 = new vectR3[N];
  fill( u );
}

//-----------------------------------------------------------------------------
//! \brief Fill the seqR3 with a value 0
//-----------------------------------------------------------------------------
void seqR3::clear(void)
{
  for( long n=0; n<N; n++) put( n, 0 );
}

//-----------------------------------------------------------------------------
//! \brief Fill the seqR3 with a value <u>
//-----------------------------------------------------------------------------
void seqR3::fill(double u)
{
  long n;
  for(n=0; n<N; n++) put( n, u );
}

//-----------------------------------------------------------------------------
//! \brief Put a single value <u> into the seqR3 x at location n
//-----------------------------------------------------------------------------
int seqR3::put(long n, double u, double v, double w)
{
  if(n<N){
    seqr3[n].otriple::put( u, v, w );
    return 0;
    }
  else{
    fprintf(stderr,"n=%ld larger than seqR3 size N=%ld\n",n,N);
    return -1;
    }
}

//-----------------------------------------------------------------------------
//! \brief Put a single value <u> into the seqR3 x at location n
//-----------------------------------------------------------------------------
int seqR3::put(long n, vectR3 abc)
{
  if(n<N)
  {
    (seqr3[n]).otriple::put( abc.getx(), abc.gety(), abc.gety() );
    return 0;
  }
  else
  {
    fprintf(stderr,"n=%ld larger than seqR3 size N=%ld\n",n,N);
    return -1;
  }
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x at location n
//-----------------------------------------------------------------------------
vectR3 seqR3::get(long n)
{
  return seqr3[n];
}

//-----------------------------------------------------------------------------
//! \brief Get the x element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::getx(long n)
{
  double u=0;
  if(n<N)u = seqr3[n].getx();
  else   fprintf(stderr,"n=%ld larger than x seqR3 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief Get the y element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::gety(long n){
  double u=0;
  if(n<N)u = seqr3[n].gety();
  else   fprintf(stderr,"n=%ld larger than y seqR3 size N=%ld\n",n,N);
  return u;
  }

//-----------------------------------------------------------------------------
//! \brief Get the z element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::getz(long n){
  double u=0;
  if(n<N)u = seqr3[n].getz();
  else   fprintf(stderr,"n=%ld larger than z seqR3 size N=%ld\n",n,N);
  return u;
  }

//-----------------------------------------------------------------------------
//! \brief list contents of sequence
//-----------------------------------------------------------------------------
void seqR3::list(const long start, const long end, const char *str1, const char *str2, FILE *ptr){
  long n,m;
  if(ptr!=NULL){
    if(strlen(str1)>0) fprintf(ptr,"%s",str1);
    for(n=start,m=1; n<=end; n++,m++){
      fprintf(ptr,"(%6.3lf,%6.3lf,%6.3lf) ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
      if(m%3==0)fprintf(ptr,"\n");
      }
    if(strlen(str2)>0)fprintf(ptr,"%s",str2);
    }
  }

//-----------------------------------------------------------------------------
//! \brief list contents of seqR3 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR3::list1(void){
  long n,m;
  for(n=0,m=1; n<N; n++,m++){
    printf("(%2.0lf,%2.0lf,%2.0lf)   ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
    if(m%5==0)printf("\n");
    }
  }
void seqR3::list1(long start, long end){
  long n,m;
  for(n=start,m=1; n<=end; n++,m++){
    printf("(%2.0lf,%2.0lf,%2.0lf)   ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
    if(m%50==0)printf("\n");
    else if(m%10==0)printf(" ");
    }
  }

//=====================================
// external operations
//=====================================
//-----------------------------------------------------------------------------
//! \brief operator: return p+q
//-----------------------------------------------------------------------------
vectR3 operator+(vectR3 p, vectR3 q)
{
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdataa() );
  const Eigen::Vector3d c = a + b;
  const vectR3 r( c(0), c(1), c(2) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief operator: return p-q
//-----------------------------------------------------------------------------
vectR3 operator-(vectR3 p, vectR3 q){
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdataa() );
  const Eigen::Vector3d c = a - b;
  const vectR3 r( c(0), c(1), c(2) );
  return r;
  }

//-----------------------------------------------------------------------------
//! \brief  operator: return -p
//-----------------------------------------------------------------------------
vectR3 operator-(vectR3 p)
{
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Vector3d b = -a;
  const vectR3 q( b(0), b(1), b(2) );
  return q;
}

//-----------------------------------------------------------------------------
//! \brief return the angle theta in radians between the two vectors induced by
//! the points <p> and <q> in the space R^3.
//! \returns On SUCCESS return theta in the closed interval [0:PI];
//!          On ERROR   return negative value or exit with value EXIT_FAILURE
//-----------------------------------------------------------------------------
double pqtheta(const vectR3 p, const vectR3 q)
{
  const double rp=p.mag();
  const double rq=q.mag();
  if(rp==0) return -1;
  if(rq==0) return -2;
  const double y = (p^q)/(rp*rq);
  if(y>+1)  {fprintf(stderr,"\nERROR using pqtheta(vectR3 p, vectR3 q): (p^q)/(rp*rq)=%lf>+1\n",y); exit(EXIT_FAILURE);}
  if(y<-1)  {fprintf(stderr,"\nERROR using pqtheta(vectR3 p, vectR3 q): (p^q)/(rp*rq)=%lf<-1\n",y); exit(EXIT_FAILURE);}
  const double theta = acos(y);
  return theta;
}

//-----------------------------------------------------------------------------
//! \brief Return the minimum element of the tupple
//-----------------------------------------------------------------------------
double otriple::min(void) const
{
  const Eigen::Map< const Eigen::Vector3d > abc( getdata() );
  int row, col;
  const double min = abc.minCoeff( &row, &col );
  return min;
}

//-----------------------------------------------------------------------------
//! \brief Return the maximum element of the tupple
//-----------------------------------------------------------------------------
double otriple::max(void) const
{
  const Eigen::Map< const Eigen::Vector3d > abc( getdata() );
  int row, col;
  const double max = abc.maxCoeff( &row, &col );
  return max;
}


#ifndef MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP
#define MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP

#include <Eigen/Core>
#include <cassert>
#include <functional>
#include <iostream>

#define MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS

namespace mathtoolbox
{
    namespace optimization
    {
        // Algorithm 3.5: Line Search Algorithm
        //
        // This algoritmh tries to find an appropriate step size that satisfies the strong Wolfe conditions (i.e., both
        // the safficient decreasing condition and the curvature condition).
        inline double
        RunStrongWolfeConditionsLineSearch(const std::function<double(const Eigen::VectorXd&)>&          f,
                                           const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                                           const Eigen::VectorXd&                                        x,
                                           const Eigen::VectorXd&                                        p,
                                           const double                                                  alpha_init,
                                           const double alpha_max = 50.0,
                                           const double c_1       = 1e-04,
                                           const double c_2       = 0.9)
        {
            auto phi      = [&](const double alpha) { return f(x + alpha * p); };
            auto phi_grad = [&](const double alpha) { return static_cast<double>(g(x + alpha * p).transpose() * p); };

            const double phi_zero      = phi(0.0);
            const double phi_grad_zero = phi_grad(0.0);

            double alpha_prev     = 0.0;
            double alpha          = alpha_init;
            double phi_alpha_prev = phi_zero;

            bool is_first = true;

            // Algorithm 3.6: Zoom
            auto zoom = [&](double alpha_l, double alpha_h) {
                constexpr unsigned int max_num_iterations = 50;
                for (unsigned int i = 0; i < max_num_iterations; ++i)
                {
                    const double alpha_j     = 0.5 * (alpha_l + alpha_h); // TODO: Use a better strategy
                    const double phi_alpha_j = phi(alpha_j);

                    if (phi_alpha_j > phi_zero + c_1 * alpha_j * phi_grad_zero || phi_alpha_j >= phi(alpha_l))
                    {
                        alpha_h = alpha_j;
                    }
                    else
                    {
                        const double phi_grad_alpha_j = phi_grad(alpha_j);
                        if (std::abs(phi_grad_alpha_j) <= -c_2 * phi_grad_zero)
                        {
                            return alpha_j;
                        }
                        if (phi_grad_alpha_j * (alpha_h - alpha_l) >= 0.0)
                        {
                            alpha_h = alpha_l;
                        }
                        alpha_l = alpha_j;
                    }
                }
#ifdef MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS
                std::cerr << "Warning: The line search did not converge." << std::endl;
#endif
                return 0.5 * (alpha_l + alpha_h);
            };

            constexpr unsigned int max_num_iterations = 50;
            for (int i = 0; i < max_num_iterations; ++i)
            {
                const double phi_alpha = phi(alpha);

                if (phi_alpha > phi_zero + c_1 * alpha * phi_grad_zero || (!is_first && (phi_alpha >= phi_alpha_prev)))
                {
                    return zoom(alpha_prev, alpha);
                }

                const double phi_grad_alpha = phi_grad(alpha);

                if (std::abs(phi_grad_alpha) <= -c_2 * phi_grad_zero)
                {
                    return alpha;
                }

                if (phi_grad_alpha >= 0.0)
                {
                    return zoom(alpha, alpha_prev);
                }

                alpha_prev     = alpha;
                phi_alpha_prev = phi_alpha;

                alpha = 0.5 * (alpha + alpha_max); // TODO: Use a better strategy
            }
#ifdef MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS
            std::cerr << "Warning: The line search did not converge." << std::endl;
#endif
            return alpha_init;
        }
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/plus.hpp>

#include <boost/hana/constant.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/pair.hpp>

#include <cassert>
#include <type_traits>
namespace hana = boost::hana;

template <typename T, typename = std::enable_if_t<
  !hana::models<hana::Constant, T>()
>>
constexpr T sqrt(T x) {
  T inf = 0, sup = (x == 1 ? 1 : x/2);
  while (!((sup - inf) <= 1 || ((sup*sup <= x) && ((sup+1)*(sup+1) > x)))) {
    T mid = (inf + sup) / 2;
    bool take_inf = mid*mid > x ? 1 : 0;
    inf = take_inf ? inf : mid;
    sup = take_inf ? mid : sup;
  }

  return sup*sup <= x ? sup : inf;
}

template <typename T, typename = std::enable_if_t<
  hana::models<hana::Constant, T>()
>>
constexpr auto sqrt(T const&) {
  return hana::integral_constant<typename T::value_type, sqrt(T::value)>;
}

namespace then {
using namespace boost::mpl;

template <typename N>
struct sqrt
  : integral_c<typename N::value_type, ::sqrt(N::value)>
{ };

template <typename X, typename Y>
struct point {
  using x = X;
  using y = Y;
};

// sample(arithmetic-then)
template <typename P1, typename P2>
struct distance {
  using xs = typename minus<typename P1::x,
                            typename P2::x>::type;
  using ys = typename minus<typename P1::y,
                            typename P2::y>::type;
  using type = typename sqrt<
    typename plus<
      typename multiplies<xs, xs>::type,
      typename multiplies<ys, ys>::type
    >::type
  >::type;
};

static_assert(equal_to<
  distance<point<int_<3>, int_<5>>, point<int_<7>, int_<2>>>::type,
  int_<5>
>::value, "");
// end-sample
}

namespace now {
using namespace boost::hana;
using namespace boost::hana::literals;

template <typename X, typename Y>
struct _point {
  X x;
  Y y;
};
template <typename X, typename Y>
constexpr _point<X, Y> point(X x, Y y) { return {x, y}; }

// sample(arithmetic-now)
template <typename P1, typename P2>
constexpr auto distance(P1 p1, P2 p2) {
  auto xs = p1.x - p2.x;
  auto ys = p1.y - p2.y;
  return sqrt(xs*xs + ys*ys);
}

static_assert(distance(point(3_c, 5_c), point(7_c, 2_c)) == 5_c, "");
// end-sample

void test() {

// sample(arithmetic-now-dynamic)
auto p1 = point(3, 5); // dynamic values now
auto p2 = point(7, 2); //
assert(distance(p1, p2) == 5); // same function works!
// end-sample

}
}

int main() {
  now::test();
}


// solving A * X = B
// A hermitian
// driver function hesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/hesv.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

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

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

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> cherml_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> chermu_t;

int main() {

  cm_t cal (3, 3), cau (3, 3);   // matrices (storage)
  cherml_t hcal (cal);   // hermitian adaptor
  chermu_t hcau (cau);   // hermitian adaptor
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  hcal (0, 0) = cmplx_t (3, 0);
  hcal (1, 0) = cmplx_t (4, -2);
  hcal (1, 1) = cmplx_t (5, 0);
  hcal (2, 0) = cmplx_t (-7, -5);
  hcal (2, 1) = cmplx_t (0, 3);
  hcal (2, 2) = cmplx_t (2, 0);

  hcau (0, 0) = cmplx_t (3, 0);
  hcau (0, 1) = cmplx_t (4, 2);
  hcau (0, 2) = cmplx_t (-7, 5);
  hcau (1, 1) = cmplx_t (5, 0);
  hcau (1, 2) = cmplx_t (0, -3);
  hcau (2, 2) = cmplx_t (2, 0);

  print_m (cal, "cal");
  cout << endl;
  print_m (cau, "cau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hcal, cx);
  cbu = prod (hcau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  cm_t cal2 (hcal), cau2 (hcau);  // for part 2
  cm_t cbl2 (cbl), cbu2 (cbu);

  int ierr = lapack::hesv (hcal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  std::vector<integer_t> ipiv (3);
  std::vector<cmplx_t> cwork (3*64);
  // 3*64 -- optimal size

  ierr = lapack::hesv (hcau, ipiv, cbu, cwork);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  // part 2

  swap (row (cal2, 0), row (cal2, 2));
  swap (column (cal2, 0), column (cal2, 2));

  swap (row (cau2, 1), row (cau2, 2));
  swap (column (cau2, 1), column (cau2, 2));

  print_m (cal2, "cal2");
  cout << endl;
  print_m (cau2, "cau2");
  cout << endl;

  cbl2 = prod (cal2, cx);
  cbu2 = prod (cau2, cx);
  print_m (cbl2, "cbl2");
  cout << endl;
  print_m (cbu2, "cbu2");
  cout << endl;

  ierr = lapack::hesv ('L', cal2, cbl2);
  if (ierr == 0)
    print_m (cbl2, "cxl2");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::hesv ('U', cau2, ipiv, cbu2, cwork);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu2, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

}


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

// Returns n!
cpp_int factorial(cpp_int n) {
	if(n == 0) {
		return 1;
	}
	return n * factorial(n - 1);
}

// Returns the sum of the factorials of the digits of a number.
cpp_int sumDigitsFactorial(cpp_int n) {
	cpp_int sum = 0;
	while(n) {
		sum += factorial(n % 10);
		n /= 10;
	}
	return sum;
}

int main(int argc, char *argv[]) {
	cpp_int sum = 0;
	// Randomly decided to put limit as 100,000 for now.
	for(cpp_int i = 3; i <= 100'000; i++) {
		sum = (i == sumDigitsFactorial(i)) ? sum + i : sum;
	}
	cout << sum << endl;
	return 0;
}


#pragma once
#include <Eigen/Core>
#include <igl/boundary_facets.h>
#include <igl/colon.h>
#include <igl/setdiff.h>
#include <igl/unique.h>

//! Finds the boundary and interior vertices, and evaluates
//! the function g on the boundary vertices.
//!
//! @param[out] u should be of size <number of vertices>. At the end:
//!             u(index) = g(vertices(index))  for each boundary index.
//!
//! @param[out] interiorVertexIndices the list of vertices that are not on
//!                                   the boundary.
//!
//! @param[in] vertices a list of vertices for the mesh
//!
//! @param[in] triangles the elements represented as triangles
//!
//! @param[in] g the boundary value function g.
void setDirichletBoundary(Eigen::VectorXd &      u,
                          Eigen::VectorXi &      interiorVertexIndices,
                          const Eigen::MatrixXd &vertices,
                          const Eigen::MatrixXi &triangles,
                          const std::function<double(double, double)> &g) {
	// Find boundary edges
	Eigen::MatrixXi boundaryEdgeIndices;
	igl::boundary_facets(triangles, boundaryEdgeIndices);

	// Find boundary vertices
	Eigen::VectorXi boundaryVertexIndices, IA, IC;
	igl::unique(boundaryEdgeIndices, boundaryVertexIndices, IA, IC);

	for (int i = 0; i < boundaryVertexIndices.size(); ++i) {
		const int   index = boundaryVertexIndices(i);
		const auto &x     = vertices.row(index);
		u(index)          = g(x(0), x(1));
	}

	Eigen::VectorXi allIndices;
	igl::colon<int>(0, vertices.rows() - 1, allIndices);
	igl::setdiff(allIndices, boundaryVertexIndices, interiorVertexIndices, IA);
}


//
// Created by philipp on 22.12.19.
//

#ifndef FUNNELS_CPP_LYAPUNOV_HH
#define FUNNELS_CPP_LYAPUNOV_HH

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <cmath>

namespace lyapunov{

  template <class DERIVED>
  inline double projected_max_radius(const Eigen::MatrixBase<DERIVED> & C0,
                                     const Eigen::MatrixBase<DERIVED> & C1){
    // todo optimize for usage of triangular shape
    // use spezialised version to compute smallest eigenvalue only
    auto Cinv = C0.inverse();
    auto P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
    return std::sqrt(max_rad);
  }

  struct lyap_zone_t{};

  class lyapunov_t{
  public:
    virtual bool intersect(const lyap_zone_t & other);
    virtual bool covers(const lyap_zone_t & other);
    virtual bool is_covered(const lyap_zone_t & other);
  };

  struct fixed_ellipsoidal_zone_t{
    virtual const Eigen::VectorXd & x0() const {
      throw std::runtime_error("Virtual");
      return Eigen::Vector2d::Zero();
    };
    virtual const Eigen::MatrixXd & C() const {
      throw std::runtime_error("Virtual");
      return Eigen::Matrix2d::Zero();
    };
  };

  struct fixed_ellipsoidal_zone_copied_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_copied_t( const Eigen::VectorXd & x0,
        const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    Eigen::VectorXd _x0;
    Eigen::MatrixXd _C;
  };

  struct fixed_ellipsoidal_zone_ref_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_ref_t( const Eigen::VectorXd & x0,
                                     const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    const Eigen::VectorXd &_x0;
    const Eigen::MatrixXd &_C;
  };

  // Does zone0 cover zone1
  template<class DIST>
  bool covers_helper(const fixed_ellipsoidal_zone_t &zone0,
                     const fixed_ellipsoidal_zone_t &zone1,
                     DIST &dist){

    // First compute the projected distance
    auto dy = zone0.C()*(dist.cp_vv(zone1.x0(), zone0.x0()));
    double dy_norm = dy.norm(); //l2 norm
    // center out of bounds
    if (dy_norm>=1.){
      return false;
    }

    // Compute radius
    return dy_norm+projected_max_radius(zone0.C(), zone1.C())<=1.;
  }

  template<class DIST>
  class fixed_ellipsoidal_lyap_t:public lyapunov_t{
  public:
    using dist_t = DIST;

    fixed_ellipsoidal_lyap_t(const Eigen::MatrixXd &P,
        double gamma, DIST &dist):
        _gamma(gamma), _dist(dist){
      // Compute cholesky
      Eigen::LLT<Eigen::MatrixXd> llt_pre_comp;
      llt_pre_comp.compute(P);
      _C = llt_pre_comp.matrixU();
    }

    const Eigen::MatrixXd &C() const {
      return _C;
    }
    double gamma() const {
      return _gamma;
    }

    // Conservative intersect
    // If true, the zone may intersect with this
    // If false, the zone does definitively not intersect
    template<class DERIVED>
    bool intersect(const Eigen::MatrixBase<DERIVED> &x0,
        const fixed_ellipsoidal_zone_t &zone) {
      // First compute the projected distance
      auto dy = _C*(_dist.cp_vv(zone.x0(), x0));
      double dy_norm = dy.norm(); //l2 norm

      if (dy_norm <= 1.){
        return true;
      }

      return dy_norm>=1.+projected_max_radius(_C, zone.C());
    }

    // Conservative cover
    // If true, zone is definitively covered by this
    // If false, it may not be covered
    template<class DERIVED>
    bool covers(const Eigen::MatrixBase<DERIVED> &x0, const fixed_ellipsoidal_zone_t &zone) {
      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
      return covers_helper(this_zone, zone, _dist);
    }

    // Conservative cover
    // If true, this is definitively covered by zone
    // If false, it may not be covered
    template<class DERIVED>
    bool is_covered(const Eigen::MatrixBase<DERIVED> &x0, const fixed_ellipsoidal_zone_t &zone) {
      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
      return covers_helper(zone, this_zone, _dist);
    }

    dist_t & dist(){
      return _dist;
    }

  protected:
    Eigen::MatrixXd _C;
    double _gamma;
    DIST &_dist;
  };

}

#endif //FUNNELS_CPP_LYAPUNOV_HH


//StandardDeviation_StandardError.hpp
//purpose: header file for standard deriation
//author: bondxue
//version: 1.0 11/22/2017

#ifndef STANDARDDEVIATION_STANDARDERROR_HPP
#define STANDARDDEVIATION_STANDARDERROR_HPP

#include <iostream>
#include <cmath>
#include <vector>

#include <boost/tuple/tuple.hpp> // include tuple class
#include <boost/tuple/tuple_io.hpp> // include I/O for tuple

using namespace std;
using boost::tuple;

// function to obtain standard deviation and standard error
boost::tuple <double, double> get_SD_SE(vector<double> price_vec, double r, double T)
{
	int price_size = price_vec.size();
	double sum_price = 0.0;
	double square_sum_price = 0.0;

	for (int i = 0; i < price_size; i++)
	{
		sum_price += price_vec[i];
		square_sum_price += pow(price_vec[i], 2);
	}

	double temp = sqrt((square_sum_price - pow(sum_price, 2)/double(price_size))/(double(price_size)-1.0)); // use temp to store the square root part in SD formula
	double SD = temp * exp(-r*T);
	double SE = SD / sqrt(double(price_size));

	return boost::tuple<double, double>(SD, SE);
}

#endif

Part D Monte Carlo Pricing Methods/Exericse D Monte Carlo Pricing Methods/StandardDeviation_StandardError.hpp


#include <Engine/MeshEdit/DeformRBF.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Dense>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

void DeformRBF::Clear() {
	triMesh = nullptr;
	origPos.clear();
}

bool DeformRBF::Init(Ptr<TriMesh> triMesh) {
	if (triMesh == this->triMesh)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::DeformRBF::Init\n"
			"\t""triMesh->GetType() == TriMesh::INVALID\n");
		return false;
	}

	this->triMesh = triMesh;
	origPos = triMesh->GetPositions();
	return true;
}

bool DeformRBF::Run(const std::vector<Constraint> & constraints, const vector<size_t> & updateIndice) {
	if (!triMesh) {
		printf("ERROR::DeformRBF::Run\n"
			"\t""!triMesh\n");
		return false;
	}

	for (auto constraint : constraints) {
		if (constraint.id >= origPos.size()) {
			printf("ERROR::DeformRBF::Run\n"
				"\t""constraint.id >= origPos.size()\n");
			return false;
		}
	}

	// compute RBF matrix
	auto phi = [](float r) -> float {return r*r*r; }; // basis function
	size_t m = constraints.size();
	size_t k = 10;

	// A = -       -
	//     | G   P |
	//     |       |
	//     |  t    |
	//     | P   0 |
	//     -       -
	MatrixXf A(m + k, m + k);

	// G : m x m
	// Gij = gj(ci) = basicFunc(||ci-cj||)
	for (size_t i = 0; i < m; i++) {
		for (size_t j = 0; j < m; j++) {
			auto ci = origPos[constraints[i].id];
			auto cj = origPos[constraints[j].id];
			float norm = pointf3::distance(ci, cj);
			A(i, j) = phi(norm);
		}
	}

	// P : m x k
	// Pij = pj(ci)
	// [p0, p1, ..., p9] is a basis of the space of trivariate quadratic polunomials
	// e[1]. [1, xx, xy, xz, yx, yy, yz, zx, zy, zz]
	for (size_t i = 0; i < m; i++) {
		auto ci = origPos[constraints[i].id];
		A(m + 0, i) = A(i, m + 0) = 1.f;
		A(m + 1, i) = A(i, m + 1) = ci[0] * ci[0];
		A(m + 2, i) = A(i, m + 2) = ci[0] * ci[1];
		A(m + 3, i) = A(i, m + 3) = ci[0] * ci[2];
		A(m + 4, i) = A(i, m + 4) = ci[1] * ci[0];
		A(m + 5, i) = A(i, m + 5) = ci[1] * ci[1];
		A(m + 6, i) = A(i, m + 6) = ci[1] * ci[2];
		A(m + 7, i) = A(i, m + 7) = ci[2] * ci[0];
		A(m + 8, i) = A(i, m + 8) = ci[2] * ci[1];
		A(m + 9, i) = A(i, m + 9) = ci[2] * ci[2];
	}

	// 0 : k x k
	for (size_t i = 0; i < k; i++) {
		for (size_t j = 0; j < k; j++) {
			A(m + i, m + j) = 0.f;
		}
	}

	MatrixXf b(m + k, 3);
	for (int i = 0; i < k; i++)
		b(m + i,0) = b(m + i, 1) = b(m + i, 2) = 0;
	for (int i = 0; i < m; i++) {
		auto bi = constraints[i].pos;
		b(i, 0) = bi[0];
		b(i, 1) = bi[1];
		b(i, 2) = bi[2];
	}

	MatrixXf RBF = A.colPivHouseholderQr().solve(b);

	// compute target pos of vertex
	vector<pointf3> qVec;
	for (auto id : updateIndice) {
		auto pos = origPos[id];
		// compute p
		MatrixXf p(1, m + k);
		for (size_t i = 0; i < m; i++) {
			auto ci = origPos[constraints[i].id];
			p(i) = phi(pointf3::distance(ci, pos));
		}
		p(0, m + 0) = 1;
		p(0, m + 1) = pos[0] * pos[0];
		p(0, m + 2) = pos[0] * pos[1];
		p(0, m + 3) = pos[0] * pos[2];
		p(0, m + 4) = pos[1] * pos[0];
		p(0, m + 5) = pos[1] * pos[1];
		p(0, m + 6) = pos[1] * pos[2];
		p(0, m + 7) = pos[2] * pos[0];
		p(0, m + 8) = pos[2] * pos[1];
		p(0, m + 9) = pos[2] * pos[2];

		// compute q
		MatrixXf q = p * RBF;
		triMesh->GetPositions()[id] = { q(0,0),q(0,1),q(0,2) };
	}

	return true;
}

void DeformRBF::Restore() {
	if (triMesh == nullptr)
		return;

	triMesh->Update(origPos);
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <map>

namespace fluid {
namespace algorithm {

class WindowFuncs
{
public:
  enum class WindowTypes {
    kHann,
    kHannD,
    kHamming,
    kBlackmanHarris,
    kGaussian
  };
  using WindowFuncsMap =
      std::map<WindowTypes,
               std::function<void(index, Eigen::Ref<Eigen::ArrayXd>)>>;

  static WindowFuncsMap& map()
  {
    using namespace std;
    static WindowFuncsMap _funcs = {
        {WindowTypes::kHann,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           { out(i) = 0.5 - 0.5 * cos((pi * 2 * i) / size); }
         }},
        {WindowTypes::kHannD,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           double norm = pi / size;
           for (index i = 0; i < size; i++)
           { out(i) = norm * sin((2 * pi * i) / size); }
         }},
        {WindowTypes::kHamming,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           { out(i) = 0.54 - 0.46 * cos((pi * 2 * i) / size); }
         }},
        {WindowTypes::kBlackmanHarris,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           {
             out(i) = 0.35875 - 0.48829 * cos((pi * 2 * i) / size) +
                      0.14128 * cos((pi * 2 * i) / size) +
                      0.01168 * cos((pi * 2 * i) / size);
           }
         }},
        {WindowTypes::kGaussian,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           double sigma = size / 3; // TODO: should be argument
           assert(size % 2);
           index h = (size - 1) / 2;
           for (index i = -h; i <= h; i++)
           { out(i + h) = exp(-i * i / (2 * sigma * sigma)); }
         }}};
    return _funcs;
  }
};
} // namespace algorithm
} // namespace fluid


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/autodif.hpp>

namespace ub = boost::numeric::ublas;

// f: R->R
kv::autodif<double> testfunc1(const kv::autodif<double>& x) {
	kv::autodif<double> tmp;

	tmp = x * x - 1.;
	return cos(tmp + sin(tmp));
}

// f: R->R (use template)
template <class T> T testfunc2(const T& x) {
	T tmp;

	tmp = x * x - 1.;
	return cos(tmp + sin(tmp));
}

// f: R^n->R
template <class T> T testfunc3(const ub::vector<T>& x) {
	return atan(x(0) * x(0) + 2. * x(1) * x(1) - 1.);
}

// f: R->R^n
template <class T> ub::vector<T> testfunc4(const T& x) {
	ub::vector<T> y(2);

	y(0) = sin(x);
	y(1) = cos(x);

	return y;
}

// f: R^n->R^n
template <class T> ub::vector<T> testfunc5(const ub::vector<T>& x) {
	ub::vector<T> y(2);

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

	return y;
}

int main()
{
	double d1, d2;
	kv::autodif<double> a1, a2, a3;
	ub::vector<double> v1, v2;
	ub::vector< kv::autodif<double> > va1, va2, va3, va4, va5;
	ub::matrix<double> m;
	int i;

	//
	// f: R->R (testfunc1)
	//

	// helper function to initialize autodif variable with single variable
	// same as " a1.v = 1.5; a1.d(0) = 1.; "
	a1 = kv::autodif<double>::init(1.5);

	a2 = testfunc1(a1);

	// helper function to separate autodif variable into value and gradient
	// same as " d1 = a2.v; d2 = a2.d(0); "
	kv::autodif<double>::split(a2, d1, d2);

	std::cout << "testfunc1\n";
	std::cout << d1 << "\n"; // f(1.5)
	std::cout << d2 << "\n"; // f'(1.5)

	//
	// f: R->R (testfunc2: use template)
	//

	a1 = kv::autodif<double>::init(1.5);
	a2 = testfunc2(a1);

	kv::autodif<double>::split(a2, d1, d2);

	std::cout << "testfunc2\n";
	std::cout << d1 << "\n"; // f(1.5)
	std::cout << d2 << "\n"; // f'(1.5)

	d1 = 1.5;
	d2 = testfunc2(d1); // double can be also passed
	std::cout << d2 << "\n"; // f(1.5)

	//
	// f: R^n->R
	//

	v1.resize(2);
	v1(0) = 5.; v1(1) = 6.;

	// helper function to initialize autodif vector with vector variable
	// same as " va1(0).v = 5.; va1(0).d(0) = 1.; va1(0).d(1) = 0,; "
	//         " va1(1).v = 6.; va1(1).d(0) = 0.; va1(1).d(1) = 1,; "
	va1 = kv::autodif<double>::init(v1);

	a1 = testfunc3(va1);

	// helper function to separate autodif variable into value and gradient
	// same as " y = a1.v; v2(0) = a1.d(0); v2(1) = a1.d(1) "
	kv::autodif<double>::split(a1, d1, v2);

	std::cout << "testfunc3\n";
	std::cout << d1 << "\n"; // f(5, 6)
	std::cout << v2 << "\n"; // gradient

	//
	// f: R->R^n
	//

	a1 = kv::autodif<double>::init(1.5);

	va1 = testfunc4(a1);

	// helper function to separate autodif variable into value and gradient
	// same as " v1(0) = va1(0).v; v1(1) = va1(1).v; "
	//         " v2(0) = va1(0).d(0); v2(1) = va1(1).d(0); "
	kv::autodif<double>::split(va1, v1, v2);

	std::cout << "testfunc4\n";
	std::cout << v1 << "\n"; // f(1.5)
	std::cout << v2 << "\n"; // f'(1.5)

	//
	// f: R^n->R^m
	//

	v1(0) = 5.; v1(1) = 6.;

	va1 = kv::autodif<double>::init(v1);

	va2 = testfunc5(va1);

	// helper function to separate autodif vector into vector value and jacobian
	// same as " v2(0) = va2(0).v; m(0)(0) = va2(0).d(0); m(0)(1) = va2(0).d(1); "
	//         " v2(1) = va2(1).v; m(1)(0) = va2(1).d(0); m(1)(1) = va2(1).d(1); "
	kv::autodif<double>::split(va2, v2, m);

	std::cout << "testfunc5\n";
	std::cout << v2 << "\n"; // f(5, 6)
	std::cout << m << "\n"; // Jacobian matrix

	//
	// operations for autodif
	//

	v1(0) = 0.7; v1(1) = 0.8;
	va1 = kv::autodif<double>::init(v1);
	a1 = va1(0);
	a2 = va1(1);

	std::cout << "autodif operations\n";

	std::cout << a1 << "\n";
	std::cout << a2 << "\n";

	a3 = a1 + a2; std::cout << a3 << "\n";
	a3 = a1 - a2; std::cout << a3 << "\n";
	a3 = a1 * a2; std::cout << a3 << "\n";
	a3 = a1 / a2; std::cout << a3 << "\n";
	a3 = pow(a1, 3); std::cout << a3 << "\n";
	a3 = pow(a1, a2); std::cout << a3 << "\n";
	a2 = sqrt(a1); std::cout << a2 << "\n";
	a2 = exp(a1); std::cout << a2 << "\n";
	a2 = log(a1); std::cout << a2 << "\n";
	a2 = sin(a1); std::cout << a2 << "\n";
	a2 = cos(a1); std::cout << a2 << "\n";
	a2 = tan(a1); std::cout << a2 << "\n";
	a2 = sinh(a1); std::cout << a2 << "\n";
	a2 = cosh(a1); std::cout << a2 << "\n";
	a2 = tanh(a1); std::cout << a2 << "\n";
	a2 = asin(a1); std::cout << a2 << "\n";
	a2 = acos(a1); std::cout << a2 << "\n";
	a2 = atan(a1); std::cout << a2 << "\n";
	// asinh, acosh, atanh may not compile for autodif<double>
	// because c++03 does not have asinh, acosh, atanh
	// a2 = asinh(a1); std::cout << a2 << "\n";
	// a2 = acosh(a1+1.); std::cout << a2 << "\n";
	// a2 = atanh(a1); std::cout << a2 << "\n";

	//
	// compress / expand
	//

	v1.resize(20);
	for (i=0; i<20; i++) v1(i) = i + 2.;
	va1 = kv::autodif<double>::init(v1);
	// use only a few of the many variables
	va2.resize(2);
	va2(0) = va1(10);
	va2(1) = va1(11);

	// usual way
	std::cout << "usual input\n";
	std::cout << va2 << "\n";
	va3 = testfunc5(va2);
	std::cout << "usual output\n";
	std::cout << va3 << "\n";

	// using compress/expand to save calculation time
	ub::matrix<double> save;
	va3 = kv::autodif<double>::compress(va2, save);
	std::cout << "compressed input\n";
	std::cout << va3 << "\n";
	va4 = testfunc5(va3);
	std::cout << "compressed output\n";
	std::cout << va4 << "\n";
	va5 = kv::autodif<double>::expand(va4, save);
	std::cout << "expanded output\n";
	std::cout << va5 << "\n";
}


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

Eigen::VectorXd poly(int const n, double & x){
	Eigen::VectorXd v(n);
	v(0)=1;
	if(n>1) v(1)=x;
	for (int i=2; i < n;i++){
		v(i)=2*x*v(i-1)-v(i-2);
	}
	return v;
}

Eigen::VectorXd zeroes(const int n){
	Eigen::VectorXd v(n);
	for (int i=0;i<n;i++){
		double temp=((2.*i-1.)/n*2)*M_PI;
		v(i)=cos(temp);
	}
	return v;
}

template <typename Function>
void bestpolchebnodes (const Function&f, Eigen::VectorXd &alpha){
		size_t n= alpha.size();
		Eigen::VectorXd v;
		Eigen::VectorXd fn(n+1);
		for (int i=0; i<n+1; i++){
			double temp=cos(M_PI*(2*k+1)/2/(n+1));
			fn(i)=f(temp);
		}
		Eigen::MatrixXd scal(n+1,n+1);
		for (int j=0; j<n+1; j++) {
			v=poly(n,cos(M_PI*(2*j+1)/2/(n+1)));
			for (int k=0; k<n+1; k++) scal(j,k)=V[k];
		}
		for (int l=0;l<n;l++){
			alpha(l)=0;
			for (int k=0;k<n;k++){
				alpha(l)+=2*(1./(n+1))*fn(k)*v(k);
			}
		}
		alpha(0)=alpha(0)/2;

}
using namespace std;
int main(){
    auto f = [] (double & x) {return 1/(pow(5*x,2)+1);};
    int n=20;
    Eigen::VectorXd alpha(n+1);
    bestpolchebnodes(f, alpha);

    //Compute the error
    Eigen::VectorXd X = Eigen::VectorXd::LinSpaced(1e6,-1,1);
    auto qn = [&alpha,&n] (double & x) {
        double temp;
        vector<double> V=poly(n,x);
        for (int k=0; k<n+1; k++) temp+=alpha(k)*V[k];
        return temp;
    };
    double err_max=abs(f(X(0))-qn(X(0)));
    for (int i=1; i<1e6; i++) err_max=std::max(err_max,abs(f(X(i))-qn(X(i))));
    cout<<"Error: "<< err_max <<endl;
}

Nummerical Methods for CSE/PS9/chebychev.cpp


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/normal.hpp>

using namespace Eigen;
using std::endl;
using std::cout;

template<typename T>
class IW : public Distribution<T>
{
public:
  Matrix<T,Dynamic,Dynamic> Delta_;
  T nu_;
  uint32_t D_;

  IW(const Matrix<T,Dynamic,Dynamic>& Delta, T nu, boost::mt19937 *pRndGen);
  IW(const Matrix<T,Dynamic,Dynamic>& Delta, T nu, const Matrix<T,Dynamic,Dynamic>& scatter,
	 const Matrix<T,Dynamic,1>& mean, T counts, boost::mt19937 *pRndGen);
  IW(const IW& iw);
  ~IW();

  IW<T>* copy();

  IW<T> posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
      uint32_t k, uint32_t zDivider=1);
  IW<T> posterior(const Matrix<T,Dynamic,Dynamic>& outer, T count) const;
  IW<T> posterior() const ;
  void resetSufficientStatistics();

  Matrix<T,Dynamic,Dynamic> sample();
//  Normal<T> sample();
  T logPdf(const Matrix<T,Dynamic,Dynamic>& Sigma) const;
  T logPdf(const Normal<T>& normal) const;
  T logLikelihoodMarginalized() const; // log pdf of SS under NIW prior
  T logLikelihoodMarginalized(const Matrix<T,Dynamic,Dynamic>& Scatter,
    T count) const;

  Matrix<T,Dynamic,Dynamic> mode() const;

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return scatter_;};
  Matrix<T,Dynamic,Dynamic>& scatter() {return scatter_;};
  const Matrix<T,Dynamic,1>& mean() const {return mean_;};
  Matrix<T,Dynamic,1>& mean() {return mean_;};
  T count() const {return count_;};
  T& count() {return count_;};

private:
  boost::random::normal_distribution<> gauss_;

  // sufficient statistics
  Matrix<T,Dynamic,Dynamic> scatter_;
  Matrix<T,Dynamic,1> mean_;
  T count_;
};

typedef IW<double> IWd;
typedef IW<float> IWf;

/* prior for spherical covariances (i.e. \Sigma = \sigma^2 I) */
template<typename T>
class IW_spherical : public Distribution<T>
{
public:
  T delta_;
  T nu_;
  uint32_t D_;

  IW_spherical(T delta, T nu, uint32_t D, boost::mt19937 *pRndGen);
  IW_spherical(const IW_spherical& iw);
  ~IW_spherical();

  IW_spherical<T>* copy();

  IW_spherical<T> posterior(const Matrix<T,Dynamic,Dynamic>& x,
      const VectorXu& z, uint32_t k, uint32_t zDivider=1);
//  T sample();
//  T logPdf(T sigma);
////  T logPdf(const Normal& normal);

private:
  boost::random::normal_distribution<> gauss_;
};

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


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

#include <Eigen/SVD>

namespace kindr {

/*!
 * \brief Gets a skew-symmetric matrix from a (column) vector
 * \param   vec 3x1-matrix (column vector)
 * \return skew   3x3-matrix
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 3> getSkewMatrixFromVector(const Eigen::Matrix<PrimType_, 3, 1>& vec) {
  Eigen::Matrix<PrimType_, 3, 3> mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

/*!
 * \brief Gets a 3x1 vector from a skew-symmetric matrix
 * \param   matrix 3x3-matrix
 * \return  column vector (3x1-matrix)
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 1> getVectorFromSkewMatrix(const Eigen::Matrix<PrimType_, 3, 3>& matrix) {
  return Eigen::Matrix<PrimType_, 3, 1> (matrix(2,1), matrix(0,2), matrix(1,0));
}

/*!
 * \brief Computes the Moore–Penrose pseudoinverse
 * info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
 * \param a: Matrix to invert
 * \param result: Result is written here
 * \param epsilon: Numerical precision (for example 1e-6)
 * \return true if successful
 */
template<typename _Matrix_TypeA_, typename _Matrix_TypeB_>
bool static pseudoInverse(const _Matrix_TypeA_ &a, _Matrix_TypeB_ &result,
                          double epsilon = std::numeric_limits<typename _Matrix_TypeA_::Scalar>::epsilon())
{
  // Shorthands
  constexpr auto rowsA = static_cast<int>(_Matrix_TypeA_::RowsAtCompileTime);
  constexpr auto colsA = static_cast<int>(_Matrix_TypeA_::ColsAtCompileTime);
  constexpr auto rowsB = static_cast<int>(_Matrix_TypeB_::RowsAtCompileTime);
  constexpr auto colsB = static_cast<int>(_Matrix_TypeB_::ColsAtCompileTime);

  // Assert wrong matrix types
  static_assert(std::is_same<typename _Matrix_TypeA_::Scalar, typename _Matrix_TypeB_::Scalar>::value,
                "[kindr::pseudoInverse] Matrices must be of the same Scalar type!");
  static_assert(rowsA == colsB && colsA == rowsB, "[kindr::pseudoInverse] Result type has wrong size!");

  // If one dimension is dynamic, compute everything as dynamic size
  constexpr auto m = Eigen::JacobiSVD< _Matrix_TypeA_ >::DiagSizeAtCompileTime;

  // JacobiSVD needs to be computed on dynamic size if we need ComputeThinU, ComputeThinV, see:
  // https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html
  Eigen::JacobiSVD< Eigen::Matrix<typename _Matrix_TypeA_::Scalar, Eigen::Dynamic, Eigen::Dynamic> > svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);

  typename _Matrix_TypeA_::Scalar tolerance =
    epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  // Sigma for ThinU and ThinV
  Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, m> sigmaThin = Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, 1>(
    (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal();

  result = svd.matrixV() * sigmaThin * svd.matrixU().transpose();

  return true;
}

} // end namespace kindr


/*
 * Using Sundaram' sieve, print the first 2,000,000 numbers
 * darkturo 2014
 */
#include <iostream>
#include <fstream>
#include <bitset>
#include <ctime>
#include <limits>
#include <boost/spirit/include/karma.hpp>

#define TIMEDIFF(start, stop) 1000.0 * (stop - start)/CLOCKS_PER_SEC
#define MAX 32452843
#define MAXPRIMES 2000000
#define BUFFER_SIZE 8l * MAX

class SundaramSieve
{
   std::bitset<MAX> N;
   std::ofstream output;
   int counter;
   char * buffer;
   char * p_input;

   public:
   SundaramSieve() :
           output("primesEveryWhere.txt", std::ios::out | std::ios::trunc),
           counter(0)
   {
      buffer = new char[BUFFER_SIZE];
      p_input = buffer;
      N.set();
   }

   ~SundaramSieve()
   {
      if (p_input != buffer)
         output.write(buffer, p_input - buffer);  // flush
      output.close();
   }

   void sieve()
   {
      // Given i,j ∈ ℕ, 1 ≤ i ≤ j
      // Sieve all the x ∈ ℕ, x = i + j + 2ij ≤ MAX
      for (long i = 1; (2*i + 2*(i^2)) <= 2*sqrt(MAX); i++)
      {
         for (long j = i; (i + j + 2*i*j) <= (MAX - 2)/2; j++)
         {
            N.set((i + j + 2*i*j) - 1, false);
         }
      }

      // Generate the primes from the sieved list.
      print( 2 );
      for (int i = 1; counter < MAXPRIMES; i++)
      {
         if (N[i-1])
         {
            print( 2*i + 1 );
         }
      }
   }

   int printedPrimes()
   {
      return counter;
   }

   private:
   inline void print(int number)
   {
      doPrint(number);
      counter ++;
   }

   inline void doPrint(int number)
   {
      boost::spirit::karma::generate(p_input, boost::spirit::int_, number);
      *p_input++ = '\n';
   }
};

int main(int argc, char ** argv)
{
   clock_t t1, t2;

   t1 = std::clock();

   SundaramSieve siever;
   siever.sieve();

   t2 = std::clock();

   // Summary
   std::cout.precision(std::numeric_limits<double>::digits10);
   std::cerr << "Used " << std::fixed << TIMEDIFF(t1, t2)
             << " msecs to calculate " << siever.printedPrimes()
             << " primes." << std::endl;
}


#pragma once

#include "modprop/compo/Interfaces.h"
#include "modprop/compo/Parametric.hpp"
#include <Eigen/Dense>

namespace percepto
{

// TODO Support matrix inputs?
/*! \brief A simple linear regression class. Outputs Weights matrix * input.
 * The matrix and input are dynamically-sized. Assumes row-major ordering
 * for vectorizing the weights matrix. */
class LinearRegressor
: public Source<VectorType>
{
public:

	typedef VectorType InputType;
	typedef VectorType OutputType;
	typedef Source<VectorType> SourceType;

	LinearRegressor( unsigned int inputDim, unsigned int outputDim )
	: _inputDim( inputDim ), _outputDim( outputDim ), _W( nullptr, 0, 0 ),
	_inputPort( this ) {}

	LinearRegressor( const LinearRegressor& other )
	: _inputDim( other._inputDim ), _outputDim( other._outputDim ),
	_params( other._params ), _W( other._W ), _inputPort( this ) {}

	void SetSource( SourceType* b ) { b->RegisterConsumer( &_inputPort ); }

	unsigned int InputDim() const { return _inputDim; }
	unsigned int OutputDim() const { return _outputDim; }

	Parameters::Ptr CreateParameters()
	{
		Parameters::Ptr params = std::make_shared<Parameters>();
		params->Initialize( (_inputDim + 1) * _outputDim );
		SetParameters( params );
		return params;
	}

	void SetParameters( Parameters::Ptr params )
	{
		if( params->ParamDim() != (_inputDim+1) * _outputDim )
		{
			throw std::runtime_error( "LinearRegressor: Invalid parameter dimension." );
		}
		_params = params;
		new (&_W) Eigen::Map<const RowMatrixType>( params->GetParamsVec().data(),
		                                        _outputDim, _inputDim + 1 );
	}

	void SetOffsets( const VectorType& off )
	{
		RowMatrixType p( _W );
		if( off.size() != p.rows() )
		{
			throw std::runtime_error( "LinearRegressor: Offsets invalid dimension." );
		}
		p.col( p.cols() - 1 ) = off;
		Eigen::Map<VectorType> v( p.data(), p.size(), 1 );
		_params->SetParamsVec( v );
		new (&_W) Eigen::Map<const RowMatrixType>( _params->GetParamsVec().data(),
		                                              _outputDim, _inputDim + 1 );
	}

	virtual void Foreprop()
	{
		VectorType out = _W * _inputPort.GetInput().homogeneous();
		SourceType::SetOutput( out );
		SourceType::Foreprop();
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		MatrixType nDodx;
		if( nextDodx.size() == 0 )
		{
			nDodx = MatrixType::Identity( OutputDim(), OutputDim() );
		}
		else
		{
			nDodx = nextDodx;
		}

		if( nDodx.cols() != OutputDim() )
		{
			throw std::runtime_error( "LinearRegressor: Backprop dim error!" );
		}

		const VectorType& input = _inputPort.GetInput();
		MatrixType thisDodw = MatrixType::Zero( nDodx.rows(), ParamDim() );
		for( unsigned int i = 0; i < nDodx.rows(); i++ )
		{
			for( unsigned int j = 0; j < OutputDim(); j++ )
			{
				thisDodw.block(i, j*(InputDim()+1), 1, InputDim()+1) =
					nDodx(i,j) * input.homogeneous().transpose();
			}
		}
		MatrixType thisDodx = nDodx * _W;

		if( !SourceType::modName.empty() )
		{
			std::cout << SourceType::modName << " dodx: " << nDodx << std::endl << "dodw: " << thisDodw << std::endl;
			std::cout << "acc dodw: " << _params->GetDerivs() << std::endl;
			std::cout << "input: " << input.transpose() << std::endl;
			std::cout << "param dim: " << ParamDim() << std::endl;
		}

		_params->AccumulateDerivs( thisDodw );
		_inputPort.Backprop( thisDodx );
	}

	virtual unsigned int ParamDim() const { return _W.size(); }

	const Eigen::Map<const RowMatrixType>& GetWeights() const { return _W; }

private:

	friend std::ostream& operator<<( std::ostream& os, const LinearRegressor& lreg );

	unsigned int _inputDim;
	unsigned int _outputDim;
	Parameters::Ptr _params;
	Eigen::Map<const RowMatrixType> _W; // The weight vector
	Sink<VectorType> _inputPort;

};

inline
std::ostream& operator<<( std::ostream& os, const LinearRegressor& lreg )
{
	os << lreg._W;
	return os;
}

}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

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

#ifndef EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP
#define EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP

#include <boost/random/uniform_real_distribution.hpp>
// boost::math::uniform is not used as it is linked to real values rather than discrete integral values

namespace rjmcmc {

    // 1_(n\in[a,b])/(b-a+1)
    class even_numbered_order_statistics_uniform_distribution {
    public:
        typedef double real_type;
        typedef int    int_type;
        typedef boost::random::uniform_real_distribution<real_type> rand_distribution_type;

        even_numbered_order_statistics_uniform_distribution(int_type k , real_type a, real_type b)
            : m_rand(a,b)
            , m_size(k) {}

        // new/old
        template<typename Iterator> real_type pdf_ratio(Iterator begin0, Iterator end0, Iterator begin1, Iterator end1) const
        {
            return pdf(begin1,end1)/pdf(begin0,end0);
        }

        template<typename Iterator> real_type pdf(Iterator begin, Iterator end) const
        {
            typedef typename std::iterator_traits<Iterator>::value_type T;
            const T a = m_rand.min();
            const T b = m_rand.max();
            const T s = b - a;
            T x = a;
            T res(1);
            int i = 2;
            for(Iterator it=begin; it != end; ++it, i+=2)
            {
                const T y = *it;
                if(y<x) return 0;
                res*=(y-x)*i*(i+1)/(s*s);
                x=y;
            }
            if(b<x) return 0;
            res *= (b - x) / s;
            return res;
        }

        template<typename Engine, typename Iterator>
        inline void operator()(Engine& e, Iterator out) const {
            std::vector<real_type> v(2*m_size+1);
            for(std::vector<real_type>::iterator it = v.begin(); it!=v.end(); ++it)
                *it = m_rand(e);
            std::sort(v.begin(),v.end());
            for(std::vector<real_type>::iterator it = ++v.begin(); it!=v.end(); it+=2)
                *out++ = *it;
        }

    private:
        mutable rand_distribution_type m_rand;
        int_type m_size;
    };

}; // namespace rjmcmc

#endif // EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP


#ifndef FVMSCALAR1D_RECONSTRUCTION_HPP
#define FVMSCALAR1D_RECONSTRUCTION_HPP

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

inline double sign(double a) { return copysign(1.0, a); }

//// ANCSE_CUT_START_TEMPLATE
//----------------SlopeLimiterABegin----------------
inline double minmod(double a, double b) {
    return 0.5 * (sign(a) + sign(b)) * std::min(std::abs(a), std::abs(b));
}

inline double maxmod(double a, double b) {
    return 0.5 * (sign(a) + sign(b)) * std::max(std::abs(a), std::abs(b));
}

inline double minmod(double a, double b, double c) {
    return minmod(a, minmod(b, c));
}

inline double minabs(double a, double b) {
    double tolerance = 1e-10;
    if (std::abs( std::abs(a) - std::abs(b) ) < tolerance)
        return 0.5*(a+b);
    return std::abs(a) < std::abs(b) ? a : b;
}
//----------------SlopeLimiterAEnd----------------

//----------------SlopeLimiterBBegin----------------
struct MinMod {
    inline double operator()(double sL, double sR) const {
        return minmod(sL, sR);
    }
};
//----------------SlopeLimiterBEnd----------------

//----------------SlopeLimiterCBegin----------------
struct MinAbs {
    inline double operator()(double sL, double sR) const {
        return minabs(sL, sR);
    }
};

struct SuperBee {
    inline double operator()(double sL, double sR) const {
        double A = minmod(2.0 * sL, sR);
        double B = minmod(sL, 2.0 * sR);

        return maxmod(A, B);
    }
};

struct VanLeer {
    inline double operator()(double sL, double sR) const {
        double r = sL / (sR + eps);
        return (r + std::abs(r)) / (1 + std::abs(r)) * sR;
    }

  private:
    double eps = 1e-10;
};

struct MonotonizedCentral {
    inline double operator()(double sL, double sR) const {
        return minmod(2.0 * sL, 0.5 * (sL + sR), 2.0 * sR);
    }
};

struct Unlimited {
    inline double operator()(double a, double b) const { return 0.5 * (a + b); }
};
//----------------SlopeLimiterCEnd----------------
//// ANCSE_END_TEMPLATE

class PWConstantReconstruction {
  public:
    /// Compute the left and right trace at the interface i + 1/2.
    /** Note: This API is agnostic to the number of cell-averages required
     *        by the method. Therefore, reconstructions with different stencil
     *        sizes can implement this API; and this call can be used in parts
     *        of the code that do not need to know about the details of the
     *        reconstruction.
     */
    inline std::pair<double, double> operator()(const Eigen::VectorXd &u,
                                                int i) const {
        return (*this)(u[i], u[i + 1]);
    }

    /// Compute the left and right trace at the interface.
    /** Piecewise constant reconstruction of the left and right trace only
     *  requires the cell-average to the left and right of the interface.
     *
     *  Note: Compared to the other overload this reduces the assumption on
     *        how the cell-averages are stored. This is useful when testing and
     *        generally makes the function useful in more situations.
     */
    inline std::pair<double, double> operator()(double ua, double ub) const {
        return {ua, ub};
    }
};

//// ANCSE_CUT_START_TEMPLATE
//----------------LinearRCBegin----------------
template <class SlopeLimiter>
class PWLinearReconstruction {
  public:
    explicit PWLinearReconstruction(const SlopeLimiter &slope_limiter)
        : slope_limiter(slope_limiter) {}

    std::pair<double, double> operator()(const Eigen::VectorXd &u,
                                         int i) const {
        return (*this)(u[i - 1], u[i], u[i + 1], u[i + 2]);
    }

    std::pair<double, double>
    operator()(double ua, double ub, double uc, double ud) const {
        double sL = ub - ua;
        double sM = uc - ub;
        double sR = ud - uc;

        double uL = ub + 0.5 * slope_limiter(sL, sM);
        double uR = uc - 0.5 * slope_limiter(sM, sR);

        return {uL, uR};
    }

  private:
    SlopeLimiter slope_limiter;
};
//----------------LinearRCEnd----------------
//// ANCSE_END_TEMPLATE

#endif // FVMSCALAR1D_RATE_OF_CHANGE_HPP