#pragma once
#include <NTL/RR.h>
#include <utility>
#include "proper_primes.hpp"

// choice of the approximation praxis for the estimated fraction of an error
// to appear in the next iteration of a bit-flipping decoder
#define ROUNDING_PRAXIS round

/* Probability that a variable node is correct, and a parity equation involving
 * it is satisfied */
NTL::RR compute_p_cc(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t ? d_c - 1 : t;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < t ? t-(n-d_c) : 0;
    /* and it should be even, since the PC equation must be satisfied */
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound + 1 : LowerTHitBound;

    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j) ) /
                         to_RR( binomial_wrapper(n-1,t) );
    }
    return result;
}

/* Probability that a variable node is correct, and a parity equation involving
 * it is *not* satisfied */
NTL::RR compute_p_ci(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t ? d_c - 1 : t;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < t ? t-(n-d_c) : 1;
    /* and it should be odd, since the PC equation must be non satisfied */
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound : LowerTHitBound + 1;

    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j) )
                 / to_RR( binomial_wrapper(n-1,t) );
    }
    return result;
}

/* Probability that a variable node is *not* correct, and a parity equation involving
 * it is *not* satisfied */
NTL::RR compute_p_ic(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t UpperTBound = (d_c - 1) < t - 1 ? d_c - 1 : t - 1;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c-1) < (t-1) ? (t-1)-(n-d_c-1) : 0;
    /* and it should be even, since the PC equation must be unsatisfied (when
     * accounting for the one we are considering as already placed*/
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound + 1 : LowerTHitBound;

    for(uint64_t j = LowerTHitBound; j <= UpperTBound; j = j+2 ){
        result += NTL::to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j-1) )
                 / to_RR( binomial_wrapper(n-1,t-1) );
    }
    return result;
}

/* Probability that a variable node is *not* correct, and a parity equation involving
 * it is satisfied */
NTL::RR compute_p_ii(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){

    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t - 1 ? d_c - 1 : t - 1;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < (t-1) ? (t-1)-(n-d_c) : 1;
    /* and it should be odd, since the PC equation must be satisfied (when
     * accounting for the one we are considering as already placed)*/
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound : LowerTHitBound +1;
    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += NTL::to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j-1) )
                 / to_RR( binomial_wrapper(n-1,t-1) );
    }
    return result;
}

/* note p_cc + p_ci = 1 */
/* note p_ic + p_ii = 1 */

/* Probability that a given erroneous variable is deemed as such, and is thus
 * corrected, given a threshold for the amount of unsatisfied parity check
 * equations. Called P_ic in most texts */
NTL::RR ComputePrBitCorrection( const NTL::RR p_ic,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t threshold ){
// 		Pic=0; /* p_correct */
// 		for (j=b,dv,
// 			term=binomial(dv,j)*(p_ic^j)*(1-p_ic)^(dv-j);
// 			Pic=Pic+term;
// 		);
  NTL::RR result = NTL::RR(0), success, failure;
  for (uint64_t j = threshold; j <= d_v; j++){
     NTL::pow(success, p_ic, NTL::to_RR(j));
     NTL::pow(failure, NTL::RR(1)-p_ic, NTL::to_RR(d_v-j));
     result += NTL::to_RR(binomial_wrapper(d_v,j)) * success * failure;
  }
  return result;
}

/* Probability that a given correct variable is not deemed as such, and is thus
 * fault-induced, given a threshold for the amount of unsatisfied parity check
 * equations. Called P_ci in most texts, p_induce in official comment */
NTL::RR ComputePrBitFaultInduction( const NTL::RR p_ci,
                                    const uint64_t d_v,
                                    const uint64_t t, /* unused */
                                    const uint64_t threshold ){

  NTL::RR result= NTL::RR(0), success, failure;
  for (uint64_t j = threshold; j <= d_v; j++){
     NTL::pow(success, p_ci, NTL::to_RR(j));
     NTL::pow(failure, NTL::RR(1)-p_ci, NTL::to_RR(d_v-j));
     result += NTL::to_RR(binomial_wrapper(d_v,j)) * success * failure;
  }
  return result;
}

/* computes the probability that toCorrect bits are corrected
 * known as P{N_ic = toCorrect}  */
NTL::RR ComputePrBitCorrectionMulti( const NTL::RR p_ic,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t threshold,
                                const uint64_t toCorrect){
   NTL::RR ProbCorrectOne = ComputePrBitCorrection(p_ic,d_v,t,threshold);
   return NTL::to_RR(binomial_wrapper(t,toCorrect)) *
          NTL::pow(ProbCorrectOne,NTL::RR(toCorrect)) *
          NTL::pow(1-ProbCorrectOne,NTL::RR(t-toCorrect));
}

/* computes the probability that toInduce faults are induced
 * known as P{N_ci = toInduce} or Pr{f_wrong = to_induce} */
NTL::RR ComputePrBitInduceMulti(const NTL::RR p_ci,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t n,
                                const uint64_t threshold,
                                const uint64_t toInduce){
//    if(toInduce <= 1 ){
//        return NTL::RR(0);
//    }
   NTL::RR ProbInduceOne = ComputePrBitFaultInduction(p_ci,d_v,t,threshold);
   return NTL::to_RR(binomial_wrapper(n-t,toInduce)) *
          NTL::pow(ProbInduceOne,NTL::RR(toInduce)) *
          NTL::pow(1-ProbInduceOne,NTL::RR(n-t-toInduce));
}

uint64_t FindNextNumErrors(const uint64_t n_0,
                           const uint64_t p,
                           const uint64_t d_v,
                           const uint64_t t){
    NTL::RR p_ci, p_ic;
     p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
     p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    uint64_t t_next=t;
//      uint64_t best_threshold = (d_v - 1)/2;
    for(uint64_t i = (d_v - 1)/2; i <= d_v - 1; i++){
       NTL::RR t_approx=  t -
                          t * ComputePrBitCorrection(p_ic, d_v, t, i) +
                          (n_0*p - t) * ComputePrBitFaultInduction(p_ci, d_v, t, i);
       unsigned long int t_curr = NTL::conv<unsigned long int>(NTL::ROUNDING_PRAXIS(t_approx)) ;
       /*Note : we increase the threshold only if it improves strictly on the
        * predicted error correction. */
       if (t_curr < t_next){
          t_next = t_curr;
//           best_threshold = i;
       }
    }
    /* considering that any code will correct a single bit error, if
     * t_next == 1, we save a computation iteration and shortcut to t_next == 0*/
    if (t_next == 1) {
        t_next = 0;
    }
    return t_next;
}

/* computes the exact 1-iteration DFR and the best threshold on the number of
 * upcs to achieve it */
std::pair<NTL::RR,uint64_t> Find1IterDFR(const uint64_t n_0,
                                         const uint64_t p,
                                         const uint64_t d_v,
                                         const uint64_t t){
    NTL::RR p_ci, p_ic, P_correct, P_induce;
    NTL::RR DFR, best_DFR = NTL::RR(1);
    p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
    p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    uint64_t best_threshold = (d_v - 1)/2;
    for(uint64_t b = best_threshold; b <= d_v - 1; b++){
       DFR = NTL::RR(1) - ComputePrBitCorrectionMulti(p_ic, d_v, t, b, t) * ComputePrBitInduceMulti(p_ci,d_v,t,n_0*p,b,0);
       /*Note : we increase the threshold only if it improves strictly on the
        * predicted error correction. */
       if (DFR < best_DFR){
          best_DFR = DFR;
          best_threshold = b;
       }
    }
//     std::cout << best_threshold << std::endl;
    return std::make_pair(best_DFR,best_threshold);
}

/* computes the exact 1-iteration probability of leaving at most t_leftover
 * uncorrected errors out of t. */
std::pair<NTL::RR,uint64_t> Find1IterTLeftoverPr(const uint64_t n_0,
                                         const uint64_t p,
                                         const uint64_t d_v,
                                         const uint64_t t,
                                         const uint64_t t_leftover){
    NTL::RR p_ci, p_ic;
    NTL::RR DFR, best_DFR = NTL::RR(1);
    p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
    p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    int n= p*n_0;
    uint64_t best_threshold = (d_v + 1)/2;

    for(uint64_t b = best_threshold; b <= d_v ; b++){
       DFR = NTL::RR(0);
       NTL::RR P_correct = ComputePrBitCorrection(p_ic, d_v, t,b);
       NTL::RR P_induce = ComputePrBitFaultInduction(p_ci,d_v, t/* unused */,b);
       for(int tau = 0 ; tau <= t_leftover; tau++){
         for(int n_to_induce = 0 ; n_to_induce <= t_leftover; n_to_induce++) {
             NTL::RR prob_induce_n = NTL::to_RR(binomial_wrapper(n-t,n_to_induce)) *
                                     NTL::pow(P_induce,NTL::to_RR(n_to_induce)) *
                                     NTL::pow(NTL::RR(1)-P_induce,NTL::to_RR(n-t-n_to_induce));
             int n_to_correct = (int)t + n_to_induce - tau;
             NTL::RR prob_correct_n = NTL::to_RR(binomial_wrapper(t,n_to_correct));
                     prob_correct_n *= NTL::pow(P_correct,NTL::to_RR(n_to_correct));

                     prob_correct_n *= NTL::pow(NTL::RR(1)-P_correct,NTL::to_RR((int)t-n_to_correct)); /*unsigned exp?*/
             DFR += prob_correct_n*prob_induce_n;
         }
       }
       DFR = NTL::RR(1) - DFR;
       if (DFR < best_DFR){
          best_DFR = DFR;
          best_threshold = b;
       }
    }
    return std::make_pair(best_DFR,best_threshold);
}

// find minimum p which, asymptotically, corrects all errors
// search performed via binary search as the DFR is decreasing monot.
// in of p
uint64_t Findpth(const uint64_t n_0,
                 const uint64_t d_v_prime,
                 const uint64_t t){

    unsigned int prime_idx = 0, prime_idx_prec;
    uint64_t p = proper_primes[prime_idx];
    while(p < d_v_prime || p < t ){
          prime_idx++;
          p=proper_primes[prime_idx];
    }

    uint64_t hi, lo;
    lo = prime_idx;
    hi = PRIMES_NO;
    prime_idx_prec = lo;

    uint64_t limit_error_num = t;
    while(hi-lo > 1){
        prime_idx_prec = prime_idx;
        prime_idx = (lo+hi)/2;
        p = proper_primes[prime_idx];
        // compute number of remaining errors after +infty iters
        limit_error_num = t;
        uint64_t current_error_num;
//        std::cout << "using p:"<< p << ", errors dropping as ";
        do {
            current_error_num = limit_error_num;
            limit_error_num = FindNextNumErrors(n_0, p, d_v_prime, current_error_num);
//           std::cout << limit_error_num << " ";
           } while (
                     (limit_error_num != current_error_num) &&
                     (limit_error_num != 0)
                   );
//        std::cout << std::endl;
        if (limit_error_num > 0){
            lo = prime_idx;
        } else {
            hi = prime_idx;
        }
    }
    if(limit_error_num == 0) {
        return proper_primes[prime_idx];
    }
    return proper_primes[prime_idx_prec];
}


/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_DH_HPP
#define INTERPOL_DH_HPP

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

#include <hand_eye_calibration/DualQuaternion.h>

namespace interpol {

template<typename T>
class DH1;

//We have a new type for (screw) tangent since multiplication with scalar is defined different there
template<typename T>
class DH1Tangent
{
public:
    DH1Tangent(const Eigen::Quaternion<T>& r,const Eigen::Quaternion<T>& d) : m_real(r), m_dual(d) {}

    // dqTangent * scalar
    DH1Tangent<T> operator*(const T& scale) const{
        DH1Tangent<T> tt(m_real,m_dual);
        tt.m_real.w() *= scale; //theta
        tt.m_dual.w() *= scale; //d
        return tt;
    }

    //implemented below
    DH1<T> expScrewBen(void) const;
    DH1<T> expScrew(void) const;
    DH1<T> expT(bool useBenKenwrightImpl=false) const{
        return useBenKenwrightImpl ? expScrewBen() : expScrew();
    }

    Eigen::Quaternion<T> m_real; // real part
    Eigen::Quaternion<T> m_dual; // dual part

    DH1<T> toDH(void){//for derivative
        return DH1<T>(m_real,m_dual);
    }
};

// scalar * dqTangent
template<typename T>
inline DH1Tangent<T> operator*(T scale, DH1Tangent<T> t){
    t.m_real.w() *= scale;
    t.m_dual.w() *= scale;
    return t;
}
typedef DH1Tangent<float> DH1Tangentf;
typedef DH1Tangent<double> DH1Tangentd;

template<typename T>
DH1<T>
operator+(const DH1<T>& dq1, const DH1<T>& dq2);

template<typename T>
class DH1
{
public:
    DH1(){}
    DH1(const Eigen::Quaternion<T>& r,
        const Eigen::Quaternion<T>& d) : DH1(px::DualQuaternion<T>(r,d)) {}
    DH1(const Eigen::Quaternion<T>& r,
        const Eigen::Matrix<T, 3, 1>& t) : DH1(px::DualQuaternion<T>(r,t)) {}

    void fromScrew(T theta, T d,
                   const Eigen::Matrix<T, 3, 1>& l,
                   const Eigen::Matrix<T, 3, 1>& m);
    void toScrew  (T& theta, T& d, //added by us
                   Eigen::Matrix<T, 3, 1>& l,
                   Eigen::Matrix<T, 3, 1>& m) const;

    DH1Tangent<T> logScrewBen(void) const;
    DH1Tangent<T> logScrew(void) const;
    DH1Tangent<T> logT(bool useBenKenwrightImpl=false) const{
        return useBenKenwrightImpl ? logScrewBen() : logScrew();
    }

    DH1<T> conjugate(void) const { return DH1<T>(dq.conjugate());}
    DH1<T> normalized(void) const {return DH1<T>(dq.normalized());}
    Eigen::Quaternion<T> real() const {return dq.real();}
    Eigen::Quaternion<T> dual() const {return dq.dual();}
    Eigen::Quaternion<T> rotation(void) const {return dq.rotation();}
    Eigen::Matrix<T, 3, 1> translation(void) const {return dq.translation();}
    DH1<T> operator*(T scale) const {return DH1<T>(dq*scale);}
    DH1<T> operator*(const DH1<T>& other) const {return DH1<T>(dq*other.dq);}
    friend DH1<T> operator+<>(const DH1<T>& dq1, const DH1<T>& dq2);

    DH1<T>& operator*=(const DH1<T>& other);//added by us

private:
    DH1(const px::DualQuaternion<T>& dq) : dq(dq) {}
    px::DualQuaternion<T> dq;
    //there must be a mistake here for the dual part....
    //DH1<T> exp(void) const;
    //DH1<T> exp(const DH1<T> base) const;
    //px::DualQuaternion<T> log(void) const;
    //px::DualQuaternion<T> log(const px::DualQuaternion<T> &b) const;
};

template<typename T>
DH1<T>&
DH1<T>::operator*=(const DH1<T>& other)
{
    dq = dq * other.dq;
    return *this;
}

template<typename T>
DH1<T>
operator+(const DH1<T>& dh1, const DH1<T>& dh2)
{
    return DH1<T>(dh1.dq+dh2.dq);
}

template<typename T>
void
DH1<T>::fromScrew(T theta, T d,
        const Eigen::Matrix<T, 3, 1>& l,
        const Eigen::Matrix<T, 3, 1>& m)
{
    //was provided in original github implementation.
    dq.fromScrew(theta,d,l,m);
    //is equal to:
    //m_real = Eigen::AngleAxis<T>(theta, l);
    //m_dual.w() = -0.5*d * std::sin(0.5*theta);
    //m_dual.vec() = std::sin(0.5*theta) * m + d / 2.0 * std::cos(theta / 2.0) * l;
}

//modeled by hand as inverse of above
template<typename T>
void
DH1<T>::toScrew(T& theta, T& d,
        Eigen::Matrix<T, 3, 1>& l,
        Eigen::Matrix<T, 3, 1>& m) const
{

    Eigen::AngleAxis<T> ar(real()) ;
    theta=ar.angle();
    l=ar.axis();

    d = -2.0 * dual().w() / (std::sin(0.5*theta));
    m = (dual().vec() - 0.5*d*std::cos(0.5*theta)*l) / std::sin(0.5*theta);
}

//First half of ScLERPGeometricCosSin
template<typename T>
DH1Tangent<T>
DH1<T>::logScrewBen(void) const
{
    Eigen::Vector3d vr = real().vec();
    Eigen::Vector3d vd = dual().vec();
    double invr = 1.0 / vr.norm();

    // Screw parameters
    double  angle = 2.0 * std::acos( real().w() );
    double  pitch = -2.0 * dual().w() * invr;
    Eigen::Vector3d direction = vr * invr;
    Eigen::Vector3d moment = (vd - direction*pitch*real().w()*0.5)*invr;

    //TODO: absorb angle into norm of axis
    Eigen::Quaterniond realTangent;
    realTangent.w()=angle;//0
    realTangent.vec()=direction;//* angle

    //absorb translation into norm of moment
    Eigen::Quaterniond dualTangent;
    dualTangent.w()=pitch;//0;
    dualTangent.vec()=moment;// * pitch;

    return DH1Tangent<T>(realTangent, dualTangent);
}

//Second half of ScLERPGeometricCosSin
template<typename T>
DH1<T>
DH1Tangent<T>::expScrewBen(void) const
{
    //TODO: real and dual tangents should have w() = 0 and the norm absorbed into their vector part
    Eigen::Vector3d vr = m_real.vec();
    Eigen::Vector3d vd = m_dual.vec();

    double angle = m_real.w();//vr.norm();
    double pitch = m_dual.w();//vd.norm();

    Eigen::Vector3d direction = vr; // / angle;
    Eigen::Vector3d moment = vd;// / pitch;

    // Convert back to dual-quaternion
    double sinAngle = std::sin(0.5*angle);
    double cosAngle = std::cos(0.5*angle);

    Eigen::Vector3d axisReal = direction * sinAngle;
    Eigen::Quaterniond real(cosAngle, axisReal.x(),axisReal.y(),axisReal.z());

    Eigen::Vector3d axisDual = sinAngle*moment+pitch*0.5* cosAngle *direction;
    Eigen::Quaterniond dual(-pitch*0.5*sinAngle, axisDual.x(),axisDual.y(),axisDual.z());

    return DH1<T>(real, dual);
}

template<typename T>
DH1Tangent<T>
DH1<T>::logScrew(void) const
{
    T theta,d;
    Eigen::Matrix<T, 3, 1> axis, moment;
    toScrew(theta,d,axis,moment); //putting amount of angle or translation in w(), so it is not 0!

    //TODO: absorb angle and translation into norm of axis and moment
    Eigen::Quaternion<T> real,dual;
    real.w() = theta; //0;
    real.vec() = axis; //*theta;
//        real.w() = 0;
//        real.vec() = axis*theta;

    dual.w() = d; //0;
    dual.vec() = moment; //*d;

//        double m = moment.norm();
//        dual.w() /=m;
//        dual.vec() /= m;
//        cout<<"axis norm="<<axis.norm()<<"  moment norm="<<m<<" d="<<d<<endl;

    return DH1Tangent<T>(real, dual);
}

template<typename T>
DH1<T>
DH1Tangent<T>::expScrew(void) const
{
    DH1<T> dualQuat;
    //TODO: get angle and translation from norm of axis and moment
    dualQuat.fromScrew(
                m_real.w(),//vec().norm(),
                m_dual.w(),//vec().norm(),
                m_real.vec(),//.normalized(),
                m_dual.vec()//.normalized()
                       );
    return dualQuat;
}

typedef DH1<float> DH1f;
typedef DH1<double> DH1d;

} // ns interpol

#endif // INTERPOL_DH_HPP


////////////////////////////////////////////////////////////////////
//
// $Id: myEigen.hxx 2021/06/05 13:36:04 kanai Exp $
//
// Copyright (c) 2021 Takashi Kanai
// Released under the MIT license
//
////////////////////////////////////////////////////////////////////

#ifndef _MYEIGEN_HXX
#define _MYEIGEN_HXX 1

#include <cmath>
using namespace std;

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

// return (double)Math.acos(dot(v1)/v1.length()/v.length());
// Numerically, near 0 and PI are very bad condition for acos.
// In 3-space, |atan2(sin,cos)| is much stable.
template <typename T>
T angleT( Eigen::Matrix<T, 3, 1>& v1, Eigen::Matrix<T, 3, 1>& v2 ) {
  Eigen::Matrix<T, 3, 1> c = v1.cross(v2);
  T s = c.norm();

  return std::fabs(std::atan2(s, v1.dot(v2)));
};

//typedef angleT<float> anglef;

#if 0
float anglef( Eigen::Vector3f& v1, Eigen::Vector3f& v2 ) {
  // return (double)Math.acos(dot(v1)/v1.length()/v.length());
  // Numerically, near 0 and PI are very bad condition for acos.
  // In 3-space, |atan2(sin,cos)| is much stable.
  Eigen::Vector3f c = v1.cross(v2);
  float s = c.norm();

  return std::fabs(std::atan2(s, v1.dot(v2)));
};
#endif

#endif // _MYEIGEN_HXX


#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include "kron.hpp"

Eigen::SparseMatrix<double> buildC(const Eigen::MatrixXd &A){
	int n= A.cols();
	Eigen::SparseMatrix<double> C(n*n,n*n);
	Eigen::MatrixXd I= Eigen::MatrixXd::Identity(n,n);
	Eigen::MatrixXd C1,C2;
	kron(A,I,C1);kron(I,A,C2);
	C1+=C2;
	//std::cout << C1 << std::endl;
	std::vector <Eigen::Triplet<double> > triplets;
	for (int i=0; i<n*n; ++i){
		for (int j=0; j<n*n; ++j){
			if (C1(i,j)!=0){
				Eigen::Triplet<double> trpl(i,j,C1(i,j));
				triplets.push_back(trpl);
				}
		}
	}
	C.setFromTriplets(triplets.begin(),triplets.end());
	C.makeCompressed();
	return C;
	//TODO
};

void solveLyapunov(const Eigen::MatrixXd &A, Eigen::MatrixXd &X){
	int n= A.cols();
	Eigen::SparseMatrix<double> C;
	C= buildC(A);
	Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n,n);
	Eigen::VectorXd b=Eigen::MatrixXd::Map(I.data(),n*n,1);
	Eigen::VectorXd x;
	Eigen::SparseLU<Eigen::SparseMatrix<double> > solver; solver.compute(C);
	x=solver.solve(b);
	X=Eigen::MatrixXd::Map(x.data(),n,n);
};

int main(){
	int n=5;
	Eigen::MatrixXd A(n,n),X(n,n);
    A<<10, 2, 3, 4, 5, 6, 20, 8, 9, 1, 1, 2, 30, 4, 5, 6, 7, 8, 20, 0, 1, 2, 3, 4, 10;
	std::cout << A << std::endl;
	///Teilaufgabe 1g
	/*Eigen::SparseMatrix<double> C;
	C= buildC(A);
	std::cout << C << std::endl;*/
	solveLyapunov(A,X);
	std::cout << X << std::endl;

	///test
	/*Eigen::VectorXd y;
	y=Eigen::MatrixXd::Map(A.data(),4,1);
	std::cout << y << std::endl;*/
	return 0;
}

Nummerical Methods for CSE/PS2/BuildC.cpp


#include "writer.hpp"
#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>

void apply_boundary_conditions(Eigen::ArrayXXd &u, int k) {
    auto N = u.rows();
    u(0, k) = u(1, k);
    u(N - 1, k) = u(N - 2, k);
}

//----------------upwindFDBegin----------------
/// Uses forward Euler and upwind finite differences to compute u from time 0 to
/// time T
///
/// @param[in] u0 the initial conditions in the physical domain, excluding
/// ghost-points.
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
upwindFD(const Eigen::VectorXd &u0,
         double dt,
         double T,
         const std::function<double(double)> &a,
         const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));

    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
    //// ANCSE_START_TEMPLATE
    u.col(0).segment(1, N) = u0;
    apply_boundary_conditions(u, 0);
    time[0] = 0.0;
    //// ANCSE_END_TEMPLATE

    /* Main loop */
    //// ANCSE_START_TEMPLATE
    for (int k = 0; k < nsteps; k++) {
        for (int j = 1; j < N + 1; j++) {

            double x = xL + (j - 1) * dx;

            double uL = u(j - 1, k);
            double uM = u(j, k);
            double uR = u(j + 1, k);
            double ax = a(x);
            double c = 0.5 * dt / dx;

            u(j, k + 1)
                = uM - c * (ax * (uR - uL) - fabs(ax) * (uR - 2 * uM + uL));
        }
        /* Outflow boundary conditions */
        apply_boundary_conditions(u, k + 1);
        time[k + 1] = (k + 1) * dt;
    }
    //// ANCSE_END_TEMPLATE

    return {std::move(u), std::move(time)};
}
//----------------upwindFDEnd----------------

//----------------centeredFDBegin----------------
/// Uses forward Euler and centered finite differences to compute u from time 0
/// to time T
///
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
centeredFD(const Eigen::VectorXd &u0,
           double dt,
           double T,
           const std::function<double(double)> &a,
           const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));
    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
    //// ANCSE_START_TEMPLATE
    u.col(0).segment(1, N) = u0;
    apply_boundary_conditions(u, 0);
    time[0] = 0.0;
    //// ANCSE_END_TEMPLATE

    /* Main loop */
    //// ANCSE_START_TEMPLATE
    for (int k = 0; k < nsteps; k++) {
        for (int j = 1; j < N + 1; j++) {
            double x = xL + (j - 1) * dx;
            u(j, k + 1)
                = u(j, k)
                  - dt / (2.0 * dx) * a(x) * (u(j + 1, k) - u(j - 1, k));
        }

        /* Outflow boundary conditions */
        apply_boundary_conditions(u, k + 1);
        time[k + 1] = (k + 1) * dt;
    }
    //// ANCSE_END_TEMPLATE

    return {std::move(u), std::move(time)};
}
//----------------centeredFDEnd----------------

/* Initial condition: rectangle */
double ic(double x) {
    if (x < 0.25 || x > 0.75)
        return 0.0;
    else
        return 2.0;
}

int main() {
    double T = 2.0;
    double dt = 0.002; // Change this for timestep comparison
    int N = 101;

    double xL = 0.0;
    double xR = 5.0;
    auto domain = std::pair<double, double>{xL, xR};

    auto a = [](double x) { return std::sin(2.0 * M_PI * x); };

    Eigen::VectorXd u0(N);
    double h = (xR - xL) / (N - 1.0);
    /* Initialize u0 */
    for (int i = 0; i < u0.size(); i++) {
        u0[i] = ic(xL + h * i);
    }

    const auto &[u_upwind, time_upwind] = upwindFD(u0, dt, T, a, domain);
    writeToFile("time_upwind.txt", time_upwind);
    writeMatrixToFile("u_upwind.txt", u_upwind);

    const auto &[u_centered, time_centered] = centeredFD(u0, dt, T, a, domain);
    writeToFile("time_centered.txt", time_centered);
    writeMatrixToFile("u_centered.txt", u_centered);
}


#ifndef MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP
#define MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    // N(x | 0, 1)
    double GetStandardNormalDist(const double x);

    // d/dx N(x | 0, 1)
    double GetStandardNormalDistDerivative(const double x);

    // integral_{- inf, x} N(x' | 0, 1) dx'
    double GetStandardNormalDistCdf(const double x);

    // N(x | mu, sigma^2)
    double GetNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx N(x | mu, sigma^2)
    double GetNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // LogNormal(x | mu, sigma^2)
    double GetLogNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx LogNormal(x | mu, sigma^2)
    double GetLogNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // log{ LogNormal(x | mu, sigma^2) }
    double GetLogOfLogNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx log{ LogNormal(x | mu, sigma^2) }
    double GetLogOfLogNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // N(x | mu, Sigma)
    double GetNormalDist(const Eigen::VectorXd& x,
                         const Eigen::VectorXd& mu,
                         const Eigen::MatrixXd& Sigma_inv,
                         const double           Sigma_det);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP


#include <iostream>
#include <iterator>
#include <algorithm>
#include <math.h>
#include <cmath>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

class Fibonacci{
    cpp_int m_previousEven;
    cpp_int m_currentEven;
    cpp_int m_nextEven;

    cpp_int even_sum(int limit) {
        if(limit == 0){
            return 0;
        }
        cpp_int ef1 = 0, ef2 = 2;
        cpp_int sum = m_previousEven + m_currentEven;
        int count = 0;

        while (count <= limit-2) {
            count++;
            m_nextEven = 4*m_currentEven + m_previousEven;
            m_previousEven = m_currentEven;
            m_currentEven = m_nextEven;
            sum += m_currentEven;
        }

        return sum;
    }

public:
    Fibonacci(){
        m_previousEven = 0;
        m_currentEven = 2;
    }

    cpp_int get_even_sum(int limit){
        int m_totalEvenNumber;
        m_totalEvenNumber = limit / 3;
        return even_sum(m_totalEvenNumber);
    }

};

int main()
{
    Fibonacci obj;
    int m_input;
    std::cout<<"Enter nth number as Limit : ";
    std::cin>> m_input;
    std::cout<<"Sum of Even Number within limit: ";
    std::cout<<obj.get_even_sum(m_input);
    return 0;
}


/**
 * optimal_pow2_rational.cpp
 * This program finds a rational number of the form
 *     mult / 2^shift
 * which approximates a user-supplied fraction to within the roundoff
 * of multshiftround when multiplied by integers on a user-specified
 * range.
 *
 * Written in 2018 by Ben Tesch.
 * Originally distributed at https://github.com/slugrustle/numerical_routines
 *
 * To the extent possible under law, the author has dedicated all copyright
 * and related and neighboring rights to this software to the public domain
 * worldwide. This software is distributed without any warranty.
 * The text of the CC0 Public Domain Dedication should be reproduced at the
 * end of this file. If not, see http://creativecommons.org/publicdomain/zero/1.0/
 */

#include <cinttypes>
#include <iostream>
#include <limits>
#include <string>
#include <stdexcept>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/round.hpp>

typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<80, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::et_off> cpp_bin_float_80;

int main(int argc, char *argv[])
{
  std::cout << "\n cmdline arguments: [range min] [range max] [fraction]\n" << std::endl;

  if (argc != 4)
  {
    std::cout << " ERROR: this program must be run with 3 arguments.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 range_min;
  try { range_min = cpp_bin_float_80(argv[1]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: range min argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((range_min < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest())) |
      (range_min > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: range min argument is outside the allowed range [-2^63,2^64-1]" << std::endl;
    std::cout << "   which is [" << std::numeric_limits<int64_t>::lowest() << ", " << std::numeric_limits<uint64_t>::max() << "]\n" << std::endl;
    return 0;
  }
  if (range_min != boost::math::round(range_min))
  {
    std::cout << " ERROR: range min argument must be an integer.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 range_max;
  try { range_max = cpp_bin_float_80(argv[2]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: range max argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((range_max < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest())) |
      (range_max > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: range max argument is outside the allowed range [-2^63,2^64-1]" << std::endl;
    std::cout << "   which is [" << std::numeric_limits<int64_t>::lowest() << ", " << std::numeric_limits<uint64_t>::max() << "]\n" << std::endl;
    return 0;
  }
  if (range_max != boost::math::round(range_max))
  {
    std::cout << " ERROR: range max argument must be an integer.\n" << std::endl;
    return 0;
  }

  if (range_max < range_min)
  {
    std::cout << " ERROR: range max must be greater than or equal to range min.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 fraction;
  try { fraction = cpp_bin_float_80(argv[3]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: fraction argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((fraction < cpp_bin_float_80(0.0)) |
      (fraction > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: fraction argument is outside the allowed range [0.0,2.0^64-1.0]" << std::endl;
    std::cout << "   which is [0.0, " << std::numeric_limits<uint64_t>::max() << ".0]\n" << std::endl;
    return 0;
  }

  for (int8_t shift = 1; shift <= 63; shift++)
  {
    cpp_bin_float_80 two_exp     = cpp_bin_float_80(1ull << shift);
    cpp_bin_float_80 approx_mult = boost::math::round(fraction * two_exp);
    cpp_bin_float_80 min_prod    = range_min * approx_mult;
    cpp_bin_float_80 approx_min  = min_prod / two_exp;
    cpp_bin_float_80 max_prod    = range_max * approx_mult;
    cpp_bin_float_80 approx_max  = max_prod / two_exp;
    cpp_bin_float_80 ratio_min   = range_min * fraction;
    cpp_bin_float_80 ratio_max   = range_max * fraction;

    if (boost::multiprecision::fabs(approx_min-ratio_min) < cpp_bin_float_80(0.5) &&
        boost::multiprecision::fabs(approx_max-ratio_max) < cpp_bin_float_80(0.5))
    {
      std::cout << " The rational " << std::setprecision(24) << approx_mult << " / 2^" << static_cast<int16_t>(shift) << " = ";
      std::cout << std::setprecision(24) << approx_mult / two_exp << std::endl;
      std::cout << "   approximates fraction = " << std::setprecision(24) << fraction << std::endl;
      std::cout << "   to within roundoff when multiplied by numbers on the range" << std::endl;
      std::cout << "   [" << range_min << ", " << range_max << "].\n" << std::endl;

      std::cout << " The internal product" << std::endl;
      std::cout << "   " << range_min << " * " << approx_mult << " = " << std::setprecision(24) << min_prod << std::endl;
      if (min_prod == cpp_bin_float_80(0.0))
      {
        std::cout << "   will not underflow or overflow any integer type.\n" << std::endl;
      }
      else if (min_prod < cpp_bin_float_80(0.0))
      {
        if (min_prod < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest()))
        {
          std::cout << "   will underflow an int64_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int32_t>::lowest()))
        {
          std::cout << "   will underflow an int32_t but not an int64_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int16_t>::lowest()))
        {
          std::cout << "   will underflow an int16_t but not an int32_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int8_t>::lowest()))
        {
          std::cout << "   will underflow an int8_t but not an int16_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not underflow an int8_t.\n" << std::endl;
        }
      }
      else if (min_prod > cpp_bin_float_80(0.0))
      {
        if (min_prod > cpp_bin_float_80(std::numeric_limits<uint64_t>::max()))
        {
          std::cout << "   will overflow a uint64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int64_t>::max()))
        {
          std::cout << "   will overflow an int64_t but not a uint64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint32_t>::max()))
        {
          std::cout << "   will overflow a uint32_t but not an int64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int32_t>::max()))
        {
          std::cout << "   will overflow an int32_t but not a uint32_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint16_t>::max()))
        {
          std::cout << "   will overflow a uint16_t but not an int32_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int16_t>::max()))
        {
          std::cout << "   will overflow an int16_t but not a uint16_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint8_t>::max()))
        {
          std::cout << "   will overflow a uint8_t but not an int16_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int8_t>::max()))
        {
          std::cout << "   will overflow an int8_t but not a uint8_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not overflow an int8_t.\n" << std::endl;
        }
      }

      std::cout << " The internal product" << std::endl;
      std::cout << "   " << range_max << " * " << approx_mult << " = " << std::setprecision(24) << max_prod << std::endl;
      if (max_prod == cpp_bin_float_80(0.0))
      {
        std::cout << "   will not underflow or overflow any integer type.\n" << std::endl;
      }
      else if (max_prod < cpp_bin_float_80(0.0))
      {
        if (max_prod < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest()))
        {
          std::cout << "   will underflow an int64_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int32_t>::lowest()))
        {
          std::cout << "   will underflow an int32_t but not an int64_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int16_t>::lowest()))
        {
          std::cout << "   will underflow an int16_t but not an int32_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int8_t>::lowest()))
        {
          std::cout << "   will underflow an int8_t but not an int16_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not underflow an int8_t.\n" << std::endl;
        }
      }
      else if (max_prod > cpp_bin_float_80(0.0))
      {
        if (max_prod > cpp_bin_float_80(std::numeric_limits<uint64_t>::max()))
        {
          std::cout << "   will overflow a uint64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int64_t>::max()))
        {
          std::cout << "   will overflow an int64_t but not a uint64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint32_t>::max()))
        {
          std::cout << "   will overflow a uint32_t but not an int64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int32_t>::max()))
        {
          std::cout << "   will overflow an int32_t but not a uint32_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint16_t>::max()))
        {
          std::cout << "   will overflow a uint16_t but not an int32_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int16_t>::max()))
        {
          std::cout << "   will overflow an int16_t but not a uint16_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint8_t>::max()))
        {
          std::cout << "   will overflow a uint8_t but not an int16_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int8_t>::max()))
        {
          std::cout << "   will overflow an int8_t but not a uint8_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not overflow an int8_t.\n" << std::endl;
        }
      }
      return 0;
    }
  }

  std::cout << " No rational with base 2 denominator was found that" << std::endl;
  std::cout << "   approximates fraction = " << std::setprecision(24) << fraction << std::endl;
  std::cout << "   to within roundoff when multiplied by numbers on the range" << std::endl;
  std::cout << "   [" << range_min << ", " << range_max << "]" << std::endl;
  std::cout << "   for denominators ranging from 2 to 2^63 inclusive.\n" << std::endl;
  return 0;
}

/*
Creative Commons Legal Code

CC0 1.0 Universal

    CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE
    LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN
    ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS
    INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES
    REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS
    PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM
    THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED
    HEREUNDER.

Statement of Purpose

The laws of most jurisdictions throughout the world automatically confer
exclusive Copyright and Related Rights (defined below) upon the creator
and subsequent owner(s) (each and all, an "owner") of an original work of
authorship and/or a database (each, a "Work").

Certain owners wish to permanently relinquish those rights to a Work for
the purpose of contributing to a commons of creative, cultural and
scientific works ("Commons") that the public can reliably and without fear
of later claims of infringement build upon, modify, incorporate in other
works, reuse and redistribute as freely as possible in any form whatsoever
and for any purposes, including without limitation commercial purposes.
These owners may contribute to the Commons to promote the ideal of a free
culture and the further production of creative, cultural and scientific
works, or to gain reputation or greater distribution for their Work in
part through the use and efforts of others.

For these and/or other purposes and motivations, and without any
expectation of additional consideration or compensation, the person
associating CC0 with a Work (the "Affirmer"), to the extent that he or she
is an owner of Copyright and Related Rights in the Work, voluntarily
elects to apply CC0 to the Work and publicly distribute the Work under its
terms, with knowledge of his or her Copyright and Related Rights in the
Work and the meaning and intended legal effect of CC0 on those rights.

1. Copyright and Related Rights. A Work made available under CC0 may be
protected by copyright and related or neighboring rights ("Copyright and
Related Rights"). Copyright and Related Rights include, but are not
limited to, the following:

  i. the right to reproduce, adapt, distribute, perform, display,
     communicate, and translate a Work;
 ii. moral rights retained by the original author(s) and/or performer(s);
iii. publicity and privacy rights pertaining to a person's image or
     likeness depicted in a Work;
 iv. rights protecting against unfair competition in regards to a Work,
     subject to the limitations in paragraph 4(a), below;
  v. rights protecting the extraction, dissemination, use and reuse of data
     in a Work;
 vi. database rights (such as those arising under Directive 96/9/EC of the
     European Parliament and of the Council of 11 March 1996 on the legal
     protection of databases, and under any national implementation
     thereof, including any amended or successor version of such
     directive); and
vii. other similar, equivalent or corresponding rights throughout the
     world based on applicable law or treaty, and any national
     implementations thereof.

2. Waiver. To the greatest extent permitted by, but not in contravention
of, applicable law, Affirmer hereby overtly, fully, permanently,
irrevocably and unconditionally waives, abandons, and surrenders all of
Affirmer's Copyright and Related Rights and associated claims and causes
of action, whether now known or unknown (including existing as well as
future claims and causes of action), in the Work (i) in all territories
worldwide, (ii) for the maximum duration provided by applicable law or
treaty (including future time extensions), (iii) in any current or future
medium and for any number of copies, and (iv) for any purpose whatsoever,
including without limitation commercial, advertising or promotional
purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each
member of the public at large and to the detriment of Affirmer's heirs and
successors, fully intending that such Waiver shall not be subject to
revocation, rescission, cancellation, termination, or any other legal or
equitable action to disrupt the quiet enjoyment of the Work by the public
as contemplated by Affirmer's express Statement of Purpose.

3. Public License Fallback. Should any part of the Waiver for any reason
be judged legally invalid or ineffective under applicable law, then the
Waiver shall be preserved to the maximum extent permitted taking into
account Affirmer's express Statement of Purpose. In addition, to the
extent the Waiver is so judged Affirmer hereby grants to each affected
person a royalty-free, non transferable, non sublicensable, non exclusive,
irrevocable and unconditional license to exercise Affirmer's Copyright and
Related Rights in the Work (i) in all territories worldwide, (ii) for the
maximum duration provided by applicable law or treaty (including future
time extensions), (iii) in any current or future medium and for any number
of copies, and (iv) for any purpose whatsoever, including without
limitation commercial, advertising or promotional purposes (the
"License"). The License shall be deemed effective as of the date CC0 was
applied by Affirmer to the Work. Should any part of the License for any
reason be judged legally invalid or ineffective under applicable law, such
partial invalidity or ineffectiveness shall not invalidate the remainder
of the License, and in such case Affirmer hereby affirms that he or she
will not (i) exercise any of his or her remaining Copyright and Related
Rights in the Work or (ii) assert any associated claims and causes of
action with respect to the Work, in either case contrary to Affirmer's
express Statement of Purpose.

4. Limitations and Disclaimers.

 a. No trademark or patent rights held by Affirmer are waived, abandoned,
    surrendered, licensed or otherwise affected by this document.
 b. Affirmer offers the Work as-is and makes no representations or
    warranties of any kind concerning the Work, express, implied,
    statutory or otherwise, including without limitation warranties of
    title, merchantability, fitness for a particular purpose, non
    infringement, or the absence of latent or other defects, accuracy, or
    the present or absence of errors, whether or not discoverable, all to
    the greatest extent permissible under applicable law.
 c. Affirmer disclaims responsibility for clearing rights of other persons
    that may apply to the Work or any use thereof, including without
    limitation any person's Copyright and Related Rights in the Work.
    Further, Affirmer disclaims responsibility for obtaining any necessary
    consents, permissions or other rights required for any use of the
    Work.
 d. Affirmer understands and acknowledges that Creative Commons is not a
    party to this document and has no duty or obligation with respect to
    this CC0 or use of the Work.
*/


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <vtkDoubleArray.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkPolyDataWriter.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;
typedef Eigen::Quaterniond Quaterniond;
typedef Eigen::AngleAxisd AngleAxisd;

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

  // We expect four command line arguments:
  // 1. Number of particles in the simulation
  // 2. Name of data file from which to read rows
  // 3. Row number to plot -- 0 indexed row number
  // 4. Output VTK file name
  if (argc < 4)
  {
    std::cout << "Usage: ./plotRow <number_of_particles> <data_file> "
              << "<row_num> <output_vtk_file>" << std::endl;
    return 0;
  }

  // The number of particles in the simulation
  size_t N = std::stoi(std::string(argv[1]));

  // The row number to be written to vtk file
  size_t row = std::stoi(std::string(argv[3]));

  std::ifstream inputfile(argv[2]);
  if (!inputfile.is_open())
  {
    std::cout << "Unable to open file " << argv[2] << std::endl;
    return 0;
  }

  //**********************************************************************//
  // Now read the data file line by line
  //

  // The first line contains gamma and beta information
  std::string line, ignore;
  double_t gamma = 0, beta = 0;
  std::getline(inputfile, line);
  std::istringstream header(line);
  header >> ignore >> gamma >> ignore >> beta;

  // Skip to the required row
  auto rowCount = 0;
  while (rowCount < row)
  {
    std::getline(inputfile, line);
    rowCount++;
    continue;
  }

  std::getline(inputfile, line);
  std::cout << "Processing row " << rowCount << std::endl;
  std::istringstream rowStream(line);

  // We will extract 3*N doubles representing particle positions
  // from the stream
  std::string value;
  size_t valCount = 0;
  Matrix3Xd positions(3, N);
  while (valCount < 3 * N && rowStream.good())
  {
    std::getline(rowStream, value, ',');
    positions(valCount % 3, valCount / 3) = std::stod(value);
    valCount++;
  }

  // Read the rotation vectors
  Matrix3Xd rotVecs(3, N);
  valCount = 0;
  while (valCount < 3 * N && rowStream.good())
  {
    std::getline(rowStream, value, ',');
    rotVecs(valCount % 3, valCount / 3) = std::stod(value);
    valCount++;
  }

  // Calculate point normals using the rotation vectors
  Matrix3Xd normals(3, N);
  Quaterniond zaxis(0.0, 0.0, 0.0, 1.0);
  for (auto i = 0; i < N; ++i)
  {
    normals.col(i) = (Quaterniond(AngleAxisd(rotVecs.col(i).norm(),
                                             rotVecs.col(i).normalized())) *
                      zaxis *
                      (Quaterniond(AngleAxisd(rotVecs.col(i).norm(),
                                              rotVecs.col(i).normalized()))
                           .conjugate()))
                         .vec();
  }

  // Stereo graphic projection
  Matrix3Xd points(3, N);

  // Project points to unit sphere
  points = positions.colwise().normalized();

  // Reset the center of the sphere to origin by translating
  Vector3d center = points.rowwise().mean();
  points = points.colwise() - center;

  // Rotate all points so that the point in 0th column is along z-axis
  Vector3d c = points.col(0);
  double_t cos_t = c(2);
  double_t sin_t = std::sqrt(1 - cos_t * cos_t);
  Vector3d axis;
  axis << c(1), -c(0), 0.;
  Matrix3d rotMat, axis_cross, outer;
  axis_cross << 0., -axis(2), axis(1), axis(2), 0., -axis(0), -axis(1), axis(0),
      0.;

  outer.noalias() = axis * axis.transpose();

  rotMat =
      cos_t * Matrix3d::Identity() + sin_t * axis_cross + (1 - cos_t) * outer;
  Matrix3Xd rPts(3, N);
  rPts = rotMat * points; // The points on a sphere rotated

  // Calculate the stereographic projections
  Vector3d p0;
  Map3Xd l0(&(rPts(0, 1)), 3, N - 1);
  Matrix3Xd l(3, N - 1), proj(3, N - 1);
  p0 << 0, 0, -1;
  c = rPts.col(0);
  l = (l0.colwise() - c).colwise().normalized();
  for (auto j = 0; j < N - 1; ++j)
  {
    proj.col(j) = ((p0(2) - l0(2, j)) / l(2, j)) * l.col(j) + l0.col(j);
  }

  // Insert the projected points in a CGAL vertex_with_info vector
  std::vector<std::pair<Point, unsigned>> verts;
  for (auto j = 0; j < N - 1; ++j)
  {
    verts.push_back(std::make_pair(Point(proj(0, j), proj(1, j)), j + 1));
  }

  // Triangulate
  Delaunay dt;
  dt.insert(verts.begin(), verts.end());

  // Write the finite faces of the triangulation to a VTK file
  auto triangles = vtkSmartPointer<vtkCellArray>::New();
  for (auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
       ++ffi)
  {
    triangles->InsertNextCell(3);
    for (auto j = 2; j >= 0; --j)
      triangles->InsertCellPoint(ffi->vertex(j)->info());
  }

  // Iterate over infinite faces
  Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
  if (fc != 0)
  {
    do
    {
      triangles->InsertNextCell(3);
      for (auto j = 2; j >= 0; --j)
      {
        auto vh = fc->vertex(j);
        auto id = dt.is_infinite(vh) ? 0 : vh->info();
        triangles->InsertCellPoint(id);
      }
    } while (++fc != done);
  }

  // Write to vtk file
  auto ptsArr = vtkSmartPointer<vtkDoubleArray>::New();
  ptsArr->SetVoidArray((void *)positions.data(), 3 * N, 1);
  ptsArr->SetNumberOfComponents(3);
  auto pts = vtkSmartPointer<vtkPoints>::New();
  pts->SetData(ptsArr);
  auto poly = vtkSmartPointer<vtkPolyData>::New();
  poly->SetPoints(pts);
  poly->SetPolys(triangles);
  auto normArr = vtkSmartPointer<vtkDoubleArray>::New();
  normArr->SetVoidArray((void *)normals.data(), 3 * N, 1);
  normArr->SetNumberOfComponents(3);
  poly->GetPointData()->SetNormals(normArr);
  auto wr = vtkSmartPointer<vtkPolyDataWriter>::New();
  wr->SetFileName(argv[4]);
  wr->SetInputData(poly);
  wr->Write();

  inputfile.close();
  return 0;
}


#include <iostream>
using namespace std;

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

int main( int argc, char** argv )
{
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix<float,3,1> vd_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Ones();
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << matrix_23 << endl;

    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;

    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl << endl;
    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << 10*matrix_33 << endl;
    cout << matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl;

    cout <<"hello world"  << endl;

    return 0;
}


#include <iostream>
#include <armadillo>
#include <tuple>
#include <cstdlib>     /* srand, rand */
#include <ctime>       /* time */
#include "logistic_function.hpp"
#include "neural.hpp"
using matrix=arma::Mat<double>;
size_t const max_epochs = 1000;
const double lambda = 0.8;
const double tolerance = 0.000001;
/**
Evaluate Output
 Given
  Input  data X(input_dim,1)
  hidden-layer (input_dim,hidden_layer_size)
  Output (hidden_layer_size,out_put_size)
 Compute
  Output of FFNN

*/
matrix Evaluate(matrix const& X, matrix const& hiddenLayer, matrix const& outputLayer)
	{
	matrix inp = X; //21*m
	matrix hidden = inp.t()*hiddenLayer; // m*21*21*4 -> m*4
	matrix output = LogisticFunction::fn(hidden)*outputLayer; // m*4 * 4 *3 -> m*3
	return LogisticFunction::fn(output).t();//3*m
	}

matrix EvaluateWithBias(matrix const& inp, matrix const& hiddenLayer, matrix const& outputLayer)
	{

	matrix hidden = hiddenLayer.t()*inp; //  4*m
	hidden = LogisticFunction::fn(hidden);//4*m
	matrix hiddenInput = arma::join_cols(arma::ones(1, inp.n_cols), hidden);//5*m
	matrix output = outputLayer.t()*hiddenInput; // (5.3)'*5.m -> 3.m
	return LogisticFunction::fn(output);
	}
/**
Calculate cost using cross entropy
*/
double CalcCost(matrix const& target, matrix const& output)
	{

	double sum = 0.0;
	size_t item_count = target.n_elem;
	for (size_t i = 0; i < item_count; ++i)
		{
		double y = target[i];
		double val = output[i];
		sum = sum - y*log(val) - (1 - y)*log(1 - val);
		}
	return sum / target.n_cols;
	}

/**Back Propogation
 Ignore lambda
 Problem statement:
 Given
  X the feature mector of m column vectors of size feature_size
  Y the result vector of m column vectors of label_size
  hidden_layer_size
  Theta1: weight vector of hidden_layer_size columns each of size (feature_size )
  Theta2: weight vector of label_size columns each of size (hidden_layer_size)
Compute
  cost using cross entropy
  Theta1_gardient: weight vector of hidden_layer_size columns each of size (feature_size )
  Theta2_gradient: weight vector of size label_size columns each of size (hidden_layer_size )
  ...using sigmoid

Updates:
	Adding bias

*/
std::tuple<double,matrix,matrix>
BackProp(
	 matrix & a1, matrix & y, matrix const& Theta1, matrix const& Theta2
	)
	{

	// This generates [0 1 2 3 ... (ElementCount(trainingData) - 1)]. The
	// sequence will be used to iterate through the training data.
	size_t m = a1.n_cols;

	matrix theta1_gradient(Theta1);
	theta1_gradient.zeros();
	matrix theta2_gradient(Theta2);
	theta2_gradient.zeros();

	matrix output(Theta2.n_cols, 1);

	for (size_t j = m; j > 0; --j)
		{
		int k = rand() % j;
		a1.swap_cols(j - 1, k);
		y.swap_cols(j - 1, k);
		}
	output = EvaluateWithBias(a1, Theta1, Theta2);
	double cost = CalcCost(y, output);

	matrix z2 = Theta1.t()*a1;
	matrix a2 = arma::join_cols(arma::ones(1, m), LogisticFunction::fn(z2));

	matrix z3 = Theta2.t()*a2;
	matrix a3 = LogisticFunction::fn(z3);
	matrix delta3 = a3 - y;
	theta2_gradient += a2*delta3.t();
	matrix delta2 = (Theta2*delta3) % arma::join_cols(arma::ones(1, m), LogisticFunction::deriv(a2.rows(1, a2.n_rows - 1)));
	delta2 = delta2.rows(1, delta2.n_rows-1);
	theta1_gradient += a1*delta2.t();
	theta1_gradient = theta1_gradient / m + lambda*arma::join_cols(arma::zeros(1, Theta1.n_cols), Theta1.rows(1, Theta1.n_rows - 1));
	theta2_gradient = theta2_gradient / m + lambda*arma::join_cols(arma::zeros(1, Theta2.n_cols), Theta2.rows(1, Theta2.n_rows - 1));
	return std::make_tuple(cost, theta1_gradient, theta2_gradient);
	}

/**
Train Network
 Ignore lambda and bias
 Given:
  X the feature mector of m column vectors of size feature_size
  Y the result vector of m column vectors of label_size
  hidden_layer_size
 Compute:
  Theta1: weight vector of hidden_layer_size columns each of size (feature_size + 1)
  Theta2: weight vector of size label_size columns each of size (hidden_layer_size +1)
 Update:
  Adding bias
*/

std::tuple<matrix,matrix>
TrainNetwork(
	matrix const& X, matrix y,size_t hidden_layer_size
	)
	{
	matrix theta1;
	theta1.randu(X.n_rows+1, hidden_layer_size);
	matrix theta2;
	theta2.randu(hidden_layer_size+1, y.n_rows);
	double eta =0.5;
	size_t m = X.n_cols;
	matrix a1 = arma::join_cols(arma::ones(1, m), X);
	double prev_cost = 1000.0;
	for(size_t epoch = 0; epoch < max_epochs; ++epoch)
		{
		matrix delta1,delta2;
		double cost=0.0;
		std::tie(cost,delta1,delta2) = BackProp(a1,y,theta1,theta2);
		theta1 = theta1 - eta *  delta1;
		theta2 = theta2 - eta *  delta2;
		//std::cout << epoch << ":" << cost << std::endl;
		if (abs(prev_cost - cost) < tolerance)
			break;
		prev_cost = cost;
		}
	return std::make_tuple(theta1,theta2);
	}

/**
Predict
	Given
	 Neural Network
	 Input sample
    Compute
	 Class it belongs to
*/
matrix Predict(matrix const& input, matrix const& hiddenLayer, matrix const& outputLayer)
	{
	matrix output = EvaluateWithBias(input, hiddenLayer, outputLayer);
	matrix result(output.n_rows, output.n_cols);
	for (size_t i = 0; i < output.n_cols; ++i)
		{
		matrix cur = output.unsafe_col(i);
		double max = cur[0];
		size_t index = 0;
		for (size_t j = 1; j < cur.n_elem; ++j)
			{
			if (cur[j] > max)
				{
				max = cur[j];
				index = j;
				}
			}
		for (size_t j = 0; j < output.n_rows; ++j)
			{
			result(j, i) = index == j ? 1.0 : 0.0;
			}
		}
	return result;
	}
double ComputeError(matrix const& result, matrix const& labels)
	{
	double error = 0.0;
	for (size_t j = 0; j < result.n_elem; ++j)
		error += abs(result[j] - labels[j]);
	return error / 2.0;
	}


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Deliberately contains some unicode characters:
//
// boost-no-inspect
//
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/simple_continued_fraction.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::math::constants::root_two;
using boost::math::constants::phi;
using boost::math::constants::pi;
using boost::math::constants::e;
using boost::math::constants::zeta_three;
using boost::math::tools::simple_continued_fraction;

int main()
{
    using Real = boost::multiprecision::cpp_bin_float_100;
    auto phi_cfrac = simple_continued_fraction(phi<Real>());
    std::cout << "φ ≈ " << phi_cfrac << "\n\n";

    auto pi_cfrac = simple_continued_fraction(pi<Real>());
    std::cout << "π ≈ " << pi_cfrac << "\n";
    std::cout << "Known: [3; 7, 15, 1, 292, 1, 1, 1, 2, 1, 3, 1, 14, 2, 1, 1, 2, 2, 2, 2, 1, 84, 2, 1, 1, 15, 3, 13, 1, 4, 2, 6, 6, 99, 1, 2, 2, 6, 3, 5, 1, 1, 6, 8, 1, 7, 1, 2, 3, 7, 1, 2, 1, 1, 12, 1, 1, 1, 3, 1, 1, 8, 1, 1, 2, 1, 6, 1, 1, 5, 2, 2, 3, 1, 2, 4, 4, 16, 1, 161, 45, 1, 22, 1, 2, 2, 1, 4, 1, 2, 24, 1, 2, 1, 3, 1, 2, 1, ...]\n\n";

    auto rt_cfrac = simple_continued_fraction(root_two<Real>());
    std::cout << "√2 ≈ " << rt_cfrac << "\n\n";

    auto e_cfrac = simple_continued_fraction(e<Real>());
    std::cout << "e ≈ " << e_cfrac << "\n";

    // Correctness can be checked in Mathematica via: ContinuedFraction[Zeta[3], 500]
    auto z_cfrac = simple_continued_fraction(zeta_three<Real>());
    std::cout << "ζ(3) ≈ " << z_cfrac << "\n";
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/Interpolation_gradient_fitting_traits_2.h>
#include <CGAL/sibson_gradient_fitting.h>
#include <CGAL/interpolation_functions.h>

#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Regular_triangulation_2.h>

#include <boost/iterator/function_output_iterator.hpp>

#include <iostream>
#include <iterator>
#include <utility>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel    K;
typedef CGAL::Interpolation_gradient_fitting_traits_2<K>       Traits;

typedef K::FT                                                  Coord_type;
typedef K::Point_2                                             Bare_point;
typedef K::Weighted_point_2                                    Weighted_point;
typedef K::Vector_2                                            Vector;

template <typename V, typename G>
struct Value_and_gradient
{
  Value_and_gradient() : value(), gradient(CGAL::NULL_VECTOR) {}

  V value;
  G gradient;
};

typedef CGAL::Triangulation_vertex_base_with_info_2<
                Value_and_gradient<Coord_type, Vector>, K,
                CGAL::Regular_triangulation_vertex_base_2<K> > Vb;
typedef CGAL::Regular_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb>           Tds;
typedef CGAL::Regular_triangulation_2<K, Tds>                  Regular_triangulation;
typedef Regular_triangulation::Vertex_handle                   Vertex_handle;

int main()
{
  Regular_triangulation rt;

  auto value_function = [](const Vertex_handle& a) -> std::pair<Coord_type, bool>
  {
    return std::make_pair(a->info().value, true);
  };

  auto gradient_function = [](const Vertex_handle& a) -> std::pair<Vector, bool>
  {
    return std::make_pair(a->info().gradient, a->info().gradient != CGAL::NULL_VECTOR);
  };

  auto gradient_output_iterator
    = boost::make_function_output_iterator
    ([](const std::pair<Vertex_handle, Vector>& p)
     {
       p.first->info().gradient = p.second;
     });

  // parameters for spherical function:
  Coord_type a(0.25), bx(1.3), by(-0.7), c(0.2);
  for (int y=0; y<4; y++) {
    for (int x=0; x<4; x++) {
      Weighted_point p(Bare_point(x,y), (x-y)/3. /*weight*/);
      Vertex_handle vh = rt.insert(p);
      Coord_type value = a + bx*x + by*y + c*(x*x+y*y);
      vh->info().value = value;
    }
  }

  CGAL::sibson_gradient_fitting_rn_2(rt,
                                     gradient_output_iterator,
                                     CGAL::Identity<std::pair<Vertex_handle, Vector> >(),
                                     value_function,
                                     Traits());

  // coordinate computation
  Weighted_point p(Bare_point(1.6, 1.4), -0.3 /*weight*/);
  std::vector<std::pair<Vertex_handle, Coord_type> > coords;
  typedef CGAL::Identity<std::pair<Vertex_handle, Coord_type> > Identity;
  Coord_type norm = CGAL::regular_neighbor_coordinates_2(rt,
                                                         p,
                                                         std::back_inserter(coords),
                                                         Identity()).second;

  // Sibson interpolant: version without sqrt:
  std::pair<Coord_type, bool> res = CGAL::sibson_c1_interpolation_square(coords.begin(),
                                                                         coords.end(),
                                                                         norm,
                                                                         p,
                                                                         value_function,
                                                                         gradient_function,
                                                                         Traits());

  if(res.second)
    std::cout << "Tested interpolation on " << p
              << " interpolation: " << res.first << " exact: "
              << a + bx * p.x()+ by * p.y()+ c*(p.x()*p.x()+p.y()*p.y())
              << std::endl;
  else
    std::cout << "C^1 Interpolation not successful." << std::endl
              << " not all gradients are provided."  << std::endl
              << " You may resort to linear interpolation." << std::endl;

  return EXIT_SUCCESS;
}


#include <Eigen/LU>
#include <cmath>
#include <mathtoolbox/rbf-interpolation.hpp>

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::PartialPivLU;
using Eigen::VectorXd;
using std::function;
using std::vector;

mathtoolbox::RbfInterpolator::RbfInterpolator(const function<double(const double)>& rbf_kernel,
                                              const bool                            use_polynomial_term)
    : m_rbf_kernel(rbf_kernel), m_use_polynomial_term(use_polynomial_term)
{
}

void mathtoolbox::RbfInterpolator::SetData(const MatrixXd& X, const VectorXd& y)
{
    assert(y.rows() == X.cols());

    this->m_X = X;
    this->m_y = y;
}

void mathtoolbox::RbfInterpolator::CalcWeights(const bool use_regularization, const double lambda)
{
    const int num_data = m_y.rows();

    // Construct the symmetric matrix of RBF values
    MatrixXd Phi{num_data, num_data};
    for (int i = 0; i < num_data; ++i)
    {
        for (int j = i; j < num_data; ++j)
        {
            const double value = m_rbf_kernel((m_X.col(i) - m_X.col(j)).norm());

            Phi(i, j) = value;
            Phi(j, i) = value;
        }
    }

    if (m_use_polynomial_term)
    {
        const int dim = m_X.rows();

        MatrixXd P{num_data, dim + 1};

        P.block(0, 0, num_data, 1)   = MatrixXd::Ones(num_data, 1);
        P.block(0, 1, num_data, dim) = m_X.transpose();

        MatrixXd A = MatrixXd::Zero(num_data + dim + 1, num_data + dim + 1);

        A.block(0, 0, num_data, num_data)       = Phi;
        A.block(0, num_data, num_data, dim + 1) = P;
        A.block(num_data, 0, dim + 1, num_data) = P.transpose();

        VectorXd b = VectorXd::Zero(num_data + dim + 1);

        b.segment(0, num_data) = m_y;

        VectorXd solution;
        if (use_regularization)
        {
            const auto I = MatrixXd::Identity(num_data + dim + 1, num_data + dim + 1);

            solution = PartialPivLU<MatrixXd>(A.transpose() * A + lambda * I).solve(A.transpose() * b);
        }
        else
        {
            solution = PartialPivLU<MatrixXd>(A).solve(b);
        }

        m_w = solution.segment(0, num_data);
        m_v = solution.segment(num_data, dim + 1);
    }
    else
    {
        const auto     I = MatrixXd::Identity(num_data, num_data);
        const MatrixXd A = use_regularization ? Phi.transpose() * Phi + lambda * I : Phi;
        const VectorXd b = use_regularization ? Phi.transpose() * m_y : m_y;

        m_w = PartialPivLU<MatrixXd>(A).solve(b);
    }
}

double mathtoolbox::RbfInterpolator::CalcValue(const VectorXd& x) const
{
    assert(x.rows() == m_X.rows());

    const int num_data = m_w.rows();
    const int dim      = x.rows();

    // Calculate the distance for each data point via broadcasting
    const Eigen::VectorXd norms = (m_X.colwise() - x).colwise().norm();

    // Calculate the RBF value associated with each data point
    // TODO: This part can be further optimized for performance by vectorization
    VectorXd rbf_values{num_data};
    for (int i = 0; i < num_data; ++i)
    {
        rbf_values(i) = m_rbf_kernel(norms(i));
    }

    // Calculate the weighted sum using dot product
    const double rbf_term = m_w.dot(rbf_values);

    if (m_use_polynomial_term)
    {
        const double polynomial_term = m_v(0) + x.transpose() * m_v.segment(1, dim);

        return rbf_term + polynomial_term;
    }
    else
    {
        return rbf_term;
    }
}


// C/C++ includes
#include <cfloat>
#include <vector>
#include <iostream>

//Eigen includes
#include <Eigen/Core>

double _bin_size = 1;

/**
 * @brief returns all the voxels that are traversed by a ray going from start to end
 * @param start : continous world position where the ray starts
 * @param end   : continous world position where the ray end
 * @return vector of voxel ids hit by the ray in temporal order
 *
 * J. Amanatides, A. Woo. A Fast Voxel Traversal Algorithm for Ray Tracing. Eurographics '87
 */

std::vector<Eigen::Vector3i> voxel_traversal(Eigen::Vector3d ray_start, Eigen::Vector3d ray_end) {
  std::vector<Eigen::Vector3i> visited_voxels;

  // This id of the first/current voxel hit by the ray.
  // Using floor (round down) is actually very important,
  // the implicit int-casting will round up for negative numbers.
  Eigen::Vector3i current_voxel(std::floor(ray_start[0]/_bin_size),
                                std::floor(ray_start[1]/_bin_size),
                                std::floor(ray_start[2]/_bin_size));

  // The id of the last voxel hit by the ray.
  // TODO: what happens if the end point is on a border?
  Eigen::Vector3i last_voxel(std::floor(ray_end[0]/_bin_size),
                             std::floor(ray_end[1]/_bin_size),
                             std::floor(ray_end[2]/_bin_size));

  // Compute normalized ray direction.
  Eigen::Vector3d ray = ray_end-ray_start;
  //ray.normalize();

  // In which direction the voxel ids are incremented.
  double stepX = (ray[0] >= 0) ? 1:-1; // correct
  double stepY = (ray[1] >= 0) ? 1:-1; // correct
  double stepZ = (ray[2] >= 0) ? 1:-1; // correct

  // Distance along the ray to the next voxel border from the current position (tMaxX, tMaxY, tMaxZ).
  double next_voxel_boundary_x = (current_voxel[0]+stepX)*_bin_size; // correct
  double next_voxel_boundary_y = (current_voxel[1]+stepY)*_bin_size; // correct
  double next_voxel_boundary_z = (current_voxel[2]+stepZ)*_bin_size; // correct

  // tMaxX, tMaxY, tMaxZ -- distance until next intersection with voxel-border
  // the value of t at which the ray crosses the first vertical voxel boundary
  double tMaxX = (ray[0]!=0) ? (next_voxel_boundary_x - ray_start[0])/ray[0] : DBL_MAX; //
  double tMaxY = (ray[1]!=0) ? (next_voxel_boundary_y - ray_start[1])/ray[1] : DBL_MAX; //
  double tMaxZ = (ray[2]!=0) ? (next_voxel_boundary_z - ray_start[2])/ray[2] : DBL_MAX; //

  // tDeltaX, tDeltaY, tDeltaZ --
  // how far along the ray we must move for the horizontal component to equal the width of a voxel
  // the direction in which we traverse the grid
  // can only be FLT_MAX if we never go in that direction
  double tDeltaX = (ray[0]!=0) ? _bin_size/ray[0]*stepX : DBL_MAX;
  double tDeltaY = (ray[1]!=0) ? _bin_size/ray[1]*stepY : DBL_MAX;
  double tDeltaZ = (ray[2]!=0) ? _bin_size/ray[2]*stepZ : DBL_MAX;

  Eigen::Vector3i diff(0,0,0);
  bool neg_ray=false;
  if (current_voxel[0]!=last_voxel[0] && ray[0]<0) { diff[0]--; neg_ray=true; }
  if (current_voxel[1]!=last_voxel[1] && ray[1]<0) { diff[1]--; neg_ray=true; }
  if (current_voxel[2]!=last_voxel[2] && ray[2]<0) { diff[2]--; neg_ray=true; }
  visited_voxels.push_back(current_voxel);
  if (neg_ray) {
    current_voxel+=diff;
    visited_voxels.push_back(current_voxel);
  }

  while(last_voxel != current_voxel) {
    if (tMaxX < tMaxY) {
      if (tMaxX < tMaxZ) {
        current_voxel[0] += stepX;
        tMaxX += tDeltaX;
      } else {
        current_voxel[2] += stepZ;
        tMaxZ += tDeltaZ;
      }
    } else {
      if (tMaxY < tMaxZ) {
        current_voxel[1] += stepY;
        tMaxY += tDeltaY;
      } else {
        current_voxel[2] += stepZ;
        tMaxZ += tDeltaZ;
      }
    }
    visited_voxels.push_back(current_voxel);
  }
  return visited_voxels;
}

int main (int, char**) {
  Eigen::Vector3d ray_start(0,0,0);
  Eigen::Vector3d ray_end(3,2,2);
  std::cout << "Voxel size: " << _bin_size << std::endl;
  std::cout << "Starting position: " << ray_start.transpose() << std::endl;
  std::cout << "Ending position: " << ray_end.transpose() << std::endl;
  std::cout << "Voxel ID's from start to end:" << std::endl;
  std::vector<Eigen::Vector3i> ids = voxel_traversal(ray_start,ray_end);

  for (auto& i : ids) {
    std::cout << "> " << i.transpose() << std::endl;
  }
  std::cout << "Total number of traversed voxels: " << ids.size() << std::endl;
  return 0;
}


/**
 * @ file
 * @ brief NPDE homework TEMPLATE MAIN FILE
 * @ author
 * @ date
 * @ copyright Developed at SAM, ETH Zurich
 */

#define _USE_MATH_DEFINES

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

#include "fluxlimitedfv.h"

typedef std::numeric_limits<double> dbl;

using namespace FluxLimitedFV;

int main(int /*argc*/, char** /*argv*/) {
  std::cout.precision(dbl::max_digits10);

  /* ADVECTION PROBLEM */
  // Data
  double T = 1.0;
  unsigned int nb_timesteps[8] = {10, 20, 40, 80, 160, 320, 640, 1280};

  // Parameter coefficient
  double beta = 1.0;

  // Flux limiter function
  auto phi = [](double theta) {
    return (abs(theta) + theta) / (1.0 + abs(theta));
  };

  // Initial conditions A and B as lambda functions
  auto mu0_A_fn = [](double x) -> double {
    if (x < 1) {
      return 0.0;
    } else if (x < 2) {
      return pow(sin(0.5 * M_PI * (x - 1)), 2);
    } else {
      return 1.0;
    }
  };
  auto mu0_B_fn = [](double x) -> double {
    if ((1 < x) && (x < 2)) {
      return 1.0;
    } else {
      return 0.0;
    }
  };

  Eigen::VectorXd error_L1_mu0_A = Eigen::VectorXd::Zero(8);
  Eigen::VectorXd error_L1_mu0_B = Eigen::VectorXd::Zero(8);
  double tau, h;
  int N;
  Eigen::VectorXd fluxlimAdvection_sol_A, fluxlimAdvection_sol_B;

  for (int k = 0; k < 8; k++) {
    tau = T / nb_timesteps[k];
    h = 1.2 * tau;
    N = std::round(5.0 / h);

    // Discrete initial conditions
    Eigen::VectorXd mu0_A(N);
    for (int j = 0; j < N; j++) {
      mu0_A(j) = mu0_A_fn(h * j);
    }
    Eigen::VectorXd mu0_B(N);
    for (int j = 0; j < N; j++) {
      mu0_B(j) = mu0_B_fn(h * j);
    }

    fluxlimAdvection_sol_A =
        fluxlimAdvection(beta, mu0_A, h, tau, nb_timesteps[k], phi);
    fluxlimAdvection_sol_B =
        fluxlimAdvection(beta, mu0_B, h, tau, nb_timesteps[k], phi);

    // Computing the L1 errors
    double sum_A = 0.0;
    double sum_B = 0.0;
    for (int j = 0; j < N; j++) {
      sum_A = sum_A +
              std::abs(h * (fluxlimAdvection_sol_A(j) - mu0_A_fn(j * h - T)));
      sum_B = sum_B +
              std::abs(h * (fluxlimAdvection_sol_B(j) - mu0_B_fn(j * h - T)));
    }
    error_L1_mu0_A[k] = sum_A;
    error_L1_mu0_B[k] = sum_B;
  }

  std::cout << "" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "error_L1_mu0_A" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "M";
  std::cout << "\t error" << std::endl;
  for (int k = 0; k < 8; k++) {
    std::cout << nb_timesteps[k];
    std::cout << "\t";
    std::cout << error_L1_mu0_A[k] << std::endl;
  }
  std::cout << "\n" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "error_L1_mu0_B" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "M";
  std::cout << "\t error" << std::endl;
  for (int k = 0; k < 8; k++) {
    std::cout << nb_timesteps[k];
    std::cout << "\t";
    std::cout << error_L1_mu0_B[k] << std::endl;
  }
  std::cout << "" << std::endl;

  // Output results to csv files
  Eigen::VectorXd x_advection(N);
  for (int j = 0; j < N; j++) {
    x_advection(j) = j * h;
  }
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream fluxlimAdvection_sol_A_csv;
  fluxlimAdvection_sol_A_csv.open(CURRENT_BINARY_DIR
                                  "/fluxlimAdvection_sol_A.csv");
  fluxlimAdvection_sol_A_csv << x_advection.transpose().format(CSVFormat)
                             << std::endl;
  fluxlimAdvection_sol_A_csv
      << fluxlimAdvection_sol_A.transpose().format(CSVFormat) << std::endl;
  fluxlimAdvection_sol_A_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimAdvection_sol_A.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_A.csv " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_A.eps");

  std::ofstream fluxlimAdvection_sol_B_csv;
  fluxlimAdvection_sol_B_csv.open(CURRENT_BINARY_DIR
                                  "/fluxlimAdvection_sol_B.csv");
  fluxlimAdvection_sol_B_csv << x_advection.transpose().format(CSVFormat)
                             << std::endl;
  fluxlimAdvection_sol_B_csv
      << fluxlimAdvection_sol_B.transpose().format(CSVFormat) << std::endl;
  fluxlimAdvection_sol_B_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimAdvection_sol_B.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_B.csv " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_B.eps");

  /* BURGERS FLUX PROBLEM */
  // Discretization parameters
  T = 2.0;
  tau = T / nb_timesteps[7];
  h = 1.2 * tau;
  N = std::round(5.0 / h);

  // Initial conditions A and B as lambda functions
  auto mu0_fn = [](double x) -> double {
    if (x < 2) {
      return -1.0;
    } else if (x < 3) {
      return 1.0;
    } else {
      return -1.0;
    }
  };

  Eigen::VectorXd mu0(N);
  for (int j = 0; j < N; j++) {
    mu0(j) = mu0_fn(h * j);
  }

  Eigen::VectorXd fluxlimBurgers_sol =
      fluxlimBurgers(mu0, h, tau, nb_timesteps[6], phi);

  Eigen::VectorXd x_Burgers(N);
  for (int j = 0; j < N; j++) {
    x_Burgers(j) = j * h;
  }

  std::ofstream fluxlimBurgers_sol_csv;
  fluxlimBurgers_sol_csv.open(CURRENT_BINARY_DIR "/fluxlimBurgers_sol.csv");
  fluxlimBurgers_sol_csv << x_Burgers.transpose().format(CSVFormat)
                         << std::endl;
  fluxlimBurgers_sol_csv << fluxlimBurgers_sol.transpose().format(CSVFormat)
                         << std::endl;
  fluxlimBurgers_sol_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimBurgers_sol.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimBurgers_sol.csv " CURRENT_BINARY_DIR
              "/fluxlimBurgers_sol.eps");

}  // main


#pragma once

#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Utils/IO/IOUtilities.hpp>
#include <iostream>

namespace myUtils {

// =============================================================================
// Matrix Utils
// =============================================================================
Eigen::MatrixXd hStack(const Eigen::MatrixXd& a_, const Eigen::MatrixXd& b_);
Eigen::MatrixXd vStack(const Eigen::MatrixXd& a_, const Eigen::MatrixXd& b_);
Eigen::MatrixXd vStack(const Eigen::VectorXd& a_, const Eigen::VectorXd& b_);
Eigen::MatrixXd deleteRow(const Eigen::MatrixXd& a_, int row);

// =============================================================================
// Simple Trajectory Generator
// =============================================================================
double smooth_changing(double ini, double end, double moving_duration,
                       double curr_time);
double smooth_changing_vel(double ini, double end, double moving_duration,
                           double curr_time);
double smooth_changing_acc(double ini, double end, double moving_duration,
                           double curr_time);
void getSinusoidTrajectory(double initTime_, const Eigen::VectorXd& midPoint_,
                           const Eigen::VectorXd& amp_,
                           const Eigen::VectorXd& freq_, double evalTime_,
                           Eigen::VectorXd& p_, Eigen::VectorXd& v_,
                           Eigen::VectorXd& a_);

// =============================================================================
// ETC
// =============================================================================

double computeAlphaGivenBreakFrequency(double hz, double dt);

double bind_half_pi(double);

bool isEqual(const Eigen::VectorXd a, const Eigen::VectorXd b,
             const double threshold = 0.00001);
double CropValue(double value, double min, double max, std::string source);
double CropValue(double value, double min, double max);

Eigen::VectorXd CropVector(Eigen::VectorXd value, Eigen::VectorXd min,
                           Eigen::VectorXd max, std::string source);

Eigen::MatrixXd CropMatrix(Eigen::MatrixXd value, Eigen::MatrixXd min,
                           Eigen::MatrixXd max, std::string source);

bool isInBoundingBox(const Eigen::VectorXd& val, const Eigen::VectorXd& lb,
                     const Eigen::VectorXd& ub);

Eigen::MatrixXd GetRelativeMatrix(const Eigen::MatrixXd value,
                                  const Eigen::MatrixXd min,
                                  const Eigen::MatrixXd max);

Eigen::VectorXd GetRelativeVector(const Eigen::VectorXd value,
                                  const Eigen::VectorXd min,
                                  const Eigen::VectorXd max);

Eigen::VectorXd eulerIntegration(const Eigen::VectorXd& x,
                                 const Eigen::VectorXd& xdot, double dt);

Eigen::VectorXd doubleIntegration(const Eigen::VectorXd& q,
                                  const Eigen::VectorXd& alpha,
                                  const Eigen::VectorXd& alphad, double dt);

Eigen::Matrix3d VecToso3(const Eigen::Vector3d& omg);
Eigen::MatrixXd Adjoint(const Eigen::MatrixXd& R, const Eigen::Vector3d& p);

double QuatToYaw(const Eigen::Quaternion<double> q);

// Euler ZYX
//     Represents either:
//     extrinsic XYZ rotations: Fixed-frame roll, then fixed-frame pitch, then fixed-frame yaw.
//     or intrinsic ZYX rotations: Body-frame yaw, body-frame pitch, then body-frame roll
//
//     The equation is similar, but the values for fixed and body frame rotations are different.
// World Orientation is R = Rz*Ry*Rx
Eigen::Quaterniond EulerZYXtoQuat(const double roll, const double pitch, const double yaw);

// Quaternion to Euler ZYX
Eigen::Vector3d QuatToEulerZYX(const Eigen::Quaterniond & quat_in);

// ZYX extrinsic rotation rates to world angular velocity
// angular vel = [wx, wy, wz]
Eigen::Vector3d EulerZYXRatestoAngVel(const double roll, const double pitch, const double yaw,
                                      const double roll_rate, const double pitch_rate, const double yaw_rate);

void avoid_quat_jump(const Eigen::Quaternion<double> &des_ori, Eigen::Quaternion<double> &act_ori);
}  // namespace myUtils


// Fractool
#include <fractool/macros.hpp>
#include <fractool/ftcore/generate_julia.hpp>

// Internal
#include <algorithm>
#include <complex>

// External
#include <boost/log/trivial.hpp>

/**
 * Run julia set generation algorithm
 */
void generate_julia(unsigned size_x, unsigned size_y, unsigned char* param_buffer)
{
    // Print message
    BOOST_LOG_TRIVIAL(info) << "Generating julia...";

    // Parameter
    std::complex<float> c(-0.4, 0.6);

    // Helper variables
    float gsc = 4.0;                            // Grid scale
    float x0 = (float)size_x/2;                 // Center x value
    float y0 = (float)size_y/2;                 // Center y value
    float scl = gsc / std::min(size_x, size_y); // Final scale factor
    BOOST_LOG_TRIVIAL(debug) << "Grid scale: " << gsc;
    BOOST_LOG_TRIVIAL(debug) << "Origin Point (x0, y0): (" << x0 << "," << y0 << ")";
    BOOST_LOG_TRIVIAL(debug) << "Final Scale: " << scl;

    // Run iteration algorithm
    std::complex<float> z;
    unsigned char n;
    for (unsigned j = 0; j < size_y; ++j) {
        for (unsigned i = 0; i < size_x; ++i) {
            // Initialize z
            z = std::complex<float>(i - x0, y0 - j) * scl;

            // Iteration
            n = 0;
            while (n < 255 && std::abs(z) < 2) {
                z = z*z + c;
                n += 1;
            }

            // Log output
            BOOST_LOG_TRIVIAL(trace)
                << "Pixel (" << i << "," << j << "): "
                << "c = " << c << " n = " << (int)n;

            // Set parameter
            param_buffer[ARRAY2D(size_x, i, j)] = n;
        }
    }
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Example of using Bayesian Filter Class to solve a simple problem.
 *
 * The example implements a simple quadratic observer.
 *  This tries to estimate the state of system while also trying to
 *  calibrate a simple linear model of the system which includes
 *  a scale factor and a bias.
 *  Estimating both the system state and a scale factor results in a
 *  quadratic (product of two states and therefore non-linear) observation.
 *  The system model is a 1D brownian motion with a known perturbation.
 */

#include "BayesFilter/infFlt.hpp"
#include "Test/random.hpp"
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp>

namespace
{
	namespace FM = Bayesian_filter_matrix;
	using namespace FM;

	// Choose Filtering Scheme to use
	typedef Bayesian_filter::Information_scheme FilterScheme;

	// Square
	template <class scalar>
	inline scalar sqr(scalar x)
	{
		return x*x;
	}

	// Random numbers from Boost
	Bayesian_filter_test::Boost_random localRng;

	// Constant Dimensions
	const unsigned NX = 3;			// Filter State dimension 	(SystemState, Scale, Bias)

	// Filter Parameters
	// Noise on observing system state
	const Float OBS_NOISE = 0.01;
	// Prediction Noise: no Prediction noise as pertubation is known
	const Float X_NOISE = 0.0;	// System State
	const Float S_NOISE = 0.0;	// Scale
	const Float B_NOISE = 0.0;	// Bias
	// Filter's Initial state uncertainty: System state is unknown
	const Float i_X_NOISE = 1000.;
	const Float i_S_NOISE = 0.1;
	const Float i_B_NOISE = 0.1;

}//namespace

/*
 * Prediction model
 * Linear state predict model with additive control input
 */
class QCpredict : public Bayesian_filter::Linrz_predict_model
{
	Float motion;
	mutable FM::Vec fx;
public:
	QCpredict();
		;
	void predict(const FM::Vec& u)
	{
		motion = u[0];
	}
	const FM::Vec& f(const FM::Vec& x) const
	{
		// Constant scale and bias, system state perturbed by control input
		fx = x;
		fx[0] += motion;
		return fx;
	};
};

QCpredict::QCpredict() : Bayesian_filter::Linrz_predict_model(NX, NX), fx(NX)
{
	FM::identity (Fx);

	// Setup constant noise model: G is identity
	q[0] = sqr(X_NOISE);
	q[1] = sqr(S_NOISE);
	q[2] = sqr(B_NOISE);
	FM::identity (G);
}

/*
 * Quadratic observation model
 */
class QCobserve : public Bayesian_filter::Linrz_uncorrelated_observe_model
{
	mutable FM::Vec z_pred;
public:
	QCobserve ();
	const FM::Vec& h(const FM::Vec& x) const
	{	// Quadratic Observation model
		z_pred[0] = x[0] * x[1] + x[2];
		return z_pred;
	};
	void state (const FM::Vec& x)
	// Linearised model, Jacobian of h at x
	{
		Hx(0,0) = x[1];
		Hx(0,1) = x[0];
		Hx(0,2) = 1.;
	}
};

QCobserve::QCobserve () :
	Bayesian_filter::Linrz_uncorrelated_observe_model(NX,1), z_pred(1)
{
	// Observation Noise variance
	Zv[0] = OBS_NOISE*OBS_NOISE;
}

int main()
{
	// Global setup for test output
	std::cout.flags(std::ios::scientific); std::cout.precision(6);

	// Setup the test filters
	FM::Vec x_true (NX);

	// True State to be observed
	x_true[0] = 10.;	// System State
	x_true[1] = 1.0;	// Scale
	x_true[2] = 0.0;	// Bias

	std::cout << "Quadratic Calibration" << std::endl;
	std::cout << "Init " << x_true << std::endl;

	// Construct Prediction and Observation model and Calibration filter
	QCpredict linearPredict;
	QCobserve nonlinObserve;
	FilterScheme obsAndCalib (NX);

	// Give the filter an true initial guess of the system state
	obsAndCalib.x[0] = x_true[0];
	obsAndCalib.x[1] = 1.;		// Assumed initial Scale
	obsAndCalib.x[2] = 0.;		// Assumed initial Bias
	obsAndCalib.X.clear();
	obsAndCalib.X(0,0) = sqr(i_X_NOISE);
	obsAndCalib.X(1,1) = sqr(i_S_NOISE);
	obsAndCalib.X(2,2) = sqr(i_B_NOISE);

	obsAndCalib.init ();

	// Iterate the filter with test observations
	FM::Vec u(1), z_true(1), z(1);
	for (unsigned i = 0; i < 100; i++ )
	{
		// Predict true state using Brownian control input
		localRng.normal (u);				// normally distributed
		x_true[0] += u[0];
		linearPredict.predict (u);

		// Predict filter with known perturbation
		obsAndCalib.predict (linearPredict);

		// True Observation: Quadratic observation model
		z_true[0] = x_true[0] * x_true[1] + x_true[2];

		// Observation with additive noise
		localRng.normal (z, z_true[0], OBS_NOISE);	// normally distributed mean z_true[0], stdDev OBS_NOISE.

		// Filter observation using model linearised at state estimate x
		nonlinObserve.state (obsAndCalib.x);
		obsAndCalib.observe (nonlinObserve, z);
	}

	// Update the filter to state and covariance are available
	obsAndCalib.update ();

	// Print everything: True, filter, covariance
	std::cout << "True " << x_true <<  std::endl;
	std::cout << "Calb " << obsAndCalib.x << std::endl;
	std::cout << obsAndCalib.X << std::endl;
	return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector2f v = Vector2f::Random();
JacobiRotation<float> G;
G.makeGivens(v.x(), v.y());
cout << "Here is the vector v:" << endl << v << endl;
v.applyOnTheLeft(0, 1, G.adjoint());
cout << "Here is the vector J' * v:" << endl << v << endl;
  return 0;
}


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXf m(2,2);
  MatrixXf n(2,2);
  MatrixXf result(2,2);

  m << 1,2,
       3,4;
  n << 5,6,
       7,8;

  result = m * n;
  cout << "-- Matrix m*n: --" << endl << result << endl << endl;
  result = m.array() * n.array();
  cout << "-- Array m*n: --" << endl << result << endl << endl;
  result = m.cwiseProduct(n);
  cout << "-- With cwiseProduct: --" << endl << result << endl << endl;
  result = m.array() + 4;
  cout << "-- Array m + 4: --" << endl << result << endl << endl;
}


/** @file 16.cpp Problem 16: Power digit sum
 *
 * 2**15 = 32768 and the sum of its digits is 3 + 2 + 7 + 6 + 8 = 26.
 *
 * What is the sum of the digits of the number 2**1000?
 */

#include "cpp_int_util.hpp"                 // sum_digits

/// @cond
#include <boost/multiprecision/cpp_int.hpp> // cpp_int

#include <iostream>                         // cout
/// @endcond

using boost::multiprecision::cpp_int;

int sum(int n)
{
    cpp_int i = 1;
    for (int p = 0; p < n; ++p)
        i *= 2;
    return cpp_int_util::sum_digits(i);
}

int main()
{
    assert(sum(15) == 26);
    std::cout << sum(1000) << std::endl;
}


// transition_dist.cpp
// (c) 2021-01 David Merrell
//
// Implementation of TransitionDist class
// and its subclasses.

#define __STDCPP_WANT_MATH_SPEC_FUNCS__ 1

#include "transition_dist.h"
#include "contingency_table.h"
#include <cmath>
#include <string>

//// If we're on __linux__, use the std::beta function
//#ifdef __linux__
//
//float my_beta(float a, float b){
//    return std::beta(a,b);
//}
//
//// If we're on __APPLE__ or _WIN32, use the boost::math::beta function
//// (which seems to be slower...)
//#else

#include <boost/math/special_functions/beta.hpp>
float my_beta(float a, float b){
    return boost::math::beta(a,b);
}

//#endif

////////////////////////////////
// Beta distribution
////////////////////////////////

float binom_coeff(int n, int k){
    return 1.0/((n+1.0)*my_beta(n-k+1,k+1));
}

float binom_prob(int N, float p, int x){
    return binom_coeff(N,x) * pow(p,x) * pow(1.0 - p, N-x);
}

std::vector<float> initialize_binom_probs(int N, float p){
    std::vector<float> probs = std::vector<float>(N+1, 0);
    for(int i=0; i < N+1; i++){
        probs[i] = binom_prob(N, p, i);
    }
    return probs;
}

BinomTransitionDist::BinomTransitionDist(float pr_a0, float pr_a1,
                                         float pr_b0, float pr_b1){
    prior_a0 = pr_a0;
    prior_a1 = pr_a1;
    prior_b0 = pr_b0;
    prior_b1 = pr_b1;
}

void BinomTransitionDist::set_state_action(ContingencyTable ct,
                                           short unsigned int a_size,
                                           short unsigned int b_size){
    // compute smoothed point estimates
    float a_smoothing = prior_a0 + prior_a1;
    float b_smoothing = prior_b0 + prior_b1;

    float p_a = (float(ct.a1) + prior_a1) / (float(ct.a0 + ct.a1) + a_smoothing);
    a_probs = initialize_binom_probs(a_size, p_a);

    float p_b = (float(ct.b1) + prior_b1) / (float(ct.b0 + ct.b1) + b_smoothing);
    b_probs = initialize_binom_probs(b_size, p_b);

}

////////////////////////////////
// Beta-Binomial distribution
////////////////////////////////

float beta_binom_prob(int N, float pr_0, float pr_1, int x){
    return binom_coeff(N,x) * my_beta(x+pr_1, N - x + pr_0) / my_beta(pr_1, pr_0);
}

std::vector<float> initialize_beta_binom_probs(int N, float prior_0, float prior_1){
    std::vector<float> probs = std::vector<float>(N+1, 0);
    for(int i=0; i < N+1; i++){
        probs[i] = beta_binom_prob(N, prior_0, prior_1, i);
    }
    return probs;
}

BetaBinomTransitionDist::BetaBinomTransitionDist(float pr_a0, float pr_a1,
                                                 float pr_b0, float pr_b1){
    prior_a0 = pr_a0;
    prior_a1 = pr_a1;
    prior_b0 = pr_b0;
    prior_b1 = pr_b1;
}

void BetaBinomTransitionDist::set_state_action(ContingencyTable ct,
                                          short unsigned int size_a,
                                          short unsigned int size_b){
    a_probs = initialize_beta_binom_probs(size_a,
                                          ct.a0 + prior_a0,
                                          ct.a1 + prior_a1);

    b_probs = initialize_beta_binom_probs(size_b,
                                          ct.b0 + prior_b0,
                                          ct.b1 + prior_b1);
}

/////////////////////////////////
// Factory method
/////////////////////////////////
TransitionDist* TransitionDist::make_transition_dist(std::string tr_dist_type,
                                                     float pr_a0, float pr_a1,
                                                     float pr_b0, float pr_b1){
    if (tr_dist_type == "binom"){
      return new BinomTransitionDist(pr_a0, pr_a1, pr_b0, pr_b1);
    }
    else if(tr_dist_type == "beta_binom"){
      return new BetaBinomTransitionDist(pr_a0, pr_a1, pr_b0, pr_b1);
    }
    else{
      std::cerr << tr_dist_type << " not a valid value for transition distribution." << std::endl;
      throw(1);
    }

}