#include <Eigen/Dense>
#include <iostream>
#include <chrono.h>

void buildSystem(Eigen::MatrixXd& A, Eigen::VectorXd& b, const unsigned int dim) {
    A = Eigen::MatrixXd::Random(dim, dim);
    A.diagonal() += (dim + 1) * Eigen::VectorXd::Ones(dim);

    b = Eigen::VectorXd::Random(dim);
    assert(b.norm() > DBL_EPSILON);
}

Eigen::VectorXd GS(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const unsigned int nbIter) {
    auto x = b;
    for (size_t iter = 0; iter < nbIter; ++iter) {
        for (size_t i = 0; i < A.rows(); i++) {
            double sum = 0.0;
            for (size_t j = 0; j < A.cols(); j++) {
                if (j != i) {
                    sum += A(i, j) * x(j);
                }
                x(i) = (b(i) - sum) / A(i, i);
            }
        }
    }
    return x;
}

int main(int argc, char const *argv[]) {
    const unsigned int arraySize = 5;
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(arraySize, arraySize);
    Eigen::VectorXd b = Eigen::VectorXd::Random(arraySize);
    buildSystem(A, b, A.size());

    TP_CPP_IMAC2::Chrono chrono;

    // Jacobi
    chrono.start();
    Eigen::VectorXd x = GS(A, b, 28);
    chrono.stop();
    std::cout << "Jacobi : " << chrono.timeSpan() << std::endl;

    // Lu
    chrono.start();
    Eigen::PartialPivLU<Eigen::MatrixXd> lu(A);
    x = A * lu.solve(b);
    chrono.stop();
    std::cout << "Lu : " << chrono.timeSpan() << std::endl;

    // QR
    chrono.start();
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(A);
    x = A * qr.solve(b);
    chrono.stop();
    std::cout << "QR : " << chrono.timeSpan() << std::endl;

    // SVD
    chrono.start();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    x = A * svd.solve(b);
    chrono.stop();
    std::cout << "SVD : " << chrono.timeSpan() << std::endl;

    return 0;
}


#include <iostream>
#include <math.h>
#include <Eigen/Dense>

using namespace std;

struct Vertex {
    double x, y, z;
};

int main()
{
    Vertex v1, v2, v3;
    cin >> v1.x >> v1.y >> v1.z
        >> v2.x >> v2.y >> v2.z
        >> v3.x >> v3.y >> v3.z;

    Eigen::Vector3d vec12(v2.x-v1.x, v2.y-v1.y, v2.z-v1.z);
    Eigen::Vector3d vec23(v3.x-v2.x, v3.y-v2.y, v3.z-v2.z);

    Eigen::Vector3d veccross = vec12.cross(vec23);
    // veccross /= sqrt(veccross.array().pow(2).sum());
    veccross.normalize();

    Eigen::Vector4d faceEqn;
    faceEqn << veccross, -veccross[0]*v1.x -veccross[1]*v1.y -veccross[2]*v1.z;

    cout << veccross << endl << faceEqn << endl;

}


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

Eigen::Matrix3d OthMatrix(const Eigen::Matrix3d& input) {
    Eigen::Quaterniond q(input);
    q.normalize();
    return q.toRotationMatrix();
}

// 解决如下问题
// 1. Affine3d, Isomtry3d,  Matrix4d表示同一个欧式变换时，求逆是否不同
// 从输出可以看出来结果一致
int main(int argv, char** argc) {
    double theta_z = M_PI * 60 / 180;
    double theta_y = M_PI * 30 / 180;
    double theta_x = M_PI * 40 / 180;
    Eigen::Matrix3d rz =
        Eigen::AngleAxisd(theta_z, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    Eigen::Matrix3d ry =
        Eigen::AngleAxisd(theta_y, Eigen::Vector3d::UnitY()).toRotationMatrix();
    Eigen::Matrix3d rx =
        Eigen::AngleAxisd(theta_x, Eigen::Vector3d::UnitX()).toRotationMatrix();

    Eigen::Quaterniond rotation(rz * ry * rx);
    std::cout << rotation.matrix() << std::endl;
    Eigen::Translation3d t(1, 2, 3);
    Eigen::Affine3d matrix_affine = t * rotation;
    std::cout <<"matrix_affine : \n" << matrix_affine.matrix() << std::endl;

    Eigen::Isometry3d isometry3d(t * rotation);
    std::cout << "isometry3 : \n" << isometry3d.matrix() << std::endl;

    Eigen::Matrix4d m4d = matrix_affine.matrix();
    std::cout << "matrix4d : \n" << m4d.matrix() << std::endl;

    std::cout <<"matrix_affine inverse: \n" <<  matrix_affine.inverse().matrix() << std::endl;
    std::cout << "matrix4d inverse : \n" << m4d.inverse() << std::endl;
    std::cout << "isometry3 inverse : \n" << isometry3d.inverse().matrix() << std::endl;

}


#include <iostream>
#include <type.hpp>

#include <Eigen/Eigen>

#include "Geometry/ParameterDesign/Interpolation.h"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

class InterpolationVisualizer :public Visualizer
{
protected:
	void AddPoint();
	void DragPoint();
	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;

	void evaluate_lagrangian()
	{
		BSplineInterpolation lagrangian(points);
		lagrangian.evaluate();

		RadialInterpolation radial(points);
		radial.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			ys1[i] = lagrangian(xs[i]);
			ys2[i] = radial(xs[i]);
		}
	}

public:
	InterpolationVisualizer() {
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = i;
		}
	}
	const float Length = 1001;

	std::vector<float> xs = std::vector<float>(Length);
	std::vector<float> ys1 = std::vector<float>(Length, 0);
	std::vector<float> ys2 = std::vector<float>(Length, 0);
};

void DrawRadial()
{
}

void InterpolationVisualizer::AddPoint()
{
	if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	{
		auto pos = ImPlot::GetPlotMousePos();
		points.emplace_back(pos.x, pos.y);
		updated = true;
	}
}

void InterpolationVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}

void InterpolationVisualizer::draw(bool* p_open)
{
	if (updated)
	{
		evaluate_lagrangian();
		updated = false;
	}
	if (ImGui::BeginTabBar("Homework 1")) {
		if (ImGui::BeginTabItem("Interpolation"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail(), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("Polynomial", &xs[0], &ys1[0], Length);
				AddPoint();
				DragPoint();

				ImPlot::PlotLine("Radial", &xs[0], &ys2[0], Length);

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}

		ImGui::EndTabBar();
	}
	ImGui::End();
}

int main()
{
	InterpolationVisualizer visualizer;
	visualizer.RenderLoop();
}


/*
 * two_dimensional_phase_lattice.cpp
 *
 * This example show how one can use matrices as state types in odeint.
 *
 * Copyright 2009-2012 Karsten Ahnert and Mario Mulansky.
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <map>
#include <string>
#include <fstream>

#ifndef M_PI //not there on windows
#define M_PI 3.1415927 //...
#endif

#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ two_dimensional_phase_lattice_definition
typedef boost::numeric::ublas::matrix< double > state_type;

struct two_dimensional_phase_lattice
{
    two_dimensional_phase_lattice( double gamma = 0.5 )
    : m_gamma( gamma ) { }

    void operator()( const state_type &x , state_type &dxdt , double /* t */ ) const
    {
        size_t size1 = x.size1() , size2 = x.size2();

        for( size_t i=1 ; i<size1-1 ; ++i )
        {
            for( size_t j=1 ; j<size2-1 ; ++j )
            {
                dxdt( i , j ) =
                        coupling_func( x( i + 1 , j ) - x( i , j ) ) +
                        coupling_func( x( i - 1 , j ) - x( i , j ) ) +
                        coupling_func( x( i , j + 1 ) - x( i , j ) ) +
                        coupling_func( x( i , j - 1 ) - x( i , j ) );
            }
        }

        for( size_t i=0 ; i<x.size1() ; ++i ) dxdt( i , 0 ) = dxdt( i , x.size2() -1 ) = 0.0;
        for( size_t j=0 ; j<x.size2() ; ++j ) dxdt( 0 , j ) = dxdt( x.size1() -1 , j ) = 0.0;
    }

    double coupling_func( double x ) const
    {
        return sin( x ) - m_gamma * ( 1.0 - cos( x ) );
    }

    double m_gamma;
};
//]

struct write_for_gnuplot
{
    size_t m_every , m_count;

    write_for_gnuplot( size_t every = 10 )
    : m_every( every ) , m_count( 0 ) { }

    void operator()( const state_type &x , double t )
    {
        if( ( m_count % m_every ) == 0 )
        {
            clog << t << endl;
            cout << "sp '-'" << endl;
            for( size_t i=0 ; i<x.size1() ; ++i )
            {
                for( size_t j=0 ; j<x.size2() ; ++j )
                {
                    cout << i << "\t" << j << "\t" << sin( x( i , j ) ) << "\n";
                }
                cout << "\n";
            }
            cout << "e" << endl;
        }

        ++m_count;
    }
};

class write_snapshots
{
public:

    typedef std::map< size_t , std::string > map_type;

    write_snapshots( void ) : m_count( 0 ) { }

    void operator()( const state_type &x , double t )
    {
        map< size_t , string >::const_iterator it = m_snapshots.find( m_count );
        if( it != m_snapshots.end() )
        {
            ofstream fout( it->second.c_str() );
            for( size_t i=0 ; i<x.size1() ; ++i )
            {
                for( size_t j=0 ; j<x.size2() ; ++j )
                {
                    fout << i << "\t" << j << "\t" << x( i , j ) << "\t" << sin( x( i , j ) ) << "\n";
                }
                fout << "\n";
            }
        }
        ++m_count;
    }

    map_type& snapshots( void ) { return m_snapshots; }
    const map_type& snapshots( void ) const { return m_snapshots; }

private:

    size_t m_count;
    map_type m_snapshots;
};

int main( int argc , char **argv )
{
    size_t size1 = 128 , size2 = 128;
    state_type x( size1 , size2 , 0.0 );

    for( size_t i=(size1/2-10) ; i<(size1/2+10) ; ++i )
        for( size_t j=(size2/2-10) ; j<(size2/2+10) ; ++j )
            x( i , j ) = static_cast<double>( rand() ) / RAND_MAX * 2.0 * M_PI;

    write_snapshots snapshots;
    snapshots.snapshots().insert( make_pair( size_t( 0 ) , string( "lat_0000.dat" ) ) );
    snapshots.snapshots().insert( make_pair( size_t( 100 ) , string( "lat_0100.dat" ) ) );
    snapshots.snapshots().insert( make_pair( size_t( 1000 ) , string( "lat_1000.dat" ) ) );
    observer_collection< state_type , double > obs;
    obs.observers().push_back( write_for_gnuplot( 10 ) );
    obs.observers().push_back( snapshots );

    cout << "set term x11" << endl;
    cout << "set pm3d map" << endl;

    integrate_const( runge_kutta4<state_type>() , two_dimensional_phase_lattice( 1.2 ) ,
                     x , 0.0 , 1001.0 , 0.1 , boost::ref( obs ) );

    // controlled steppers work only after ublas bugfix
    //integrate_const( make_dense_output< runge_kutta_dopri5< state_type > >( 1E-6 , 1E-6 ) , two_dimensional_phase_lattice( 1.2 ) ,
    //        x , 0.0 , 1001.0 , 0.1 , boost::ref( obs ) );

    return 0;
}


/**
\file   quintic_spline.cpp
\brief  quintic_spline parameterization
 *
\author  Mahmoud Ali
\date    21/6/2019
*/

#include<vector>
#include<iostream>
#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"

//#include<normal_toppra_traj_instant_1.h>
#include<normal_toppra_traj_instant_3.h>
#include <Eigen/Dense>
#include <math.h>

#include <python2.7/Python.h>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
using namespace Eigen;
using namespace std;
using Eigen::MatrixXd;

const double sm=180, vm= 130, am=250, jm=1000; // pos, vel, acc, jrk max limits

// finds in which segment time instant tg belons to
int find_seg_number (double tg, std::vector<double> T_vec){
    // segment changes from 0 to n-2 which mean n-1 seg
    int seg = 0;
    if(tg<= T_vec[0]) //less than tstart
        return seg;
    else if(tg>= T_vec[T_vec.size() -2 ] ) //if tg is greater than the starting time of the last segment
        return T_vec.size() - 2;
    else {
        for (int i=0; i< T_vec.size()-2 ; i++) { // between tstart and tend
            if(tg>= T_vec[i] && tg< T_vec[i+1])
                seg = i;
        }
        return seg;
    }
}

// to sample trajectory, find state(pos, vel, acc, jrk) at a given time tg, T_vec is time vector of waypoints, cof is coef matrix of quintic spline
void sample (const std::vector<double>& T_vec, const double& tg, const std::vector<std::vector<double>> &cof, std::vector<double> & state){//, bool pos_plt=true, bool vel_plt=true, bool acc_plt=true, bool jrk_plt=true ){
    std::vector<double> a=cof[0], b=cof[1], c=cof[2], d=cof[3], e=cof[4], f=cof[5];
    double pos=0, vel=0, acc=0, jrk=0;
    int n = T_vec.size(), seg=0; // n:number of points
    double t=0; // to represent tg-tseg
    //check for vector sizes
    //    std::cout<<"\nsize of a= "<< a.size() << " size of T= "<< T_vec.size();
    if( a.size() != T_vec.size()-1 )
        std::cout<<"error: coef and time have different size\n";

    // check for tg inside T_vec or not, if yes find corrresponding segment
    if(tg < T_vec[0]){
        std::cout<<"Warn: request time instant is before start time of trajectory\n";
        pos= a[0];
        vel= b[0];
        acc= 2*c[0];
        jrk= 6*d[0];
    }
    else if(tg > T_vec[n-1] ){
        std::cout<<"Warn: request time instant is after the end time of trajectory\n ";
        t = tg - T_vec[n-1];
        pos=   a[n-2] +   b[n-2]*t +         c[n-2]*pow( t, 2) +        d[n-2]*pow( t, 3) +    e[n-2]*pow( t, 4) +    f[n-2]* pow( t, 5)  ;
        vel=              b[n-2]        +  2*c[n-2]*t          +      3*d[n-2]*pow( t, 2) +  4*e[n-2]*pow( t, 3) +  5*f[n-2]* pow( t, 4)  ;
        acc=                               2*c[n-2]                 + 6*d[n-2]*t          + 12*e[n-2]*pow( t, 2) + 20*f[n-2]* pow( t, 3)  ;
        jrk=                                                          6*d[n-2]                 + 24*e[n-2]*t          + 60*f[n-2]* pow( t, 2)  ;
    }
    else{
        seg= find_seg_number(tg, T_vec);
        t = tg - T_vec[seg];
        //        std::cout<<"      tg= " <<tg<< "   seg_number= "<<  seg<<"   t= "<<t<<std::endl;
        pos=   a[seg] +   b[seg]*t +    c[seg]*pow( t, 2) +   d[seg]*pow( t, 3) +    e[seg]*pow( t, 4) +    f[seg]* pow( t, 5) ;
        vel=              b[seg]        +  2*c[seg]*t          + 3*d[seg]*pow( t, 2) +  4*e[seg]*pow( t, 3) +  5*f[seg]* pow( t, 4) ;
        acc=                               2*c[seg]                 + 6*d[seg]*t          + 12*e[seg]*pow( t, 2) + 20*f[seg]* pow( t, 3) ;
        jrk=                                                          6*d[seg]                 + 24*e[seg]*t          + 60*f[seg]* pow( t, 2) ;
    }
    // assign state to state vector
    state[0] = pos;
    state[1] = vel;
    state[2] = acc;
    state[3] = jrk;

}

// generate C1 matrix which is depends on the time of each segment, idx is segment indx, h vectore contains segments times
void generate_coef_matrix_c1(const int idx, const VectorXd &h, MatrixXd &C1){
    MatrixXd C(5,6);
    C<< 1,  h(idx),  pow(h(idx), 2),    pow(h(idx), 3),    pow(h(idx), 4),     pow(h(idx), 5),
            0,       1,        2*h(idx),  3*pow(h(idx), 2),  4*pow(h(idx), 3),   5*pow(h(idx), 4),
            0,       0,               1,          3*h(idx),  6*pow(h(idx), 2),  10*pow(h(idx), 3),
            0,       0,               0,                 1,          4*h(idx),  10*pow(h(idx), 2),
            0,       0,               0,                 0,                 1,           5*h(idx);
    C1 = C;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "quintic_isp");
    ros::NodeHandle nh;

    ROS_INFO("initializing path .... ");
    //============ read trajectory ==========
    trajectory_msgs::JointTrajectory  traj;
    traj = generate_traj();
    int n_jts = traj.joint_names.size();
    int n_pts = traj.points.size();

    // get waypoints from trajectory
    std::vector< std::vector<double> > P_jt_wpt;
    P_jt_wpt.resize( n_jts);
    for(int jt=0; jt<n_jts; jt++){
        for(int pt=0; pt<n_pts; pt++){
            P_jt_wpt[jt].push_back( traj.points[pt].positions[jt] );
        }
    }

    //pos vector
    VectorXd pos(n_pts);
    for (int pt=0; pt< n_pts; pt++) {
        pos(pt) = P_jt_wpt[4][pt];
    }
    //    for (int pt=0; pt< n_pts; pt++) {
    //        pos(pt+n_pts) = P_jt_wpt[5][pt];
    //    }
    // put equations in form of AX=B
    int n = pos.size(); //number of waypoints
    int n_eqs = 6*(n-1); // number of equations/variables
    double vel0=0, veln=0, acc0=0, accn=0; // initial/final  conditions(vel, acc)

    // store stop point at which trajectory reverse direction
    std::vector<int> stp_pts_idx;
    for (int i=1; i<n-1; i++) {
        if( (  (pos(i+1) - pos(i))> 0 && (pos(i) - pos(i-1))> 0  ) || ( (pos(i+1) - pos(i))< 0 && (pos(i) - pos(i-1))< 0 ) )
            continue;
        stp_pts_idx.push_back( i );
    }
    //    ROS_INFO_STREAM("stp_pts_idx"); //check
    //    for (int i=0; i< stp_pts_idx.size(); i++)
    //        ROS_INFO_STREAM(" "<< stp_pts_idx[i] );

    //from pos vector: generate initial time for way points. t: time vector of waypoinys times, h is vector for segments times
    VectorXd t(n), h(n-1); //t0 ==> tn-1  ...   h0 ==> hn-2 (#n of t and #n-1 of h)
    for (int i = 0; i < n-1; ++i) {
        h(i) = abs( (abs(pos(i+1)) - abs(pos(i))) ) /vm;
    }
    //  ROS_INFO_STREAM("h vector: \n"<<h);
    t(0)=0; //starting time of trajectory
    for (int i = 0; i < n-1; ++i) {
        t(i+1) = h(i) + t(i);
    }
    //  ROS_INFO_STREAM("t vector: \n"<<t);

    // define coef matrix and B matrix: AX=B
    VectorXd B(n_eqs), X(n_eqs);
    MatrixXd A =   MatrixXd::Zero(n_eqs, n_eqs);
    MatrixXd C1 =  MatrixXd::Zero(5, 6);
    MatrixXd C2 =  MatrixXd::Zero(5, 6);

    // C1 and C2 store coef of 5(n-2) equations
    for (int i=0; i<5; i++)
        C2(i,i)=-1.0; // C2(0,0)  to   C2(4,4)

    for (int i=0; i<n-1; i++) {
        generate_coef_matrix_c1(i, h, C1);
        ROS_INFO_STREAM(" C1("<<i <<") initialization:\n "<< C1);
    }

    //>>>>>>>>>>>step_1:  3 boundary conditions for first point (pos0, vel0, acc0), 3 rows
    A(0,0) = 1;
    A(1,1) = 1;
    A(2,2) = 1;  // 3 rows are filled so far, so next r=3

    //>>>>>>>>>>> step_2:  pos condition for all waypoints except first and last, (n-2) rows
    int col_idx= 6;
    for (int r = 3; r < 3+(n-2); ++r) {  //start filling from 4th row, #rows= n-2
        A(r,col_idx) = 1;
        col_idx+=6;
    }  // n-2 rows are filled so far, so total rows are n-2+3= n+1

    int count =0; // count no of iteration
    std::vector<int> seg_idx;  //store segments indices that violate jerk
    bool update_all_seg_time= false; // select: true=> updates times of all segments, false: update time for segments that voilate jerk
    //================================================= loop: update time till the jerk become within the limit ================================
    // bif while loop
    bool lmtd_jrk = false; //will be set to true when all jrk values are in the limits
    while (!lmtd_jrk){
        if(count>0){ //no update in the first loop
            if(update_all_seg_time) // update all segment time
                for (int idx = 0; idx < n-1; ++idx)
                    h(idx)=h(idx)+0.01;

            else{  // only update the time of segment that violate jerk limit
                for (int idx = 0; idx < seg_idx.size(); ++idx){
                    ROS_INFO_STREAM("seg_idx["<<idx<< "]:  "<<seg_idx[idx]);
                    if (seg_idx[idx] == n-1)
                        seg_idx[idx] = seg_idx[idx] -1;
                    h(seg_idx[idx])=h(seg_idx[idx])+0.01;
                }
            }
        }
        seg_idx.clear();
        for (int i = 0; i < n-1; ++i) {
            t(i+1) = h(i) + t(i);
        }

        // checks for velocity and acc limits

        //>>>>>>>>>>>step_3: boundary conditions for last point (posn, veln, accn), 3 rows (depends on time so it is inside the loop)
        // ROS_INFO("3 boundary conditions for last poit");
        for (int r = n+1 ; r < 3+(n+1); ++r) {// for 3 row
            for (int c = n_eqs-6 ; c < n_eqs; ++c) {  // last 6 columns
                generate_coef_matrix_c1( n-2, h, C1);
                A(r, c) = C1(r-(n+1), c-(n_eqs-6));     // last h ==> hn-2
            }
        }//3+(n-2)+3 = n+4 rows are filled so far,  so next row to fill is (n+5)th row [its is index= n+4]
        //>>>>>>>>>>>step_4: 5(n-2) coninuity conditions for each waypoints except first and last, 5(n-2) rows (depend on time so it is inside the loop)
        //   ROS_INFO(" 5(n-2) continuity conditions for midle poits");
        int row_idx = n+4; col_idx=0;  //start filling from (n+2)th row [its is index= n+1],
        for (int blk=0; blk< n-2; blk++) { // n-2 blks, each blk is 5 equations with 6 coef
            //       ROS_INFO_STREAM("row_idx: "<< row_idx <<"   col_idx:"<< col_idx);
            for (int r = 0 ; r < 5 ; ++r) {  // 5(n-2) rows
                for (int c = 0 ; c < 6; ++c) {  // last 6 columns
                    //               ROS_INFO_STREAM("r: "<< r <<"   c:"<< c);
                    generate_coef_matrix_c1( blk, h, C1);
                    A(row_idx+r, col_idx+c)   = C1(r,c);     // last h ==> hn-2
                    A(row_idx+r, col_idx+c+6) = C2(r,c);
                }
            }
            col_idx += 6;
            row_idx += 5;
        } //all row s are filled

        //====================================check for stop points:==================================

        //>>>>>>>>>>>step_5: fill B vector B
        B(0) = pos(0);
        B(1) = vel0;
        B(2) = acc0; //  3 rows so far
        for (int i=1; i< n-1; i++) // all pos from pos1 to pos(n-1)= (n-2) rows
            B(i+2)= pos(i); // so far 3+n-2 = n+1

        for (int i=n+4; i< n_eqs; i++) // last 5(n-2) ros
            B(i)= 0; // so far 3+n-2 = n+1

        B(n+1) = pos(n-1);
        B(n+2) = veln;
        B(n+3) = accn;
        ROS_INFO_STREAM("B vector: \n"<<B);

        //find the solution: using matirx inverse
        //   MatrixXd A_inv = A.inverse();
        //   X = A.inverse()*B;  // some times gives nan or inf
        //   ROS_INFO_STREAM("X vector: \n"<<X);

        HouseholderQR<MatrixXd> qr(A);
        X = qr.solve(B); // computes A^-1 * b
        ROS_INFO_STREAM("X vector: \n"<<X );

        // extract spline coefs from the vector  ========================================================
        std::vector<double> a, b, c, d, e, f;
        for (int i = 0; i < n_eqs; i+=6) {
            a.push_back( X(i) );
            b.push_back( X(i+1) );
            c.push_back( X(i+2) );
            d.push_back( X(i+3) );
            e.push_back( X(i+4) );
            f.push_back( X(i+5) );
        }

        // sample trajectory using sample func: create variables,  sampling T_vec, Coef_mtrx
        std::vector<double> T_vec;
        T_vec.resize(n);
        for (int i=0; i< n; i++) {
            T_vec[i] = t(i);
        }
        std::vector<double> POS, VEL, ACC, JRK, T;
        std::vector<std::vector<double>> cof_mtrx;
        cof_mtrx.push_back(a);
        cof_mtrx.push_back(b);
        cof_mtrx.push_back(c);
        cof_mtrx.push_back(d);
        cof_mtrx.push_back(e);
        cof_mtrx.push_back(f);
        std::vector<double> state;
        state.resize(4);
        // states at each intermdiate times
        double frq =125, tg=0;
        while (tg< T_vec[n-1]) {
            sample(T_vec, tg, cof_mtrx, state);
            //    std::cout<< "state:"<< "p= "<< state[0]<<"   v= "<< state[1]<<"   a= "<< state[2]<<"  j= "<< state[3] ;
            T.push_back(tg);
            POS.push_back( state[0]);
            VEL.push_back( state[1]);
            ACC.push_back( state[2]);
            JRK.push_back( state[3]);
            tg += 1/frq;
        }

        // states at waypoints and check limit conditions
        lmtd_jrk = true;
        std::vector<std::vector<double>> wpt_states;
        wpt_states.resize(4);
        for (int pt = 0; pt < n; ++pt) {
            tg= T_vec[pt];
            sample(T_vec, tg, cof_mtrx, state);
            wpt_states[0].push_back( state[0] );
            wpt_states[1].push_back( state[1] );
            wpt_states[2].push_back( state[2] );
            wpt_states[3].push_back( state[3] );
            std::cout<< "jrk["<<pt<<"]= "<< state[3] ;
            if(  state[3] > 1000 || state[3] < -1000 ){
                seg_idx.push_back( pt);
                lmtd_jrk = false;
            }
        }
        // ROS_INFO( "waypoints times:    ");
        // for (int i=0; i< n; i++ )
        //    ROS_INFO_STREAM( "    "<< T_vec[i] );
        // ROS_INFO( "segment peroids:    ");
        // for (int i=0; i< n-1; i++ )
        //    ROS_INFO_STREAM( "    "<< h[i] );

        // plot section =================================
        count++;
        int plot_per_loop = 3; //to plot 9 iteration to fit on sbplts number
        int sbplt_rows= 3, sbplt_cols= 3;
        bool pos_plt=false, vel_plt=true, acc_plt=true, jrk_plt = false;// select states to plot
        pos_plt=false, vel_plt=false, acc_plt=false, jrk_plt = true;
        std::string  sbplt_name;

        //   here choose few iterations, plot them  in subplot
        if(  (count)%plot_per_loop == 0 ){
            plt::figure(1);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot( T, POS);
            plt::plot( T_vec, wpt_states[0],"r*");
            sbplt_name= "pos_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(2);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, VEL);
            plt::plot( T_vec, wpt_states[1],"r*");
            sbplt_name= "vel_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(3);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, ACC);
            plt::plot( T_vec, wpt_states[2],"r*");
            sbplt_name= "acc_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            plt::figure(4);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop   );
            plt::plot(  T, JRK);
            plt::plot( T_vec, wpt_states[3],"r*");
            sbplt_name= "jrk_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);
            //    plt::legend(); // Enable legend.

        }
        if(lmtd_jrk){ // to plot last iteration
            plt::figure(1);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop  +1);
            plt::plot( T, POS);
            plt::plot( T_vec, wpt_states[0],"r*");
            sbplt_name= "pos_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(2);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, VEL);
            plt::plot( T_vec, wpt_states[1],"r*");
            sbplt_name= "vel_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(3);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, ACC);
            plt::plot( T_vec, wpt_states[2],"r*");
            sbplt_name= "acc_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            plt::figure(4);
            plt::subplot(  sbplt_rows, sbplt_cols,count/plot_per_loop    +1);
            plt::plot(  T, JRK);
            plt::plot( T_vec, wpt_states[3],"r*");
            sbplt_name= "jrk_last_iter_" + std::to_string(count);
            plt::title(  sbplt_name);
            plt::grid(true);

            //    plt::legend(); // Enable legend.
        }
        ROS_INFO_STREAM("#iteration= "<<count <<"is_lmtd_jrk: "<< lmtd_jrk);
    }

    ROS_INFO_STREAM("#loops= "<<count);
    plt::show();

}


#ifndef _glia_type_bignum_hxx_
#define _glia_type_bignum_hxx_

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "glia_base.hxx"

namespace glia {

typedef boost::multiprecision::int512_t BigInt;
typedef boost::multiprecision::cpp_dec_float_100 BigFloat;

};

#endif


//
// Created by Vlad Argunov on 10/01/2022.
//

#include "HestonOption.h"

#include <Eigen/Dense>
#include <random>
#include <string>
#include <cmath>
#include <tuple>
#include <algorithm>
#include "Simulation.h"

HestonOption::HestonOption(std::string type_option_, double tn_, int n_processes_, int n_steps_, double strike_) {
    sim = Simulation();
    type_option = type_option_;
    tn = tn_;
    n_processes = n_processes_;
    n_steps = n_steps_;
    strike = strike_;
}

void HestonOption::set_params_stock_process(double stock_price_start_, double mu_) {
    stock_price_start = stock_price_start_;
    mu = mu_;
}

void HestonOption::set_prams_variance_process(double variance_start_, double kappa_, double theta_, double sigma_,
                                              double rho_) {
    variance_start = variance_start_;
    kappa = kappa_;
    theta = theta_;
    sigma = sigma_;
    rho = rho_;
}

double HestonOption::compute_price() {
    auto [stocks, _] = sim.generate_heston_process(n_processes, tn, stock_price_start, mu, variance_start, kappa, theta, sigma, rho, n_steps);
    Eigen::Index stocks_cols = stocks.cols();
    double total_payoffs = 0;
    for (int row = 0; row < stocks.rows(); ++row) {
        if (type_option == "Call"){
            total_payoffs += std::max(stocks(row,stocks_cols - 1) - strike, 0.0);
        }
        if (type_option == "Put"){
            total_payoffs += std::max(strike - stocks(row,stocks_cols), 0.0);
        }
    }
    return exp(- mu * tn) * total_payoffs / n_processes;
}


/**
 * @file convolutional_network_test.cpp
 * @author Marcus Edel
 *
 * Tests the convolutional neural network.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>

#include <mlpack/methods/ann/layer/one_hot_layer.hpp>
#include <mlpack/methods/ann/layer/conv_layer.hpp>
#include <mlpack/methods/ann/layer/pooling_layer.hpp>
#include <mlpack/methods/ann/layer/softmax_layer.hpp>
#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>

#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>

#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/cnn.hpp>

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

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(ConvolutionalNetworkTest);

/**
 * Train and evaluate a vanilla network with the specified structure.
 */
template<
    typename PerformanceFunction
>
void BuildVanillaNetwork()
{
  arma::mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  arma::uword nPoints = X.n_cols;
  for (arma::uword i = 0; i < nPoints; i++)
  {
    X.col(i) /= norm(X.col(i), 2);
  }

  // Build the target matrix.
  arma::mat Y = arma::zeros<arma::mat>(10, nPoints);
  for (size_t i = 0; i < nPoints; i++)
  {
    if (i < nPoints / 2)
    {
      Y.col(i)(5) = 1;
    }
    else
    {
      Y.col(i)(8) = 1;
    }
  }

  arma::cube input = arma::cube(28, 28, nPoints);
  for (size_t i = 0; i < nPoints; i++)
    input.slice(i) = arma::mat(X.colptr(i), 28, 28);

  /*
   * Construct a convolutional neural network with a 28x28x1 input layer,
   * 24x24x8 convolution layer, 12x12x8 pooling layer, 8x8x12 convolution layer
   * and a 4x4x12 pooling layer which is fully connected with the output layer.
   * The network structure looks like:
   *
   * Input    Convolution  Pooling      Convolution  Pooling      Output
   * Layer    Layer        Layer        Layer        Layer        Layer
   *
   *          +---+        +---+        +---+        +---+
   *          | +---+      | +---+      | +---+      | +---+
   * +---+    | | +---+    | | +---+    | | +---+    | | +---+    +---+
   * |   |    | | |   |    | | |   |    | | |   |    | | |   |    |   |
   * |   +--> +-+ |   +--> +-+ |   +--> +-+ |   +--> +-+ |   +--> |   |
   * |   |      +-+   |      +-+   |      +-+   |      +-+   |    |   |
   * +---+        +---+        +---+        +---+        +---+    +---+
   */

  ConvLayer<> convLayer0(1, 8, 5, 5);
  BiasLayer2D<> biasLayer0(8);
  BaseLayer2D<> baseLayer0;
  PoolingLayer<> poolingLayer0(2);

  ConvLayer<> convLayer1(8, 12, 5, 5);
  BiasLayer2D<> biasLayer1(12);
  BaseLayer2D<> baseLayer1;
  PoolingLayer<> poolingLayer1(2);

  LinearMappingLayer<> linearLayer0(192, 10);
  BiasLayer<> biasLayer2(10);
  SoftmaxLayer<> softmaxLayer0;

  OneHotLayer outputLayer;

  auto modules = std::tie(convLayer0, biasLayer0, baseLayer0, poolingLayer0,
                          convLayer1, biasLayer1, baseLayer1, poolingLayer1,
                          linearLayer0, biasLayer2, softmaxLayer0);

  CNN<decltype(modules), decltype(outputLayer),
      RandomInitialization, MeanSquaredErrorFunction> net(modules, outputLayer);
  biasLayer0.Weights().zeros();
  biasLayer1.Weights().zeros();

  RMSprop<decltype(net)> opt(net, 0.01, 0.88, 1e-8, 10 * input.n_slices, 0);

  net.Train(input, Y, opt);

  arma::mat prediction;
  net.Predict(input, prediction);

  size_t error = 0;
  for (size_t i = 0; i < nPoints; i++)
  {
    if (arma::sum(arma::sum(
        arma::abs(prediction.col(i) - Y.col(i)))) == 0)
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / nPoints;
  BOOST_REQUIRE_LE(classificationError, 0.6);
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkTest)
{
  BuildVanillaNetwork<LogisticFunction>();
}

BOOST_AUTO_TEST_SUITE_END();


#include "blas.h"
#include "mlfe/device_context/cpu_context.h"
#include <Eigen/Dense>

namespace mlfe{ namespace math{

template<>
void gemm<float, CPUContext>(const bool trans_a,
                             const bool trans_b,
                             const int m,
                             const int n,
                             const int k,
                             const float alpha,
                             const float *a_ptr,
                             const int lda,
                             const float *b_ptr,
                             const int ldb,
                             const float beta,
                             float *c_ptr,
                             const int ldc,
                             CPUContext *context
                            )
{
    using ConstMapMatXf = Eigen::Map<const Eigen::MatrixXf>;
    Eigen::Map<Eigen::MatrixXf> c(c_ptr, n, m);
    if(beta == 0.f){
        c.setZero();
    }
    else{
        c *= beta;
    }
    if(!trans_a && !trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, n, k) *
                                ConstMapMatXf(a_ptr, k, m)
                               );
    }
    else if(trans_a && !trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, n, k) *
                                ConstMapMatXf(a_ptr, m, k).transpose()
                               );
    }
    else if(!trans_a && trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, k, n).transpose() *
                                ConstMapMatXf(a_ptr, k, m)
                               );
    }
    else{
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, k, n).transpose() *
                                ConstMapMatXf(a_ptr, m, k).transpose()
                               );
    }
}

template<>
void gemm<double, CPUContext>(const bool trans_a,
                              const bool trans_b,
                              const int m,
                              const int n,
                              const int k,
                              const double alpha,
                              const double *a_ptr,
                              const int lda,
                              const double *b_ptr,
                              const int ldb,
                              const double beta,
                              double *c_ptr,
                              const int ldc,
                              CPUContext *context
                             )
{
    using ConstMapMatXf = Eigen::Map<const Eigen::MatrixXd>;
    Eigen::Map<Eigen::MatrixXd> c(c_ptr, n, m);
    if(beta == 0.){
        c.setZero();
    }
    else{
        c *= beta;
    }
    if(!trans_a && !trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, n, k) *
                                ConstMapMatXf(a_ptr, k, m)
                               );
    }
    else if(trans_a && !trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, n, k) *
                                ConstMapMatXf(a_ptr, m, k).transpose()
                               );
    }
    else if(!trans_a && trans_b){
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, k, n).transpose() *
                                ConstMapMatXf(a_ptr, k, m)
                               );
    }
    else{
        c.noalias() += alpha * (ConstMapMatXf(b_ptr, k, n).transpose() *
                                ConstMapMatXf(a_ptr, m, k).transpose()
                               );
    }
}

template <>
void gemv<float, CPUContext>(const bool trans_a,
                             const int m,
                             const int n,
                             const float alpha,
                             const float *a_ptr,
                             const int lda,
                             const float *b_ptr,
                             const float beta,
                             float *c_ptr,
                             const int ldc,
                             CPUContext *context
                            )
{
    using ConstMapMatXf = Eigen::Map<const Eigen::MatrixXf>;
    using ConstMapVecXf = Eigen::Map<const Eigen::VectorXf>;
    Eigen::Map<Eigen::VectorXf> c(c_ptr, !trans_a ? m : n);
    if(beta == 0.f){
        c.setZero();
    }
    else{
        c *= beta;
    }

    if(!trans_a){
        c.noalias() += alpha * (ConstMapMatXf(a_ptr, n, m).transpose() *
                                ConstMapVecXf(b_ptr, n)
                                );
    }
    else{
        c.noalias() += alpha * (ConstMapMatXf(a_ptr, n, m) *
                                ConstMapVecXf(b_ptr, m)
                                );
    }
}

template <>
void gemv<double, CPUContext>(const bool trans_a,
                              const int m,
                              const int n,
                              const double alpha,
                              const double *a_ptr,
                              const int lda,
                              const double *b_ptr,
                              const double beta,
                              double *c_ptr,
                              const int ldc,
                              CPUContext *context
                             )
{
    using ConstMapMatXf = Eigen::Map<const Eigen::MatrixXd>;
    using ConstMapVecXf = Eigen::Map<const Eigen::VectorXd>;
    Eigen::Map<Eigen::VectorXd> c(c_ptr, !trans_a ? m : n);
    if(beta == 0.){
        c.setZero();
    }
    else{
        c *= beta;
    }

    if(!trans_a){
        c.noalias() += alpha * (
                                ConstMapMatXf(a_ptr, n, m).transpose() *
                                ConstMapVecXf(b_ptr, n)
                                );
    }
    else{
        c.noalias() += alpha * (
                                ConstMapMatXf(a_ptr, n, m) *
                                ConstMapVecXf(b_ptr, m)
                                );
    }
}

} // end namespace math
} // end namespace mlfe


// Simple Kalman Filter implementation for one-dimensional processes
// Author: Marcell Missura <missura@ais.uni-bonn.de>
// Modified for Eigen by Max Schwarz <max.schwarz@uni-bonn.de>

#include <rc_utils/kalmanfilter.h>
#include <Eigen/LU>

namespace rc_utils
{

KalmanFilter::KalmanFilter()
 : m_smoothing(0.3)
 , m_timeStep(DefaultTimeStep)
 , m_lastZ(0)
 , m_px(0)
 , m_pv(0)
{
	init();
}

KalmanFilter::~KalmanFilter()
{
}

void KalmanFilter::init()
{
	// Process noise
	m_Q = Eigen::Matrix2d::Identity();

	// Measurement noise
	m_R << 0.001 + m_smoothing*100.0, 0.0,
	       0.0,                       0.001 + m_smoothing*100.0;

	// Passive linear system dynamics matrix A
	m_A << 1.0, m_timeStep,
	       0.0, 1.0;

	// State and measurement vectors
	m_X << 0.0, 0.0;

	m_H = m_I = m_K = m_P = Eigen::Matrix2d::Identity();
}

void KalmanFilter::reset()
{
	m_K = Eigen::Matrix2d::Identity();
	m_P = Eigen::Matrix2d::Zero();
}

void KalmanFilter::update(double z)
{
	m_Z << z, (z - m_lastZ) / m_timeStep;

	m_X = m_A * m_X;
	m_P = m_A * m_P * m_A.transpose() + m_Q;
	m_K = m_P * m_H.transpose() * (m_H * m_P * m_H.transpose() + m_R).inverse();
	m_X = m_X + m_K * (m_Z - m_H * m_X);
	m_P = (m_I - m_K * m_H) * m_P;

	m_lastZ = z;

	m_px = m_P(0,0);
	m_pv = m_P(1,1);
}

void KalmanFilter::setState(double pos, double vel)
{
	m_X << pos, vel;
	m_lastZ = pos;
}

}


// combination_iterator.hpp
//
// Produces all k-combinations of the universe {0,...,n-1} in
// lexicographical order. The algorithm is a tuned-up version
// of a simple generation method ("Algorithm T") described in:
//
//   Knuth, Donald E.
//   The Art of Computer Programming, Volume 4A: Combinatorial Algorithms, Part 1.
//   Pearson Education Inc., 2011.

#ifndef COMBINATION_ITERATOR_HPP
#define COMBINATION_ITERATOR_HPP

#include <vector>
#include <numeric>
#include <cassert>
#include <cstdint>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class combination_iterator
	: public boost::iterator_facade<
	combination_iterator<T>,
	const std::vector<T>&,
	boost::forward_traversal_tag
	>
{
public:
	combination_iterator() : end_(true), n_(0), size_(0), comb_() { }

	explicit combination_iterator(T n, T k) : end_(false), n_(n), size_(k), comb_(k)
	{
		assert(k != 0 && n_ > k);
		std::iota(comb_.begin(), comb_.end(), 0);
		assert(!end_);
	}

private:
	friend class boost::iterator_core_access;

	void increment()
	{
		std::int64_t j = size_ - 1;

		for (const T end = n_ - size_; j >= 0 && comb_[j] >= end + j; --j) { }

		if (j < 0)
		{
			assert(comb_.front() == n_ - size_);
			end_ = true;
			return;
		}

		++comb_[j];

		for (const std::int64_t end = size_ - 1; j < end; ++j)
		{
			comb_[j + 1] = comb_[j] + 1;
		}

		return;
	}

	bool equal(const combination_iterator& other) const
	{
		return end_ == other.end_;
	}

	const std::vector<T>& dereference() const
	{
		return comb_;
	}

	bool end_;
	const int n_;
	const int size_;
	std::vector<T> comb_;
};

#endif


#include "ukf.hpp"
#include "house.hpp"
#include "eigen_csv.hpp"
#include "timer.hpp"

#include <Eigen/Dense>

#include <iostream>
#include <random>
#include <vector>
#include <string>

using namespace Eigen;
using namespace std;

// One degree (in radians)
const double deg = M_PI / 180;

// Dynamical model
VectorXd ct_prop(double ti, double tf, const VectorXd& x, const VectorXd& w) {
    VectorXd xf(5);
    double W = x(4);
    double T = tf - ti;
    /*if (fabs(W) < 1E-9) {
        xf = x;
        xf(0) += T * xf(1);
        xf(2) += T * xf(3);
    } else {*/
        double sinWT = sin(W*T);
        double cosWT = cos(W*T);
        MatrixXd F(5,5);
        F << 1, sinWT/W,     0, -(1-cosWT)/W, 0,
             0, cosWT,       0, -sinWT,       0,
             0, (1-cosWT)/W, 1, sinWT/W,      0,
             0, sinWT,       0, cosWT,        0,
             0, 0,           0, 0,            1;
        xf = F * x;
    //}
    return xf + w;
}

// Measurement model
Vector2d ct_meas(double t, const VectorXd& x) {

    Vector2d z;

    double xi  = x(0);
    double eta = x(2);

    z << sqrt(xi*xi + eta*eta), atan2(eta, xi);

    return z;

}

// True state as function of time
VectorXd ct_tru(double t) {

    const double v  = 120;
    const double w1 =  1 * deg;
    const double w2 = -3 * deg;
    const double r1 = fabs(v / w1);
    const double r2 = fabs(v / w2);
    const double t1 = 90;
    const double t2 = 30;
    const double tl = 125;
    const double l  = v * tl;
    const double x0 = 25000;
    const double y0 = 10000;

    VectorXd x(5);
    double dt, xc, yc;

    if (t <= tl) {
        x(0) = x0 - v * t;
        x(2) = y0;
        x(1) = -v;
        x(3) = 0;
        x(4) = 0;
    } else if (t <= tl + t1) {
        dt = t - tl;
        xc = x0 - l;
        yc = y0 - r1;
        x(0) =  xc + r1 * cos(w1 * dt + M_PI/2);
        x(2) =  yc + r1 * sin(w1 * dt + M_PI/2);
        x(1) = -w1 * r1 * sin(w1 * dt + M_PI/2);
        x(3) =  w1 * r1 * cos(w1 * dt + M_PI/2);
        x(4) =  w1;
    } else if (t <= 2*tl + t1) {
        dt = t - tl - t1;
        x(0) = x0 - l - r1;
        x(2) = y0 - r1 - v * dt;
        x(1) = 0;
        x(3) = -v;
        x(4) = 0;
    } else if (t <= 2*tl + t1 + t2) {
        dt = t - 2*tl - t1;
        xc = x0 - l - r1 - r2;
        yc = y0 - l - r1;
        x(0) =  xc + r2 * cos(w2 * dt);
        x(2) =  yc + r2 * sin(w2 * dt);
        x(1) = -w2 * r2 * sin(w2 * dt);
        x(3) =  w2 * r2 * cos(w2 * dt);
        x(4) =  w2;
    } else {
        dt = t - 2*tl - t1 - t2;
        x(0) = x0 - l - r1 - r2 - v*dt;
        x(2) = y0     - r1 - r2 - l;
        x(1) = -v;
        x(3) = 0;
        x(4) = 0;
    }

    return x;

}

// Generate filename for state estimates
std::string est_filename(const std::string& filter, int i, int j) {
    std::string f = "out/ct_est_";
    f += filter;
    f += "_";
    f += std::to_string(i+1);
    f += "_";
    f += std::to_string(j+1);
    f += ".csv";
    return f;
}

int main() {

    // CUT directory
    UKF::cut_dir = "../CUT/";

    // UKF state & measurement models
    UKF::dyn_model f = ct_prop;
    UKF::meas_model h = ct_meas;

    // HOUSE measurement model
    HOUSE::meas_model hh = [] (double t, const VectorXd& x, const VectorXd& n)
        -> VectorXd {
            return ct_meas(t, x) + n;
        };

    // Constants
    double sr, sth, l1, l2;
    sr  = 100;
    sth = 1 * deg;
    l1  = 0.16;
    l2  = 0.01;

    // Measurement noise covariance
    Matrix2d R;
    R << sr*sr, 0,
         0,     sth*sth;

    // Prior mean & covariance
    VectorXd mxi(5), cxx(5);
    cxx << 1000, 10, 1000, 10, 1*deg;
    mxi << 25000, -120, 10000, 0, 0.000001;
    MatrixXd Pxxi = cxx.cwiseAbs2().asDiagonal();

    // HOUSE distributions
    HOUSE::Dist distXi(Pxxi), distn(R);
    distXi.mean = mxi;

    // Normal noise generator
    mt19937_64 mt;
    normal_distribution<double> nd;

    // Time step lengths
    const int Ntimes = 4;
    VectorXd Ts(Ntimes);
    Ts << 1, 2.5, 3.9, 5;
    //Ts.setLinSpaced(Ntimes, 1, 5);

    // Save time step lengths
    EigenCSV::write(Ts, "out/times.csv");

    // Number of tests
    const int Ntest = 100;

    // Timer
    Timer timer;

    // Average runtime table
    MatrixXd runtime_table(Ntimes, 6);
    runtime_table.col(0) = Ts;

    // Run tests for various time step lengths
    for (int i = 0; i < Ntimes; i++) {

        // Time step
        double T = Ts(i);

        //cout << "-------------------------------------------" << endl;
        cout << "Running T = " << T << " s" << endl;
        //cout << "-------------------------------------------" << endl;

        // Times
        double Ttot = 125 + 90 + 125 + 30 + 125;
        int steps = (int) ceil(Ttot / T);
        VectorXd t(steps);
        t.setLinSpaced(steps, 0, T*(steps-1));

        // True states
        vector<VectorXd> Xtru;
        for (int k = 0; k < steps; k++)
            Xtru.push_back(ct_tru(t(k)));

        // Save true states
        MatrixXd xtru_table(steps, 6);
        xtru_table.col(0) = t;
        for (int k = 0; k < steps; k++)
            xtru_table.row(k).tail(5) = Xtru[k];
        string filename = "out/ct_true_";
        filename += to_string(i+1);
        filename += ".csv";
        vector<string> header(6);
        header[0] = "Time (s)";
        header[1] = "x (m)";
        header[2] = "xd (m/s)";
        header[3] = "y (m)";
        header[4] = "yd (m/s)";
        header[5] = "Omega (rad/s)";
        EigenCSV::write(xtru_table, header, filename);

        // Process noise covariance
        double t22 = T * T / 2;
        double t33 = T * T * T / 3;
        MatrixXd Q(5,5);
        Q << t33, t22, 0,   0,   0,
             t22, T,   0,   0,   0,
             0,   0,   t33, t22, 0,
             0,   0,   t22, T,   0,
             0,   0,   0,   0,   (l2/l1)*T;
        Q *= l1;

        // HOUSE process noise distributions
        HOUSE::Dist distw(Q);

        // Initialize UKF & CUT filters
        UKF ukf (f, h, true, 0, mxi, Pxxi, Q, R, UKF::sig_type::JU,   1);
        UKF cut4(f, h, true, 0, mxi, Pxxi, Q, R, UKF::sig_type::CUT4, 1);
        UKF cut6(f, h, true, 0, mxi, Pxxi, Q, R, UKF::sig_type::CUT6, 1);
        UKF cut8(f, h, true, 0, mxi, Pxxi, Q, R, UKF::sig_type::CUT8, 1);

        // Initialize HOUSE
        HOUSE house(f, hh, 2, 0, distXi, distw, distn, 0.1);

        // Runtimes
        MatrixXd runtime(5, Ntest);

        // Run multiple tests
        for (int j = 0; j < Ntest; j++) {

            //cout << "Running Trial " << j+1 << endl;

            // Reset filters
            ukf.reset(0, mxi, Pxxi);
            cut4.reset(0, mxi, Pxxi);
            cut6.reset(0, mxi, Pxxi);
            cut8.reset(0, mxi, Pxxi);
            house.reset(0, distXi);

            // Measurements
            MatrixXd Ztru(2,steps);
            for (int k = 0; k < steps; k++) {
                Ztru.col(k) = ct_meas(t(k), Xtru[k]);
                Ztru(0,k) += sr  * nd(mt);
                Ztru(1,k) += sth * nd(mt);
            }

            // Run UKF
            //cout << "    UKF" << endl;
            timer.tick();
            ukf.run(t, Ztru);
            runtime(0, j) = timer.tock();
            ukf.save(est_filename("ukf", i, j));

            // Run CUT-4
            //cout << "    CUT-4" << endl;
            timer.tick();
            cut4.run(t, Ztru);
            runtime(1, j) = timer.tock();
            cut4.save(est_filename("cut4", i, j));

            // Run CUT-6
            //cout << "    CUT-6" << endl;
            timer.tick();
            cut6.run(t, Ztru);
            runtime(2, j) = timer.tock();
            cut6.save(est_filename("cut6", i, j));

            // Run CUT-8
            //cout << "    CUT-8" << endl;
            timer.tick();
            cut8.run(t, Ztru);
            runtime(3, j) = timer.tock();
            cut8.save(est_filename("cut8", i, j));

            // Run HOUSE
            //cout << "    HOUSE" << endl;
            timer.tick();
            house.run(t, Ztru);
            runtime(4, j) = timer.tock();
            house.save(est_filename("house", i, j));

        }

        // Average runtimes
        runtime_table.row(i).tail(5) = runtime.rowwise().mean();

    }

    // Save average runtimes
    vector<string> rth(6);
    rth[0] = "STEP";
    rth[1] = "UKF";
    rth[2] = "CUT-4";
    rth[3] = "CUT-6";
    rth[4] = "CUT-8";
    rth[5] = "HOUSE";
    EigenCSV::write(runtime_table, rth, "out/runtimes.csv");

    return 0;

}


// Copyright (c) 2020-2021 The PoWx Core developers

#include <crypto/heavyhash_dummyArray.h>

#include <matrix-utils/singular/Svd.h>
#include <test/util/setup_common.h>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(singular_tests, BasicTestingSetup)

using namespace std;
const int N = 4;
const int M = 64;
const double DATA[] = {
        1, 7, 7, 5,
        2, 1, 8, 10,
        1, 2, 9, 17,
        1, 2, 10, 2
};

const double WRONG_DATA1[] = {
        1, 7, 7, 5,
        1, 7, 7, 5,
        1, 2, 9, 17,
        1, 2, 10, 2
};

const double WRONG_DATA2[] = {
        0, 0, 0, 0,
        0, 0, 0, 0,
        0, 0, 0, 0,
        0, 0, 0, 0
};

static bool CheckCompatibility(const double *data) {
    singular::Matrix< N, N > matrix;
    matrix.fill(data);
    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < N; ++j) {
            if (data[i * N + j] != matrix(i, j))
                return false;
        }
    }
    return true;
}

static bool CheckMatrixRank4x4(const double *data) {
    singular::Matrix< N, N > matrix;
    matrix.fill(data);
    singular::Svd< N, N >::USV usv = singular::Svd< N, N >::decomposeUSV(matrix);
    const singular::DiagonalMatrix< N, N >& sing = singular::Svd< N, N >::getS(usv);
    cout << sing << endl;
    cout << endl;
    return singular::Svd< N, N >::isFullRank(sing, N);
}

static bool CheckMatrixRank64x64(const double *data) {
    singular::Matrix< M, M > matrix;
    matrix.fill(data);
    singular::Svd< M, M >::USV usv = singular::Svd< M, M >::decomposeUSV(matrix);
    const singular::DiagonalMatrix< M, M >& sing = singular::Svd< M, M >::getS(usv);
    cout << sing << endl;
    cout << endl;
    return singular::Svd< M, M >::isFullRank(sing, M);
}

static void ConvertReferenceArrayToInline(double* matrix) {
    for (int i = 0; i < 64; ++i) {
        for (int j = 0; j < 64; ++j) {
            matrix[64*i + j] = (double) reference_matrix[i][j];
        }
    }
}

BOOST_AUTO_TEST_CASE(raw_data_to_singular_matrix_conversion) {
        assert(CheckCompatibility(DATA));
}

BOOST_AUTO_TEST_CASE(compute_svd_of_a_matrix) {
        assert(CheckMatrixRank4x4(DATA));
        assert(!CheckMatrixRank4x4(WRONG_DATA1));
        assert(!CheckMatrixRank4x4(WRONG_DATA2));
}

BOOST_AUTO_TEST_CASE(check_full_rank_for_64x64) {
        double matrix[64*64];
        ConvertReferenceArrayToInline(matrix);
        assert(CheckMatrixRank64x64(matrix));
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <Eigen/dense>
#include "file_manage.h"
#include "model_selection.h"
#include "KMeans.h"
using namespace std;
using namespace Eigen;

int main()
{
	MatrixXd features;
	VectorXd labels;

	read_csv("data/iris.csv", features, labels, 150, 5);
	//read_csv("data/seeds.csv", features, labels, 210, 8);

	KMeans km(3);
	km.fit(features, 1, 5);

	double score = evaluate_model(km, features, labels);
	cout << "Silhouette score : " << score << endl;

	return 0;
}


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 8-jun-20
 * @license GPLv3, see 'LICENSE' file
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Quat1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>
#include <boost/math/quaternion.hpp>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<long double, double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_equals, t_real, t_types)
{
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;
	using t_quat = boost::math::quaternion<t_real>;

	t_real angle = tl2::pi<t_real>/t_real{4};
	auto vec = tl2::create<t_vec>({1, 2, 3});
	auto quat = tl2::rotation_quat<t_vec, t_quat>(vec, angle);

	auto [vec2, angle2] = tl2::rotation_axis<t_quat, t_vec>(quat);

	vec /= tl2::norm(vec);
	vec2 /= tl2::norm(vec2);

	std::cout << "q = " << quat << std::endl;
	std::cout << "axis1 = " << vec << ", angle1 = " << angle << std::endl;
	std::cout << "axis2 = " << vec2 << ", angle2 = " << angle2 << std::endl;

	BOOST_TEST(tl2::equals(vec, vec2, 1e-5));
	BOOST_TEST(tl2::equals<t_real>(angle, angle2, 1e-5));
}


#include <iostream>
#include <armadillo>
#include <random>
#include <math.h>
#include "NN3Layer.hpp"
using namespace std;
using namespace arma;

mat gen_and_example(int randNum){
    mat A(1,3);
    switch (randNum)
    {
    case 1:
        A={{0,0,0}};
        break;
    case 2:
        A={{0,1,0}};
        break;
    case 3:
        A={{1,0,0}};
        break;
    case 4:
        A={{1,1,1}};
        break;
    default:
        A={{1,1,1}};
        break;
    }
    return A;
}
mat gen_or_example(int randNum){
    mat A(1,3);
    switch (randNum)
    {
    case 1:
        A={{0,0,0}};
        break;
    case 2:
        A={{0,1,1}};
        break;
    case 3:
        A={{1,0,1}};
        break;
    case 4:
        A={{1,1,1}};
        break;
    default:
        A={{1,1,1}};
        break;
    }
    return A;
}
mat gen_xor_example(int randNum){
    mat A(1,3);
    switch (randNum)
    {
    case 1:
        A={{0,0,0}};
        break;
    case 2:
        A={{0,1,1}};
        break;
    case 3:
        A={{1,0,1}};
        break;
    case 4:
        A={{1,1,0}};
        break;
    default:
        A={{1,1,0}};
        break;
    }
    return A;
}

int main(int argc, char** argv){
      /* 隨機設備 */
  std::random_device rd;

  /* 亂數產生器 梅森旋轉演算法*/
  std::mt19937 generator( rd() );

  /* 亂數的機率分布 */
  std::uniform_real_distribution<float> unif(0.0, 5.0);
  NN3Layer nn3Layer(2,2,1,0.1);
  //train
  for( int a = 0; a < 100000; a++){
      /* 產生亂數 */
      float x = unif(generator);
      mat data = gen_xor_example((int)x);
      //data.print("data:");
      nn3Layer.train_step_sgd({data(0,0),data(0,1)},{data(0,2)});
  }
  //test
    while(true){
        cout << "Test Neural Net \n";
        cout << "Input X1:";
        double x1=0;
        cin >> x1;
        cout << "Input X2:";
        double x2=0;
        cin >> x2;
        mat o = nn3Layer.only_forward({x1,x2});
        o.print("o:");
        cout << "\n result:";
        if(o(0,0)>0.5){
            cout << 1<<"\n";
        }else{
            cout << 0<<"\n";
        };
        cout << "Enter 0 to end ,1 to keep testing.\n";
        int c=0;
        cin>>c;
        if(!c){
            break;
        }
    }
}


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

/** To enforce matrix output from root process only */
// #define ROOT_OUTPUT

/** To enforce matrix generation for local->local */
// #define LOCAL

/** To denote whether rowwise sketching is attempted (columnwise otherwise) */
#define ROWWISE

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

#include <boost/mpi.hpp>
#include <El.hpp>
#include <iostream>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

/** Aliases */

typedef El::Matrix<double>     dense_matrix_t;

typedef El::DistMatrix<double> dist_dense_matrix_t;
typedef El::DistMatrix<double, El::VC, El::STAR>
dist_VC_STAR_dense_matrix_t;
typedef El::DistMatrix<double, El::VR, El::STAR>
dist_VR_STAR_dense_matrix_t;
typedef El::DistMatrix<double, El::STAR, El::VC>
dist_STAR_VC_dense_matrix_t;
typedef El::DistMatrix<double, El::STAR, El::VR>
dist_STAR_VR_dense_matrix_t;

typedef El::DistMatrix<double, El::CIRC, El::CIRC>
dist_CIRC_CIRC_dense_matrix_t;

typedef El::DistMatrix<double, El::STAR, El::STAR>
dist_STAR_STAR_dense_matrix_t;

/* Set the following 2 typedefs for various matrix-type tests */
typedef dist_dense_matrix_t input_matrix_t;
typedef dist_dense_matrix_t output_matrix_t;

typedef skylark::sketch::JLT_t<input_matrix_t, output_matrix_t>
sketch_transform_t;

int main(int argc, char* argv[]) {

    /** Initialize MPI  */
    boost::mpi::environment env(argc, argv);
    boost::mpi::communicator world;

    /** Initialize Elemental */
    El::Initialize (argc, argv);

    MPI_Comm mpi_world(world);
    El::Grid grid(mpi_world);

    /** Example parameters */
    int height      = 20;
    int width       = 10;
    int sketch_size = 5;

    /** Define input matrix A */

#ifdef LOCAL
    input_matrix_t A;
    El::Uniform(A, height, width);
#else
    dist_CIRC_CIRC_dense_matrix_t A_CIRC_CIRC(grid);
    input_matrix_t A(grid);
    El::Uniform(A_CIRC_CIRC, height, width);
    A = A_CIRC_CIRC;
#endif

    /** Initialize context */
    skylark::base::context_t context(0);

#ifdef ROWWISE

    /** Sketch transform (rowwise)*/
    int size = width;
    /** Distributed matrix computation */
    output_matrix_t sketched_A(height, sketch_size);
    sketch_transform_t sketch_transform(size, sketch_size, context);
    sketch_transform.apply(A, sketched_A, skylark::sketch::rowwise_tag());

#else

    /** Sketch transform (columnwise)*/
    int size = height;
    /** Distributed matrix computation */
    output_matrix_t sketched_A(sketch_size, width);
    sketch_transform_t sketch_transform(size, sketch_size, context);
    sketch_transform.apply(A, sketched_A, skylark::sketch::columnwise_tag());

#endif

#ifdef ROOT_OUTPUT
    if (world.rank() == 0) {
#endif
        El::Print(sketched_A, "sketched_A");
#ifdef ROOT_OUTPUT
    }
#endif
    El::Finalize();
    return 0;
}


/**
 * @ file avgvalboundary.cc
 * @ brief NPDE homework AvgValBoundary code
 * @ author Simon Meierhans, edited by Oliver Rietmann
 * @ date 11.03.2019
 * @ copyright Developed at ETH Zurich
 */

#include "avgvalboundary.h"

#include <lf/assemble/assemble.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <cmath>
#include <memory>
#include <utility>
#include <vector>

namespace AvgValBoundary {

/**
 * @brief computes H1 seminorm over the computational domain
 * @param dofh DofHandler of FEspace.
 *        u coefficient vector
 */
/* SAM_LISTING_BEGIN_1 */
double compH1seminorm(const lf::assemble::DofHandler &dofh,
                      const Eigen::VectorXd &u) {
  double result = 0.0;

  // Compute Stiffness matrix with alpha := 1, beta := 0, gamma := 0
  auto const_one = [](Eigen::Vector2d x) -> double { return 1.0; };
  auto const_zero = [](Eigen::Vector2d x) -> double { return 0.0; };
  auto A = compGalerkinMatrix(dofh, const_one, const_zero, const_zero);

  // Compute seminorm using the Stiffness matrix
  result = u.transpose() * A * u;
  result = std::sqrt(result);
  return result;
}
/* SAM_LISTING_END_1 */

/**
 * @brief solves pde for some simple test problem with
 *        alpha = beta = gamma := 1.0 and load f := 1.0
 * @param dofh DofHandler of FEspace.
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solveTestProblem(const lf::assemble::DofHandler &dofh) {
  // Obtain Galerkin matrix for alpha = beta = gamma := 1.0
  auto const_one = [](Eigen::Vector2d x) -> double { return 1.0; };
  auto A = compGalerkinMatrix(dofh, const_one, const_one, const_one);

  // Set up load vector
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(dofh.Mesh());
  lf::mesh::utils::MeshFunctionConstant mf_identity{1.0};
  lf::uscalfe::ScalarLoadElementVectorProvider elvec_builder(fe_space,
                                                             mf_identity);
  Eigen::VectorXd phi(dofh.NumDofs());
  phi.setZero();
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  // Solve system of linear equations
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A);
  Eigen::VectorXd mu = solver.solve(phi);

  return mu;
}
/* SAM_LISTING_END_2 */

/** @brief generate sequence of nested triangular meshes with L+1 levels */
/* SAM_LISTING_BEGIN_3 */
std::shared_ptr<lf::refinement::MeshHierarchy> generateTestMeshSequence(
    unsigned int L) {
  auto mesh = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1.0 / 3.0);
  std::shared_ptr<lf::refinement::MeshHierarchy> meshes =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh, L);
  return meshes;
}
/* SAM_LISTING_END_3 */

/**
 * @brief Compute the boundary functional values for a sequence of L meshes
 * @param L The number of meshes in the sequence
 * @returns A vector of pairs containing the number of DOFs and value of the
 *	    boundary functional for each level
 */
/* SAM_LISTING_BEGIN_5 */
std::vector<std::pair<unsigned int, double>> approxBoundaryFunctionalValues(
    unsigned int L) {
  std::vector<std::pair<unsigned int, double>> result;
  auto meshes = generateTestMeshSequence(L - 1);
  int num_meshes = meshes->NumLevels();
  for (int level = 0; level < num_meshes; ++level) {
    auto mesh_p = meshes->getMesh(level);

    // Set up global FE space; lowest order Lagrangian finite elements
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    // Obtain local->global index mapping for current finite element space
    const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
    const lf::base::size_type N_dofs(dofh.NumDofs());

    // compute galerkin matrix with alpha = 1.0, gamma = 1.0, beta = 0.0
    auto const_one = [](Eigen::Vector2d x) -> double { return 1.0; };
    auto const_zero = [](Eigen::Vector2d x) -> double { return 0.0; };
    auto A = compGalerkinMatrix(dofh, const_one, const_one, const_zero);

    // compute load vector for f(x) = x.norm()
    auto f = [](Eigen::Vector2d x) -> double { return x.norm(); };
    lf::mesh::utils::MeshFunctionGlobal mf_f{f};
    lf::uscalfe::ScalarLoadElementVectorProvider elvec_builder(fe_space, mf_f);
    Eigen::VectorXd phi(N_dofs);
    phi.setZero();
    AssembleVectorLocally(0, dofh, elvec_builder, phi);

    // solve system of equations
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    solver.compute(A);
    Eigen::VectorXd u = solver.solve(phi);

    // set up weight function
    auto w = [](Eigen::Vector2d x) -> double { return 1.0; };
    double functional_value = compBoundaryFunctional(dofh, u, w);

    result.push_back({N_dofs, functional_value});
  }
  return result;
}
/* SAM_LISTING_END_5 */

}  // namespace AvgValBoundary


#include <blitzml/smooth_loss/squared_loss.h>
#include <blitzml/base/math_util.h>

namespace BlitzML {

value_t SquaredLoss::compute_loss(value_t a_dot_omega, value_t label) const {
  value_t residual = a_dot_omega - label;
  return sq(residual) / 2;
}

value_t SquaredLoss::compute_conjugate(value_t dual_variable,
                                       value_t label) const {
  return dual_variable * label + sq(dual_variable) / 2;
}

value_t SquaredLoss::compute_deriative(value_t a_dot_omega,
                                       value_t label) const {
  return a_dot_omega - label;
}

value_t SquaredLoss::compute_2nd_derivative(value_t a_dot_omega,
                                            value_t label) const {
  return 1.;
}

value_t SquaredLoss::compute_bias_update(
    const std::vector<value_t> &dual_values) const {
  value_t delta = -sum_vector(dual_values) / dual_values.size();
  return delta;
}

value_t SquaredLoss::lipschitz_constant() const {
  return 1.;
}

} // namespace BlitzML


//
// // Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//

#include <boost/gil/typedefs.hpp>
#include <boost/gil/image_processing/numeric.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>
#include <string>
#include <iostream>

namespace gil = boost::gil;

// Demonstrates the use of Sobel and Scharr filters to detect edges
// The example generates a couple of images representing respectively the result of
// applying the filter horizontally and vertically.
// The functions generating the filters (or kernels) are defined in include/boost/gil/image_processing/numeric.hpp.

int main(int argc, char* argv[])
{
    if (argc != 5)
    {
        std::cerr << "usage: " << argv[0] << ": <input.png> <sobel|scharr> <output-x.png> <output-y.png>\n";
        return -1;
    }

    gil::gray8_image_t input_image;
    gil::read_image(argv[1], input_image, gil::png_tag{});
    auto input = gil::view(input_image);
    auto filter_type = std::string(argv[2]);

    gil::gray16_image_t dx_image(input_image.dimensions());
    auto dx = gil::view(dx_image);
    gil::gray16_image_t dy_image(input_image.dimensions());
    auto dy = gil::view(dy_image);
    if (filter_type == "sobel")
    {
        gil::detail::convolve_2d(input, gil::generate_dx_sobel(1), dx);
        gil::detail::convolve_2d(input, gil::generate_dy_sobel(1), dy);
    }
    else if (filter_type == "scharr")
    {
        gil::detail::convolve_2d(input, gil::generate_dx_scharr(1), dx);
        gil::detail::convolve_2d(input, gil::generate_dy_scharr(1), dy);
    }
    else
    {
        std::cerr << "unrecognized gradient filter type. Must be either sobel or scharr\n";
        return -1;
    }

    gil::write_view(argv[3], dx, gil::png_tag{});
    gil::write_view(argv[4], dy, gil::png_tag{});
}


#include "NeuralNetwork.h"

#include <iostream>
#include <string>
#include <algorithm>
#include <vector>

#include <Eigen/Dense>

NeuralNetwork NeuralNetwork::from_stream(std::istream &is)
{
	float learning_factor;
    int num_layers;

    is.read((char *)&learning_factor, sizeof(learning_factor));
    is.read((char *)&num_layers, sizeof(num_layers));

	std::vector<int> sizes;

	std::cout << "reading size: ";

	for (int i = 0; i < num_layers; i++)
	{
		int size;
		is.read((char *)&size, sizeof(size));
		sizes.push_back(size);
		std::cout << size << " ";
	}

	std::cout << std::endl;

    NeuralNetwork nn(sizes, learning_factor);

	std::cout << "Sizes: " << std::endl;
	for (int size : nn.m_sizes)
	{
		std::cout << size << " ";
	}
	std::cout << std::endl;

	for (auto & weight_matrix : nn.m_weights)
	{
		for (int r = 0; r < weight_matrix.rows(); r++)
		{
			for (int c = 0; c < weight_matrix.cols(); c++)
			{
				is.read((char *)&weight_matrix(r, c), sizeof(float));
			}
		}
	}

    return nn;
}

NeuralNetwork::NeuralNetwork(std::vector<int> layer_sizes, float learning_factor)
	: m_num_layers(layer_sizes.size())
	, m_sizes(layer_sizes)
	, m_layers()
	, m_deltas()
	, m_weights()
	, m_learning_factor(learning_factor)
{
	if (m_num_layers == 0)
	{
		throw std::invalid_argument("The number of layers must be at least one!");
	}

	for (int i = 0; i < m_num_layers - 1; i++)
	{
		m_layers.push_back(Eigen::VectorXf::Ones(m_sizes[i] + 1)); // extra for the bias
	}

	m_layers.push_back(Eigen::VectorXf::Ones(m_sizes[m_num_layers - 1])); // output doesn't get bias

	for (int i = 1; i < m_num_layers; i++)
	{
		m_deltas.push_back(Eigen::VectorXf::Ones(m_sizes[i]));
		m_weights.push_back(Eigen::MatrixXf::Random(m_sizes[i], m_sizes[i - 1] + 1)); // extra for the bias
	}

	std::cout << m_layers.size() << std::endl;
}

void NeuralNetwork::forward_propagate(const Eigen::VectorXf& input)
{
    if (input.size() != m_sizes[0])
        throw std::invalid_argument("The input vector must be of size <" + std::to_string(m_sizes[0]) + ">");

	m_layers[0].head(m_sizes[0]) = input;

	for (size_t i = 1; i < m_layers.size(); ++i)
	{
		m_layers[i].head(m_sizes[i]) = (m_weights[i - 1] * m_layers[i - 1]).unaryExpr(&sigmoid);
	}
}

void NeuralNetwork::back_propagate(const Eigen::VectorXf& output)
{
    if (output.size() != m_sizes[m_num_layers-1])
        throw std::invalid_argument("The output vector must be of size <" + std::to_string(m_sizes[m_num_layers - 1]) + ">");

    Eigen::VectorXf error = output - m_layers[m_num_layers - 1];
	Eigen::VectorXf delta_o = error.array() * (m_layers[m_num_layers - 1].unaryExpr(&sigmoid_prime)).array();
	m_deltas[m_deltas.size() - 1] = delta_o;

	int delta_size = m_deltas.size();
	for (int delta_index = delta_size - 2; delta_index >= 0; delta_index--)
	{
		int layer_index = delta_index + 1;
		Eigen::VectorXf derivative = m_layers[layer_index].head(m_sizes[layer_index]).unaryExpr(&sigmoid_prime);
		Eigen::VectorXf err = (m_weights[delta_index + 1].transpose() * m_deltas[delta_index + 1]).head(m_sizes[layer_index]);

		Eigen::VectorXf delta = derivative.array() * err.array();

		m_deltas[delta_index] = delta;
	}

	int weights_size = m_weights.size();
	for (int i = 0; i < weights_size; ++i)
	{
		m_weights[i] += m_learning_factor * (m_deltas[i] * m_layers[i].transpose());
	}
}

Eigen::VectorXf NeuralNetwork::test(const Eigen::VectorXf& input, const Eigen::VectorXf& output)
{
    forward_propagate(input);
    return output - m_layers[m_num_layers - 1];
}

int NeuralNetwork::classify(const Eigen::VectorXf& input)
{
    float max_val = -1;
    int max_index = -1;

    forward_propagate(input);

    for (int i = 0; i < m_sizes[m_num_layers - 1]; ++i)
    {
        if (max_val < m_layers[m_num_layers - 1][i])
        {
            max_val = m_layers[m_num_layers - 1][i];
            max_index = i;
        }
    }

    return max_index;
}

std::vector<std::pair<int, float>> NeuralNetwork::classification(const Eigen::VectorXf& input)
{
	std::vector<std::pair<int, float>> classification_vector(m_sizes[m_num_layers - 1]);

	forward_propagate(input);

	for (int i = 0; i < m_sizes[m_num_layers - 1]; i++)
	{
		classification_vector.push_back({ i, m_layers[m_num_layers - 1][i] });
	}

	std::sort
	(
		classification_vector.begin(), classification_vector.end(),
		[](std::pair<int, float> lhs, std::pair<int, float> rhs)
		{
			return lhs.second > rhs.second;
		}
	);

	return classification_vector;
}

void NeuralNetwork::serialize(std::ostream &os) const
{
	os.write((char*)&m_learning_factor, sizeof(m_learning_factor));

	os.write((char *)&m_num_layers, sizeof(m_num_layers));

	for (int size : m_sizes)
	{
		os.write((char *)&size, sizeof(size));
	}

	for (const auto & weight_matrix : m_weights)
	{
		for (int r = 0; r < weight_matrix.rows(); r++)
		{
			for (int c = 0; c < weight_matrix.cols(); c++)
			{
				os.write((char *)&weight_matrix(r, c), sizeof(float));
			}
		}
	}
}

int NeuralNetwork::get_num_inputs() const
{
    return m_sizes[0];
}

int NeuralNetwork::get_num_outputs() const
{
    return m_sizes[m_num_layers - 1];
}

void NeuralNetwork::set_learning_factor(float learning_factor)
{
	m_learning_factor = learning_factor;
}

Eigen::VectorXf NeuralNetwork::get_output() const
{
	return m_layers[m_num_layers - 1];
}

const Eigen::MatrixXf& NeuralNetwork::get_weights_ih() const
{
    return m_weights[0];
}

const Eigen::MatrixXf& NeuralNetwork::get_weights_ho() const
{
    return m_weights[m_weights.size() - 1];
}


#ifndef TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP
#define TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP

#include <cmath>

// #include <boost/multiprecision/cpp_dec_float.hpp>

#include <triumf/constants/codata_2018.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/phenomenology.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace superconductivity {

// Pippard's phenomenological theory of superconductivity
namespace pippard {

///
template <typename T = double>
T J_0(T temperature, T critical_temperature, T gap_meV, T exponent) {
  // Boltzmann constant (meV / K)
  constexpr T k_B_meV_per_K =
      1e3 *
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  //
  // T reduced_temperature = temperature / critical_temperature;
  //
  return std::pow(phenomenology::reduced_penetration_depth<T>(
                      temperature, critical_temperature, exponent),
                  2) *
         bcs::reduced_gap<T>(temperature, critical_temperature) *
         std::tanh(bcs::gap<T>(temperature, critical_temperature, gap_meV) /
                   (2.0 * k_B_meV_per_K * temperature));
}

/// temperature dependence of the Pippard coherence length,
/// "borrowed" from BCS theory
template <typename T = double>
T coherence_length(T temperature, T critical_temperature, T gap_meV, T exponent,
                   T xi_0, T mean_free_path) {
  T fraction_1 =
      J_0<T>(temperature, critical_temperature, gap_meV, exponent) / xi_0;
  T fraction_2 = 1.0 / mean_free_path;
  T fraction = fraction_1 + fraction_2;
  // handle a some edge cases for very big/small args
  if (std::isinf(fraction)) {
    return 0.0;
  } else if (fraction == 0.0) {
    return std::numeric_limits<T>::infinity();
  } else {
    return 1.0 / fraction;
  }
}

/// temperature dependence of the (reduced) Pippard coherence length,
/// "borrowed" from BCS theory
template <typename T = double>
T reduced_coherence_length(T temperature, T critical_temperature, T gap_meV,
                           T exponent, T xi_0, T mean_free_path) {
  return coherence_length<T>(temperature, critical_temperature, gap_meV,
                             exponent, xi_0, mean_free_path) /
         coherence_length<T>(0.0, critical_temperature, gap_meV, exponent, xi_0,
                             mean_free_path);
}

/// helper function for the Pippard Kernel
template <typename T = double> T g(T x) {
  /*
  boost::multiprecision::cpp_dec_float_100 value =
      (3.0 / 2.0) * ((1.0 + x * x) * std::atan(x) - x) / (x * x * x);
  return value;
  */
  // return correct result when x ~ 0.
  if (x < 1.0e-4) {
    return 1.0;
  } else {
    return (3.0 / 2.0) * ((1.0 + x * x) * std::atan(x) - x) / (x * x * x);
  }
}

/// Pippard Kernel
template <typename T = double>
T kernel(T q, T temperature, T critical_temperature, T gap_meV, T xi_0,
         T mean_free_path, T lambda_0, T exponent) {
  T x = q * coherence_length<T>(temperature, critical_temperature, gap_meV,
                                exponent, xi_0, mean_free_path);
  return std::pow(phenomenology::penetration_depth<T>(
                      temperature, critical_temperature, exponent, lambda_0),
                  -2) *
         reduced_coherence_length<T>(temperature, critical_temperature, gap_meV,
                                     exponent, xi_0, mean_free_path) *
         g<T>(x);
}

/// (reduced) Pippard Kernel
template <typename T = double>
T reduced_kernel(T q, T temperature, T critical_temperature, T gap_meV, T xi_0,
                 T mean_free_path, T lambda_0, T exponent) {
  return kernel<T>(q, temperature, critical_temperature, gap_meV, xi_0,
                   mean_free_path, lambda_0, exponent) /
         kernel<T>(0.0, temperature, critical_temperature, gap_meV, xi_0,
                   mean_free_path, lambda_0, exponent);
}

/// (reduced) magnetic field penetration proifile
template <typename T = double>
T reduced_field_penetration(T z, T temperature, T critical_temperature,
                            T gap_meV, T xi_0, T mean_free_path, T lambda_0,
                            T exponent) {
  //
  if (z <= 0.0) {
    return 1.0;
  } else {
    //
    auto pippard_integrand = [&](T q) -> T {
      T K = kernel<T>(q, temperature, critical_temperature, gap_meV, xi_0,
                      mean_free_path, lambda_0, exponent);
      return q / (q * q + K);
    };
    // create the integrator w/ specific tolerance and evaluation levels
    // (defaults: root_epsilon and eight levels for type double).
    // static T tolerance = std::sqrt(std::numeric_limits<T>::epsilon());
    static T tolerance = std::cbrt(std::numeric_limits<T>::epsilon());
    std::size_t levels = sizeof(T);
    static boost::math::quadrature::ooura_fourier_sin<T> pippard_integrator =
        boost::math::quadrature::ooura_fourier_sin<T>(tolerance, levels);
    // evaluate the integral, which returns a pair
    // (first = integral, second = relative error)
    std::pair<T, T> result = pippard_integrator.integrate(pippard_integrand, z);
    // return the integral multiplied by the prefactors to get B vs. z
    return boost::math::constants::two_div_pi<T>() * result.first;
  }
}

/// magnetic field penetration proifile
template <typename T = double>
T field_penetration(T z, T temperature, T critical_temperature, T gap_meV,
                    T xi_0, T mean_free_path, T lambda_0, T exponent,
                    T applied_field) {
  return applied_field * reduced_field_penetration<T>(
                             z, temperature, critical_temperature, gap_meV,
                             xi_0, mean_free_path, lambda_0, exponent);
}

} // namespace pippard

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP


//
// This program is used to study how to calculate (SS|SS)^{m}
// for the recursive relation. Basically, it focus on the
// fmt function:
// f_{m}(t) = \int^{1}_{0} u^{2m} e^{-tu^{2}} du
// it needs the boost library to calcualte fmt
// fenglai liu
// Oct. 2013
//
// for reference, see Harris paper and the reference cited inside:
// Harris, Frank E
// Evaluation of GTO molecular integrals
// International Journal of Quantum Chemistry, 1983, Vol. 23, Page 1469--1478
//
// note:
// we found that the approximation of (2m-1)!!/R expression is
// not numerical stable. Combined with down recursive relation,
// it introduces greater error compared with other methods.
// Therefore we finally abadon it.
//
//
//

// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>

// common C++ head files may be used in the program
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<vector>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>

#include <boost/math/special_functions/binomial.hpp>  // special functions in math
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/timer.hpp>
using namespace boost::math;
using namespace boost;
using namespace std;
#define PI              3.1415926535897932384626E0

double fm(const double& a, const int& m);
double doubleFac(const int& m, const int& n);

double fm(const double& a, const int& m) {
	double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	double f12 = 1.0E0/2.0E0;
	double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

double doubleFac(const int& m, const int& n) {
	// calculate (2m-1)!!/(2m+2n+1)!!
	double x = 1.0E0;
	for(int i=0; i<=n; i++) {
		x *= 1.0E0/(2*m+2*i+1);
	}
	return x;
}

int main()
{
	///////////////////////////////////////////////////
	// global settings
	// step length and number of claculation for
	// time performance
	// global error is the error range
	// if the difference is less than the error, we
	// think the error could be omitted
	///////////////////////////////////////////////////
	double steplength  = 0.000001;
	double globalError = 1.0E-14;
	int top_M_limmit   = 40;
	double T_max_limit = 100.0E0;

	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the comparison between f_{m}(t) and f_{m+1}(t) for m up to " << top_M_limmit << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	int nSteps = T_max_limit/steplength;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			double T = steplength*j;
			double fmt= fm(T,n);
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				if (e0>e) {
					double d = fabs(e0-e);
					if (d>globalError) {
						printf("T is %-16.14f\n", T);
						printf("for m=%d difference is %-16.14f\n", n, d);
					}
				}
				e0 = e;
			}
		}
	}
	return 0;
}