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

namespace mlfe{ namespace math{

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

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

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

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

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

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

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


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

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

namespace rc_utils
{

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

KalmanFilter::~KalmanFilter()
{
}

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

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

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

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

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

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

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

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

	m_lastZ = z;

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

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

}


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

#ifndef COMBINATION_ITERATOR_HPP
#define COMBINATION_ITERATOR_HPP

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

#include <boost/iterator/iterator_facade.hpp>

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

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

private:
	friend class boost::iterator_core_access;

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

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

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

		++comb_[j];

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

		return;
	}

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

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

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

#endif


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

#include <Eigen/Dense>

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

using namespace Eigen;
using namespace std;

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

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

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

    Vector2d z;

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

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

    return z;

}

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

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

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

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

    return x;

}

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

int main() {

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

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

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

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

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

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

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

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

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

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

    // Number of tests
    const int Ntest = 100;

    // Timer
    Timer timer;

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

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

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

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

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

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

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

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

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

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

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

        // Runtimes
        MatrixXd runtime(5, Ntest);

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

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

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

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

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

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

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

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

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

        }

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

    }

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

    return 0;

}


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

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

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

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

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


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

#include "avgvalboundary.h"

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

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

namespace AvgValBoundary {

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

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

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

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

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

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

  return mu;
}
/* SAM_LISTING_END_2 */

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

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

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

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

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

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

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

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

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

}  // namespace AvgValBoundary


#include "NeuralNetwork.h"

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

#include <Eigen/Dense>

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

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

	std::vector<int> sizes;

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

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

	std::cout << std::endl;

    NeuralNetwork nn(sizes, learning_factor);

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

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

    return nn;
}

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

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

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

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

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

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

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

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

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

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

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

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

		m_deltas[delta_index] = delta;
	}

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

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

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

    forward_propagate(input);

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

    return max_index;
}

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

	forward_propagate(input);

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

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

	return classification_vector;
}

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

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

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

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

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

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

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

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

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

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


#ifndef TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP
#define TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP

#include <cmath>

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

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

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

//
namespace superconductivity {

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

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

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

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

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

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

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

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

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

} // namespace pippard

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_PIPPARD_HPP


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

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

// external math functions
#include<cmath>

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

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

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

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

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

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

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


/*
 * fmath.cpp
 *
 * Created on: Jul 17, 2012
 * @author Ralph Schurade
 */
#include "fmath.h"

#include "math.h"

#include "../thirdparty/newmat10/newmatap.h"
#include "../thirdparty/newmat10/newmatio.h"
#include "../thirdparty/newmat10/newmatrm.h"
#include "../thirdparty/newmat10/precisio.h"

#include <QDebug>
#include <qmath.h>

#include <boost/math/special_functions/spherical_harmonic.hpp>

#include <float.h>

FMath::FMath() {}
FMath::~FMath() {}

ColumnVector FMath::vlog( ColumnVector v )
{
    // allocate memory of output object:
    ColumnVector tmp( v.Nrows() );

    // for each element of the vector:
    for ( int i( 1 ); i <= v.Nrows(); ++i )
    {
        if ( v( i ) <= 0.0 )
            tmp( i ) = -1.0e+17;
        else
            tmp( i ) = log( v( i ) );
    }
    return tmp;
}

double FMath::legendre_p( int k )
{
    double z = 1.0;
    double n = 1.0;

    for ( int i = 1; i <= k + 1; i += 2 ) z *= i;
    for ( int i = 2; i <= k - 2; i += 2 ) n *= i;

    if (( k / 2 ) % 2 == 0)
    {
        return -1.0 / ( 8 * M_PI ) * ( z / n );
    }
    else
    {
        return 1.0 / ( 8 * M_PI ) * ( z / n );
    }

}

double FMath::boostLegendre_p( int order, int arg1, double arg2 )
{
    return boost::math::legendre_p<double>( order, arg1, arg2 );
}

Matrix FMath::sh_base( Matrix g, int maxOrder )
{
    //qDebug() << "start calculating sh base";
    // allcoate result matrix
    unsigned long sh_dirs( ( maxOrder + 2 ) * ( maxOrder + 1 ) / 2 );
    Matrix out( g.Nrows(), sh_dirs );
    out = 0.0;

    // for each direction
    for ( int i = 0; i < g.Nrows(); ++i )
    {
        // transform current direction to polar coordinates
        double theta( acos( g( i + 1, 3 ) ) );
        double phi( atan2( g( i + 1, 2 ), g( i + 1, 1 ) ) );

        // calculate spherical harmonic base
        for ( int order = 0, j = 0; order <= maxOrder; order += 2 )
        {
            for ( int degree( -order ); degree <= order; ++degree, ++j )
            {
                out( i + 1, j + 1 ) = sh_base_function( order, degree, theta, phi );
            }
        }
    }
    //qDebug() << "finished calculating sh base";
    return out;
}

double FMath::sh_base_function( int order, int degree, double theta, double phi )
{
    using namespace boost::math;
#if 0
    double P = legendre_p<double>( order, abs(degree), cos(theta) );

    if ( degree > 0 )
    {
        return P * cos( degree * phi );
    }
    else if ( degree < 0 )
    {
        return P * sin( -degree * phi );
    }
    else
    {
        return P;
    }
#else
    if ( degree > 0 )
    {
        return spherical_harmonic_r( order, abs( degree ), theta, phi );
    }
    else if ( degree < 0 )
    {
        return spherical_harmonic_i( order, abs( degree ), theta, phi );
    }
    else
    {
        return spherical_harmonic_r( order, 0, theta, phi );
    }
#endif
}

SymmetricMatrix FMath::moment_of_inertia( const ColumnVector& values, const QVector<ColumnVector>& points )
{
    SymmetricMatrix result( 3 );
    result = 0.0;

    double sum( 0.0 );
    for ( int i = 0; i < points.size(); ++i )
    {
        double x( points[i]( 1 ) );
        double y( points[i]( 2 ) );
        double z( points[i]( 3 ) );
        double val = fabs( values( i + 1 ) );

        result( 1, 1 ) += x * x * val;
        result( 1, 2 ) += x * y * val;
        result( 1, 3 ) += x * z * val;
        result( 2, 2 ) += y * y * val;
        result( 2, 3 ) += y * z * val;
        result( 3, 3 ) += z * z * val;

        sum += val * val;
    }
    result = result / sum;

    return result;
}

double FMath::iprod( const ColumnVector& v1, const ColumnVector& v2 )
{
    double result( 0.0 );

    if ( v1.Nrows() != v2.Nrows() )
    {
        throw std::range_error( "Vectors in scalar product need same size!" );
    }

    for ( int i = 1; i < v1.Nrows() + 1; ++i )
    {
        result += v1( i ) * v2( i );
    }

    return result;
}

ColumnVector FMath::cprod( const ColumnVector& v1, const ColumnVector& v2 )
{
    ColumnVector result( 3 );

    if ( v1.Nrows() != 3 || v2.Nrows() != 3 )
        throw std::range_error( "Vectors in cross product both need size 3!" );

    result( 1 ) = v1( 2 ) * v2( 3 ) - v1( 3 ) * v2( 2 );
    result( 2 ) = v1( 3 ) * v2( 1 ) - v1( 1 ) * v2( 3 );
    result( 3 ) = v1( 1 ) * v2( 2 ) - v1( 2 ) * v2( 1 );

    result = result / sqrt( FMath::iprod( result, result ) );

    return result;
}

void FMath::evd3x3( ColumnVector tensor, QVector<ColumnVector>& vecs, ColumnVector& vals )
{
    double i1, i2, i3, v, s, phi, l1, l2, l3;
    double ev1_x, ev1_y, ev1_z, ev2_x, ev2_y, ev2_z, ev3_x, ev3_y, ev3_z, vec_norm;

    double xx = tensor( 1 );
    double xy = tensor( 2 );
    double xz = tensor( 3 );
    double yy = tensor( 4 );
    double yz = tensor( 5 );
    double zz = tensor( 6 );

    // three invariants of D (dt)
    // i1=l1+l2+l3 (trace)
    // i2=l1*l2+l1*l3+l2*l3
    // i3=l1*l2*l3 (determinante)
    i1 = xx + yy + zz;
    i2 = xx * yy + xx * zz + yy * zz - ( pow2( xy ) + pow2( xz ) + pow2( yz ) );
    i3 = xx * yy * zz + 2. * xy * xz * yz - ( zz * pow2( xy ) + yy * pow2( xz ) + xx * pow2( yz ) );

    v = pow2( i1 / 3 ) - i2 / 3;
    s = pow3( i1 / 3 ) - i1 * i2 / 6 + i3 / 2;

    if ( ( v > 0 ) && ( pow2( s ) < pow3( v ) ) )
        phi = acos( s / v * sqrt( 1. / v ) ) / 3;
    else
        phi = 0;

    // eigenvalues
    if ( phi != 0 )
    {
        l1 = i1 / 3 + 2 * sqrt( v ) * cos( phi );
        l2 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. + phi );
        l3 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. - phi );
    }
    else
        l1 = l2 = l3 = 0.0;

    // eigenvectors
    ev1_x = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * yz - ( zz - l1 ) * xy );
    ev1_y = ( xz * yz - ( zz - l1 ) * xy ) * ( xz * xy - ( xx - l1 ) * yz );
    ev1_z = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * xy - ( xx - l1 ) * yz );

    ev2_x = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * yz - ( zz - l2 ) * xy );
    ev2_y = ( xz * yz - ( zz - l2 ) * xy ) * ( xz * xy - ( xx - l2 ) * yz );
    ev2_z = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * xy - ( xx - l2 ) * yz );

    ev3_x = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * yz - ( zz - l3 ) * xy );
    ev3_y = ( xz * yz - ( zz - l3 ) * xy ) * ( xz * xy - ( xx - l3 ) * yz );
    ev3_z = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * xy - ( xx - l3 ) * yz );

    vec_norm = sqrt( pow2( ev1_x ) + pow2( ev1_y ) + pow2( ev1_z ) );

    if ( vec_norm > 0 )
    {
        ev1_x = ev1_x / vec_norm;
        ev1_y = ev1_y / vec_norm;
        ev1_z = ev1_z / vec_norm;
    }
    else
        ev1_x = ev1_y = ev1_z = 0.0;

    vec_norm = sqrt( pow2( ev2_x ) + pow2( ev2_y ) + pow2( ev2_z ) );

    if ( vec_norm > 0 )
    {
        ev2_x = ev2_x / vec_norm;
        ev2_y = ev2_y / vec_norm;
        ev2_z = ev2_z / vec_norm;
    }
    else
        ev2_x = ev2_y = ev2_z = 0.0;

    vec_norm = sqrt( pow2( ev3_x ) + pow2( ev3_y ) + pow2( ev3_z ) );

    if ( vec_norm > 0 )
    {
        ev3_x = ev3_x / vec_norm;
        ev3_y = ev3_y / vec_norm;
        ev3_z = ev3_z / vec_norm;
    }
    else
        ev3_x = ev3_y = ev3_z = 0.0;

    ColumnVector ev1( 3 );
    ColumnVector ev2( 3 );
    ColumnVector ev3( 3 );

    vals(1) = l1;
    vals(2) = l2;
    vals(3) = l3;

    ev1( 1 ) = ev1_x;
    ev1( 2 ) = ev1_y;
    ev1( 3 ) = ev1_z;
    ev2( 1 ) = ev2_x;
    ev2( 2 ) = ev2_y;
    ev2( 3 ) = ev2_z;
    ev3( 1 ) = ev3_x;
    ev3( 2 ) = ev3_y;
    ev3( 3 ) = ev1_z;
    vecs.clear();
    vecs.push_back( ev3 );
    vecs.push_back( ev2 );
    vecs.push_back( ev1 );
}

///* ***************************************************************************
///* @file math.cpp
///* @fn Vector cart2sphere(
///*
///*     const Vector& input )
///*
///* @brief Transforms a cartesian-coordinate vector to spherical coordinates.
///*
///* For further information view:

///*  Wikipedia article on cartesian and spherical coordinates.
///*
///* @param Vector: a in cartesian-coordinates
///*
///* @return Vector: representation of a in spherical coordinates,
///*                 order: r, theta, phi.
///* ***************************************************************************
ColumnVector FMath::cart2sphere( const ColumnVector& input )
{
  if ( input.Nrows() != 3 )
    throw std::range_error("Can only transform 3D-vectors.");

  ColumnVector result( 3 );

    result( 1 ) = sqrt( input( 1 ) * input( 1 ) + input( 2 ) * input( 2 ) + input( 3 ) * input( 3 ) );
    result( 2 ) = acos( input( 3 ) / result( 1 ) );
    result( 3 ) = atan2( input( 2 ), input( 1 ) );

  return result;
}

///* ***************************************************************************
///* @file math.cpp
///* @fn Vector sphere2cart(
///*
///*     const Vector& input )
///*
///* @brief Transforms a spherical-coordinate vector to cartesian-coordinates.
///*
///* For further information view:

///*  Wikipedia article on cartesian and spherical coordinates.

///*
///* @param Vector: a in spherical-coordinates
///*
///* @return Vector: represantation of a in cartesian-coordinates, order: x,y,z.
///* ***************************************************************************
ColumnVector FMath::sphere2cart( const ColumnVector& input )
{
    if ( input.Nrows() != 3 )
        throw std::range_error( "Can only transform 3D-vectors." );

    ColumnVector result( 3 );

    result( 1 ) = input( 1 ) * sin( input( 2 ) ) * cos( input( 3 ) );
    result( 2 ) = input( 1 ) * sin( input( 2 ) ) * sin( input( 3 ) );
    result( 3 ) = input( 1 ) * cos( input( 2 ) );

    return result;
}

ColumnVector FMath::SH_opt_max( const ColumnVector& startX, const ColumnVector& coeff )
{
    ColumnVector newX( startX );
    ColumnVector oldX( 3 );

    // set values for computation
    const double precision( 10.e-6 );
    const double eps( 10.e-4 );

    double delta( 1.0 );
    int steps( 0 );
    while ( delta > precision && steps < 100 )
    {
        // Update old value
        ++steps;
        oldX = newX;
        double SH_old( sh_eval( oldX, coeff ) );

        // define the delta steps
        ColumnVector dt( oldX );
        ColumnVector dp( oldX );
        dt( 2 ) += eps;
        dp( 3 ) += eps;

        // calculate Jacobian
        double Jt( ( FMath::sh_eval( dt, coeff ) - SH_old ) );
        double Jp( ( FMath::sh_eval( dp, coeff ) - SH_old ) );

        // do iteration
        newX( 2 ) = oldX( 2 ) + Jt;
        newX( 3 ) = oldX( 3 ) + Jp;

        delta = fabs( Jt ) + fabs( Jp );
        //std::cout<<delta<<" ";
    }
    return newX;
}

///* ***************************************************************************
///* @file math.cpp
///* @fn double sh_eval(
///*
///*     const Vector& position,
///*     const Vector& coeff,
///*     const unsigned long max_order )
///*
///* @brief Evaluates a spherical harmonic function defined by its coefficients
///* in direction of the position vector.
///*
///* important: this function works in place.

///*
///* For further information view:

///*  Cormen, Leiversn, Rivest, and Stein: Introduction to Algorithms

///*  Wikipedia article on bubble sort.
///*
///* @param Vector: values the vector which determies the sorting
///* @param Vector: indices to be sorted going along
///*
///* @return TODO
///* ***************************************************************************
double FMath::sh_eval( const ColumnVector& position, const ColumnVector& coeff )
{
    const int max_order( ( -3 + static_cast<int>( sqrt( 8 * coeff.Nrows() ) + 1 ) ) / 2 );

    const double radius( position( 1 ) );
    if ( radius == 0.0 )
    {
        return 0.0;
    }

    // for easier readability
    const double theta( position( 2 ) );
    const double phi( position( 3 ) );

    double result( 0 );
    for ( int order( 0 ), j( 1 ); order <= max_order; order += 2 )
    {
        for ( int degree( -order ); degree <= order; degree++, ++j )
        {
            result += coeff( j ) * FMath::sh_base_function( order, degree, theta, phi );
        }
    }
    return result;
}

// calculate rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
Matrix FMath::RotationMatrix( const double angle, const ColumnVector& axis )
{
    double s( sin( angle ) );
    double c( cos( angle ) );
    double d( 1. - c );

    Matrix R( 3, 3 );
    R( 1, 1 ) = c + axis( 1 ) * axis( 1 ) * d;
    R( 1, 2 ) = axis( 1 ) * axis( 2 ) * d - axis( 3 ) * s;
    R( 1, 3 ) = axis( 1 ) * axis( 3 ) * d + axis( 2 ) * s;
    R( 2, 1 ) = axis( 1 ) * axis( 2 ) * d + axis( 3 ) * s;
    R( 2, 2 ) = c + axis( 1 ) * axis( 1 ) * d;
    R( 2, 3 ) = axis( 2 ) * axis( 3 ) * d - axis( 1 ) * s;
    R( 3, 1 ) = axis( 1 ) * axis( 3 ) * d - axis( 2 ) * s;
    R( 3, 2 ) = axis( 2 ) * axis( 3 ) * d + axis( 1 ) * s;
    R( 3, 3 ) = c + axis( 3 ) * axis( 3 ) * d;

    return R;
}

void FMath::debugColumnVector3( const ColumnVector& v, QString name )
{
    if ( v.Nrows() != 3 )
    {
        qDebug() << name << "error not 3 elements in vector";
    }
    qDebug() << name << v( 1 ) << v( 2 ) << v( 3 );
}

Matrix FMath::pseudoInverse( const Matrix& A )
{
    /*
    Matrix U( A.Nrows(), A.Ncols() );
    DiagonalMatrix D( A.Ncols() );
    Matrix V( A.Ncols(), A.Ncols() );
    SVD( A, D, U, V );
    return ( V * ( D.t() * D ) * D.t() * U.t() );
    */

    return ( ( A.t() * A ).i() * A.t() );
}

void FMath::fitTensors( QVector<ColumnVector>& data, QVector<float>& b0Images, QVector<QVector3D>& bvecs, QVector<float>& bvals, QVector<Matrix>& out )
{
    int N = bvecs.size();

    Matrix B( N, 6 );
    Matrix U( N, 6 );
    Matrix V( 6, 6 );
    Matrix BI( 6, N );
    DiagonalMatrix D( 6 );

    double mult_c;

    for ( int i = 0; i < N; ++i )
    {
        mult_c = bvals[i] / (float) ( bvecs[i].x() * bvecs[i].x() + bvecs[i].y() * bvecs[i].y() + bvecs[i].z() * bvecs[i].z() );

        B( i + 1, 1 ) = mult_c * bvecs[i].x() * bvecs[i].x();
        B( i + 1, 2 ) = 2 * mult_c * bvecs[i].x() * bvecs[i].y();
        B( i + 1, 3 ) = 2 * mult_c * bvecs[i].x() * bvecs[i].z();
        B( i + 1, 4 ) = mult_c * bvecs[i].y() * bvecs[i].y();
        B( i + 1, 5 ) = 2 * mult_c * bvecs[i].y() * bvecs[i].z();
        B( i + 1, 6 ) = mult_c * bvecs[i].z() * bvecs[i].z();
    }

    SVD( B, D, U, V );

    for ( int j = 1; j <= 6; ++j )
    {
        D( j ) = 1. / D( j );
    }
    BI = V * D * U.t();

    double s0 = 0.0;
    double si = 0.0;
    vector<double> log_s0_si_pixel( N );

    out.clear();
    out.reserve( data.size() );

    Matrix blank( 3, 3 );
    blank = 0.0;

    for ( int i = 0; i < data.size(); ++i )
    {
        s0 = b0Images.at( i );

        if ( s0 > 0 )
        {
            // compute log(s0)-log(si)
            s0 = log( s0 );
            for ( int j = 0; j < N; ++j )
            {
                si = data.at( i )( j + 1 ); //dti[j*blockSize+i];
                if ( si > 0 )
                {
                    si = log( si );
                }
                else
                {
                    si = 0.0;
                }
                log_s0_si_pixel[j] = s0 - si;
            }

            double value;
            // compute tensor
            ColumnVector t( 6 );
            for ( int l = 0; l < 6; l++ )
            {
                value = 0;
                for ( int m = 1; m <= N; ++m )
                {
                    value += BI( l + 1, m ) * log_s0_si_pixel[m - 1];
                }
                t( l + 1 ) = (float) ( value ); // save the tensor components in a adjacent memory
            }
            Matrix m( 3, 3 );
            m( 1, 1 ) = t( 1 );
            m( 1, 2 ) = t( 2 );
            m( 1, 3 ) = t( 3 );
            m( 2, 1 ) = t( 2 );
            m( 2, 2 ) = t( 4 );
            m( 2, 3 ) = t( 5 );
            m( 3, 1 ) = t( 3 );
            m( 3, 2 ) = t( 5 );
            m( 3, 3 ) = t( 6 );

            out.push_back( m );
        } // end if s0 > 0
        else
        {
            out.push_back( blank );
        }
    }
}

void FMath::fa( QVector<Matrix>& tensors, QVector<float>& faOut )
{
    int blockSize = tensors.size();

    faOut.resize( blockSize );

    QVector<float> trace( blockSize );
    float value = 0;
    for ( int i = 0; i < blockSize; ++i )
    {
        value = tensors.at( i )( 1, 1 );
        value += tensors.at( i )( 2, 2 );
        value += tensors.at( i )( 3, 3 );
        trace[i] = value / 3.0;
    }

    QVector<float> fa( blockSize );

    double xx, xy, xz, yy, yz, zz, tr, AA, DD;

    for ( int i = 0; i < blockSize; ++i )
    {
        xx = tensors.at( i )( 1, 1 );
        xy = tensors.at( i )( 1, 2 );
        xz = tensors.at( i )( 1, 3 );
        yy = tensors.at( i )( 2, 2 );
        yz = tensors.at( i )( 2, 3 );
        zz = tensors.at( i )( 3, 3 );
        tr = trace[i];

        AA = pow2( xx - tr ) + pow2( yy - tr ) + pow2( zz - tr ) + 2 * pow2( xy ) + 2 * pow2( xz ) + 2 * pow2( yz );
        DD = pow2( xx ) + pow2( yy ) + pow2( zz ) + 2 * pow2( xy ) + 2 * pow2( xz ) + 2 * pow2( yz );

        if ( DD > 0 )
        {
            faOut[i] = qMin( 1.0f, (float) ( sqrt( AA ) / sqrt( DD ) * sqrt( 1.5 ) ) );
        }
        else
        {
            faOut[i] = 0.0;
        }
    }
}

float FMath::fa( Matrix tensor )
{
    float trace;
    float value = 0;
    value = tensor( 1, 1 );
    value += tensor( 2, 2 );
    value += tensor( 3, 3 );
    trace = value / 3.0;

    float fa;

    double xx, xy, xz, yy, yz, zz, tr, AA, DD;

    xx = tensor( 1, 1 );
    xy = tensor( 1, 2 );
    xz = tensor( 1, 3 );
    yy = tensor( 2, 2 );
    yz = tensor( 2, 3 );
    zz = tensor( 3, 3 );
    tr = trace;

    AA = pow2( xx - tr ) + pow2( yy - tr ) + pow2( zz - tr ) + 2 * pow2( xy ) + 2 * pow2( xz ) + 2 * pow2( yz );
    DD = pow2( xx ) + pow2( yy ) + pow2( zz ) + 2 * pow2( xy ) + 2 * pow2( xz ) + 2 * pow2( yz );

    if ( DD > 0 )
    {
        fa = qMin( 1.0f, (float) ( sqrt( AA ) / sqrt( DD ) * sqrt( 1.5 ) ) );
    }
    else
    {
        fa = 0.0;
    }
    return fa;
}

void FMath::evec1( QVector<Matrix>& tensors, QVector<QVector3D>& evec1 )
{
    int blockSize = tensors.size();

    evec1.resize( blockSize );

    double xx, xy, xz, yy, yz, zz;
    double i1, i2, i3, v, s, phi, l1, l2, l3;
    double vec_norm, ev1_x, ev1_y, ev1_z;

    for ( int i = 0; i < blockSize; ++i )
    {
        xx = tensors.at( i )( 1, 1 );
        xy = tensors.at( i )( 1, 2 );
        xz = tensors.at( i )( 1, 3 );
        yy = tensors.at( i )( 2, 2 );
        yz = tensors.at( i )( 2, 3 );
        zz = tensors.at( i )( 3, 3 );

        // three invariants of D (dt)
        // i1=l1+l2+l3 (trace)
        // i2=l1*l2+l1*l3+l2*l3
        // i3=l1*l2*l3 (determinante)
        i1 = xx + yy + zz;
        i2 = xx * yy + xx * zz + yy * zz - ( pow2( xy ) + pow2( xz ) + pow2( yz ) );
        i3 = xx * yy * zz + 2. * xy * xz * yz - ( zz * pow2( xy ) + yy * pow2( xz ) + xx * pow2( yz ) );

        v = pow2( i1 / 3 ) - i2 / 3;
        s = pow3( i1 / 3 ) - i1 * i2 / 6 + i3 / 2;
        if ( ( v > 0 ) && ( pow2( s ) < pow3( v ) ) )
            phi = acos( s / v * sqrt( 1. / v ) ) / 3;
        else
            phi = 0;

        // eigenvalues
        if ( phi != 0 )
        {
            l1 = i1 / 3 + 2 * sqrt( v ) * cos( phi );
            l2 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. + phi );
            l3 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. - phi );
        }
        else
            l1 = l2 = l3 = 0.0;

        // eigenvectors
        ev1_x = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * yz - ( zz - l1 ) * xy );
        ev1_y = ( xz * yz - ( zz - l1 ) * xy ) * ( xz * xy - ( xx - l1 ) * yz );
        ev1_z = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * xy - ( xx - l1 ) * yz );

        vec_norm = sqrt( pow2( ev1_x ) + pow2( ev1_y ) + pow2( ev1_z ) );

        if ( vec_norm > 0 )
        {
            ev1_x = ev1_x / vec_norm;
            ev1_y = ev1_y / vec_norm;
            ev1_z = ev1_z / vec_norm;
        }
        else
            ev1_x = ev1_y = ev1_z = 0.0;

        evec1[i].setX( ev1_x );
        evec1[i].setY( ev1_y );
        evec1[i].setZ( ev1_z );
    }
}

void FMath::evecs( QVector<Matrix>& tensors, QVector<QVector3D>& evec1, QVector<float>& eval1,
                                               QVector<QVector3D>& evec2, QVector<float>& eval2,
                                               QVector<QVector3D>& evec3, QVector<float>& eval3 )
{
    int blockSize = tensors.size();

    evec1.resize( blockSize );
    evec2.resize( blockSize );
    evec3.resize( blockSize );
    eval1.resize( blockSize );
    eval2.resize( blockSize );
    eval3.resize( blockSize );

    double xx, xy, xz, yy, yz, zz;
    double i1, i2, i3, v, s, phi, l1, l2, l3;
    double vec_norm, ev1_x, ev1_y, ev1_z, ev2_x, ev2_y, ev2_z, ev3_x, ev3_y, ev3_z;

    for ( int i = 0; i < blockSize; ++i )
    {
        xx = tensors.at( i )( 1, 1 );
        xy = tensors.at( i )( 1, 2 );
        xz = tensors.at( i )( 1, 3 );
        yy = tensors.at( i )( 2, 2 );
        yz = tensors.at( i )( 2, 3 );
        zz = tensors.at( i )( 3, 3 );

        // three invariants of D (dt)
        // i1=l1+l2+l3 (trace)
        // i2=l1*l2+l1*l3+l2*l3
        // i3=l1*l2*l3 (determinante)
        i1 = xx + yy + zz;
        i2 = xx * yy + xx * zz + yy * zz - ( pow2( xy ) + pow2( xz ) + pow2( yz ) );
        i3 = xx * yy * zz + 2. * xy * xz * yz - ( zz * pow2( xy ) + yy * pow2( xz ) + xx * pow2( yz ) );

        v = pow2( i1 / 3 ) - i2 / 3;
        s = pow3( i1 / 3 ) - i1 * i2 / 6 + i3 / 2;
        if ( ( v > 0 ) && ( pow2( s ) < pow3( v ) ) )
            phi = acos( s / v * sqrt( 1. / v ) ) / 3;
        else
            phi = 0;

        // eigenvalues
        if ( phi != 0 )
        {
            l1 = i1 / 3 + 2 * sqrt( v ) * cos( phi );
            l2 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. + phi );
            l3 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. - phi );
        }
        else
            l1 = l2 = l3 = 0.0;

        eval1[i] = l1;
        eval2[i] = l2;
        eval3[i] = l3;

        // eigenvectors
        ev1_x = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * yz - ( zz - l1 ) * xy );
        ev1_y = ( xz * yz - ( zz - l1 ) * xy ) * ( xz * xy - ( xx - l1 ) * yz );
        ev1_z = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * xy - ( xx - l1 ) * yz );

        ev2_x = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * yz - ( zz - l2 ) * xy );
        ev2_y = ( xz * yz - ( zz - l2 ) * xy ) * ( xz * xy - ( xx - l2 ) * yz );
        ev2_z = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * xy - ( xx - l2 ) * yz );

        ev3_x = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * yz - ( zz - l3 ) * xy );
        ev3_y = ( xz * yz - ( zz - l3 ) * xy ) * ( xz * xy - ( xx - l3 ) * yz );
        ev3_z = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * xy - ( xx - l3 ) * yz );

        vec_norm = sqrt( pow2( ev1_x ) + pow2( ev1_y ) + pow2( ev1_z ) );

        if ( vec_norm > 0 )
        {
            ev1_x = ev1_x / vec_norm;
            ev1_y = ev1_y / vec_norm;
            ev1_z = ev1_z / vec_norm;
        }
        else
            ev1_x = ev1_y = ev1_z = 0.0;

        vec_norm = sqrt( pow2( ev2_x ) + pow2( ev2_y ) + pow2( ev2_z ) );

        if ( vec_norm > 0 )
        {
            ev2_x = ev2_x / vec_norm;
            ev2_y = ev2_y / vec_norm;
            ev2_z = ev2_z / vec_norm;
        }
        else
            ev2_x = ev2_y = ev2_z = 0.0;

        vec_norm = sqrt( pow2( ev3_x ) + pow2( ev3_y ) + pow2( ev3_z ) );

        if ( vec_norm > 0 )
        {
            ev3_x = ev3_x / vec_norm;
            ev3_y = ev3_y / vec_norm;
            ev3_z = ev3_z / vec_norm;
        }
        else
            ev3_x = ev3_y = ev3_z = 0.0;

        evec1[i].setX( ev1_x );
        evec1[i].setY( ev1_y );
        evec1[i].setZ( ev1_z );
        evec2[i].setX( ev2_x );
        evec2[i].setY( ev2_y );
        evec2[i].setZ( ev2_z );
        evec3[i].setX( ev3_x );
        evec3[i].setY( ev3_y );
        evec3[i].setZ( ev3_z );
    }
}

Matrix FMath::expT( Matrix& t )
{
    double xx, xy, xz, yy, yz, zz;
    double i1, i2, i3, v, s, phi, l1, l2, l3;
    double ev1_x, ev1_y, ev1_z, ev2_x, ev2_y, ev2_z, ev3_x, ev3_y, ev3_z, vec_norm;

    xx = t( 1, 1 );
    xy = t( 1, 2 );
    xz = t( 1, 3 );
    yy = t( 2, 2 );
    yz = t( 2, 3 );
    zz = t( 3, 3 );

    // three invariants of D (dt)
    // i1=l1+l2+l3 (trace)
    // i2=l1*l2+l1*l3+l2*l3
    // i3=l1*l2*l3 (determinante)
    i1 = xx + yy + zz;
    i2 = xx * yy + xx * zz + yy * zz - ( FMath::pow2( xy ) + FMath::pow2( xz ) + FMath::pow2( yz ) );
    i3 = xx * yy * zz + 2. * xy * xz * yz - ( zz * FMath::pow2( xy ) + yy * FMath::pow2( xz ) + xx * FMath::pow2( yz ) );

    v = FMath::pow2( i1 / 3 ) - i2 / 3;
    s = FMath::pow3( i1 / 3 ) - i1 * i2 / 6 + i3 / 2;
    if ( ( v > 0 ) && ( FMath::pow2( s ) < FMath::pow3( v ) ) )
        phi = acos( s / v * sqrt( 1. / v ) ) / 3;
    else
        phi = 0;

    // eigenvalues
    if ( phi != 0 )
    {
        l1 = i1 / 3 + 2 * sqrt( v ) * cos( phi );
        l2 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. + phi );
        l3 = i1 / 3 - 2 * sqrt( v ) * cos( M_PI / 3. - phi );
    }
    else
        l1 = l2 = l3 = 0.0;
    /*
     eval1[i] = l1;
     eval2[i] = l2;
     eval3[i] = l3;
     */
    // eigenvectors
    ev1_x = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * yz - ( zz - l1 ) * xy );
    ev1_y = ( xz * yz - ( zz - l1 ) * xy ) * ( xz * xy - ( xx - l1 ) * yz );
    ev1_z = ( xy * yz - ( yy - l1 ) * xz ) * ( xz * xy - ( xx - l1 ) * yz );

    ev2_x = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * yz - ( zz - l2 ) * xy );
    ev2_y = ( xz * yz - ( zz - l2 ) * xy ) * ( xz * xy - ( xx - l2 ) * yz );
    ev2_z = ( xy * yz - ( yy - l2 ) * xz ) * ( xz * xy - ( xx - l2 ) * yz );

    ev3_x = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * yz - ( zz - l3 ) * xy );
    ev3_y = ( xz * yz - ( zz - l3 ) * xy ) * ( xz * xy - ( xx - l3 ) * yz );
    ev3_z = ( xy * yz - ( yy - l3 ) * xz ) * ( xz * xy - ( xx - l3 ) * yz );

    vec_norm = sqrt( FMath::pow2( ev1_x ) + FMath::pow2( ev1_y ) + FMath::pow2( ev1_z ) );

    if ( vec_norm > 0 )
    {
        ev1_x = ev1_x / vec_norm;
        ev1_y = ev1_y / vec_norm;
        ev1_z = ev1_z / vec_norm;
    }
    else
        ev1_x = ev1_y = ev1_z = 0.0;

    vec_norm = sqrt( FMath::pow2( ev2_x ) + FMath::pow2( ev2_y ) + FMath::pow2( ev2_z ) );

    if ( vec_norm > 0 )
    {
        ev2_x = ev2_x / vec_norm;
        ev2_y = ev2_y / vec_norm;
        ev2_z = ev2_z / vec_norm;
    }
    else
        ev2_x = ev2_y = ev2_z = 0.0;

    vec_norm = sqrt( FMath::pow2( ev3_x ) + FMath::pow2( ev3_y ) + FMath::pow2( ev3_z ) );

    if ( vec_norm > 0 )
    {
        ev3_x = ev3_x / vec_norm;
        ev3_y = ev3_y / vec_norm;
        ev3_z = ev3_z / vec_norm;
    }
    else
        ev3_x = ev3_y = ev3_z = 0.0;

    Matrix U( 3, 3 );
    DiagonalMatrix D( 3 );

    U( 1, 1 ) = ev1_x;
    U( 2, 1 ) = ev1_y;
    U( 3, 1 ) = ev1_z;
    U( 1, 2 ) = ev2_x;
    U( 2, 2 ) = ev2_y;
    U( 3, 2 ) = ev2_z;
    U( 1, 3 ) = ev3_x;
    U( 2, 3 ) = ev3_y;
    U( 3, 3 ) = ev3_z;
    D( 1 ) = exp( l1 );
    D( 2 ) = exp( l2 );
    D( 3 ) = exp( l3 );
    Matrix expM(3,3);
    expM = U * D * U.t();

    return expM;
}
//void evd3x3_2( ColumnVector tensor, QVector<ColumnVector>& vecs, ColumnVector& vals );
void FMath::evd3x3_2( ColumnVector &d, QVector<ColumnVector>& vec, ColumnVector& val )
{
    if ( d.Nrows() != 6 )
        throw std::invalid_argument( "Tensor (6-component vector) expected!" );

    // calculate the eigenvalues:
    val = ColumnVector( 3 );
    vec.reserve( 3 );
    std::vector<int> index( 3 );

    ColumnVector e( 3 );
    // Create work variables:
    Matrix A( 3, 3 ), Q( 3, 3 );
    A( 1, 1 ) = d( 1 );
    A( 2, 1 ) = A( 1, 2 ) = d( 2 );
    A( 3, 1 ) = A( 1, 3 ) = d( 3 );
    A( 2, 2 ) = d( 4 );
    A( 3, 2 ) = A( 2, 3 ) = d( 5 );
    A( 3, 3 ) = d( 6 );

    double m, c1, c0;

    // Determine coefficients of characteristic poynomial. We write
    //       | a   d   f  |
    //  A =  | d*  b   e  |
    //       | f*  e*  c  |
    double de = A( 1, 2 ) * A( 2, 3 );                                    // d * e
    double dd = A( 1, 2 ) * A( 1, 2 );                                         // d^2
    double ee = A( 2, 3 ) * A( 2, 3 );                                         // e^2
    double ff = A( 1, 3 ) * A( 1, 3 );                                         // f^2
    m = A( 1, 1 ) + A( 2, 2 ) + A( 3, 3 );
    c1 = ( A( 1, 1 ) * A( 2, 2 ) + A( 1, 1 ) * A( 3, 3 ) + A( 2, 2 ) * A( 3, 3 ) )        // a*b + a*c + b*c - d^2 - e^2 - f^2
    - ( dd + ee + ff );
    c0 = A( 3, 3 ) * dd + A( 1, 1 ) * ee + A( 2, 2 ) * ff - A( 1, 1 ) * A( 2, 2 ) * A( 3, 3 ) - 2.0 * A( 1, 3 ) * de; // c*d^2 + a*e^2 + b*f^2 - a*b*c - 2*f*d*e)

    double p, sqrt_p, q, c, s, phi;
    p = m * m - 3.0 * c1;
    q = m * ( p - ( 3.0 / 2.0 ) * c1 ) - ( 27.0 / 2.0 ) * c0;
    sqrt_p = sqrt( fabs( p ) );

    phi = 27.0 * ( 0.25 * c1 * c1 * ( p - c1 ) + c0 * ( q + 27.0 / 4.0 * c0 ) );
    phi = ( 1.0 / 3.0 ) * atan2( sqrt( fabs( phi ) ), q );

    c = sqrt_p * cos( phi );
    s = ( 1.0 / sqrt( 3.0 ) ) * sqrt_p * sin( phi );

    e( 2 ) = ( 1.0 / 3.0 ) * ( m - c );
    e( 3 ) = e( 2 ) + s;
    e( 1 ) = e( 2 ) + c;
    e( 2 ) -= s;

    if ( e( 1 ) > e( 2 ) )
    {
        if ( e( 1 ) > e( 3 ) )
        {
            if ( e( 2 ) > e( 3 ) )
            {
                index = { 0,1,2};
            }
            else
            {
                index = {0,2,1};
            }
        }
        else
        {
            index = { 2,0,1};
        }
    }
    else
    {
        if( e(2) > e(3) )
        {
            if( e(1) > e(3) )
            {
                index = {   1,0,2};
            }
            else
            {
                index = {   1,2,0};
            }
        }
        else
        {
            index = {2,1,0};
        }
    }

    for ( unsigned long i( 1 ); i < 4; ++i )
        val( i ) = e( index[i-1]+1 );

    double wmax = ( fabs( val( 1 ) ) > fabs( val( 3 ) ) ) ? val( 1 ) : val( 3 );
    double thresh = ( 8.0 * DBL_EPSILON * wmax ) * ( 8.0 * DBL_EPSILON * wmax );

    // Prepare calculation of eigenvectors:
    double n0tmp = A( 1, 2 ) * A( 1, 2 ) + A( 1, 3 ) * A( 1, 3 );
    double n1tmp = A( 1, 2 ) * A( 1, 2 ) + A( 2, 3 ) * A( 2, 3 );
    Q( 1, 2 ) = A( 1, 2 ) * A( 2, 3 ) - A( 1, 3 ) * A( 2, 2 );
    Q( 2, 2 ) = A( 1, 3 ) * A( 2, 1 ) - A( 2, 3 ) * A( 1, 1 );
    Q( 3, 2 ) = A( 1, 2 ) * A( 1, 2 );

    // Calculate first eigenvector by the formula
    //   v(0) = (A - w(0)).e1 x (A - w(0)).e2
    A( 1, 1 ) -= val( 1 );
    A( 2, 2 ) -= val( 1 );
    Q( 1, 1 ) = Q( 1, 2 ) + A( 1, 3 ) * val( 1 );
    Q( 2, 1 ) = Q( 2, 2 ) + A( 2, 3 ) * val( 1 );
    Q( 3, 1 ) = A( 1, 1 ) * A( 2, 2 ) - Q( 3, 2 );
    double norm = Q( 1, 1 ) * Q( 1, 1 ) + Q( 2, 1 ) * Q( 2, 1 ) + Q( 3, 1 ) * Q( 3, 1 );
    double n0 = n0tmp + A( 1, 1 ) * A( 1, 1 );
    double n1 = n1tmp + A( 2, 2 ) * A( 2, 2 );
    double error = n0 * n1;
    double t( 0 ), f( 0 );
    unsigned long i( 0 ), j( 0 );

    if ( n0 <= thresh )         // If the first column is zero, then (1,0,0) is an eigenvector
    {
        Q( 1, 1 ) = 1.0;
        Q( 2, 1 ) = 0.0;
        Q( 3, 1 ) = 0.0;
    }
    else if ( n1 <= thresh )    // If the second column is zero, then (0,1,0) is an eigenvector
    {
        Q( 1, 1 ) = 0.0;
        Q( 2, 1 ) = 1.0;
        Q( 3, 1 ) = 0.0;
    }
    else if ( norm < 64.0 * 64.0 * DBL_EPSILON * DBL_EPSILON * error )
    {                         // If angle between A(0) and A(1) is too small, don't use
        t = A( 1, 2 ) * A( 1, 2 );       // cross product, but calculate v ~ (1, -A0/A1, 0)
        f = -A( 1, 1 ) / A( 1, 2 );
        if ( A( 2, 2 ) * A( 2, 2 ) > t )
        {
            t = A( 2, 2 ) * A( 2, 2 );
            f = -A( 1, 2 ) / A( 2, 2 );
        }
        if ( A( 2, 3 ) * A( 2, 3 ) > t )
            f = -A( 1, 3 ) / A( 2, 3 );
        norm = 1.0 / sqrt( 1 + f * f );
        Q( 1, 1 ) = norm;
        Q( 2, 1 ) = f * norm;
        Q( 3, 1 ) = 0.0;
    }
    else                      // This is the standard branch
    {
        norm = sqrt( 1.0 / norm );
        for ( j = 1; j < 4; ++j )
            Q( j, 1 ) = Q( j, 1 ) * norm;
    }

    // Prepare calculation of second eigenvector
    t = val( 1 ) - val( 2 );
    if ( fabs( t ) > 8.0 * DBL_EPSILON * wmax )
    {
        // For non-degenerate eigenvalue, calculate second eigenvector by the formula
        //   v(1) = (A - w(1)).e1 x (A - w(1)).e2
        A( 1, 1 ) += t;
        A( 2, 2 ) += t;
        Q( 1, 2 ) = Q( 1, 2 ) + A( 1, 3 ) * val( 1 );
        Q( 2, 2 ) = Q( 2, 2 ) + A( 2, 3 ) * val( 1 );
        Q( 3, 2 ) = A( 1, 1 ) * A( 2, 2 ) - Q( 3, 2 );
        norm = Q( 1, 2 ) * Q( 1, 2 ) + Q( 2, 2 ) * Q( 2, 2 ) + Q( 3, 2 ) * Q( 3, 2 );
        n0 = n0tmp + A( 1, 1 ) * A( 1, 1 );
        n1 = n1tmp + A( 2, 2 ) * A( 2, 2 );
        error = n0 * n1;

        if ( n0 <= thresh )       // If the first column is zero, then (1,0,0) is an eigenvector
        {
            Q( 1, 2 ) = 1.0;
            Q( 2, 2 ) = 0.0;
            Q( 3, 2 ) = 0.0;
        }
        else if ( n1 <= thresh )  // If the second column is zero, then (0,1,0) is an eigenvector
        {
            Q( 1, 2 ) = 0.0;
            Q( 2, 2 ) = 1.0;
            Q( 3, 2 ) = 0.0;
        }
        else if ( norm < 64.0 * DBL_EPSILON * 64.0 * DBL_EPSILON * error )
        {                       // If angle between A(0) and A(1) is too small, don't use
            t = A( 1, 2 ) * A( 1, 2 );     // cross product, but calculate v ~ (1, -A0/A1, 0)
            f = -A( 1, 1 ) / A( 1, 2 );
            if ( A( 2, 2 ) * A( 2, 2 ) > t )
            {
                t = A( 2, 2 ) * A( 2, 2 );
                f = -A( 1, 2 ) / A( 2, 2 );
            }
            if ( A( 2, 3 ) * A( 2, 3 ) > t )
                f = -A( 1, 3 ) / A( 2, 3 );
            norm = 1.0 / sqrt( 1 + f * f );
            Q( 1, 2 ) = norm;
            Q( 2, 2 ) = f * norm;
            Q( 3, 2 ) = 0.0;
        }
        else
        {
            norm = sqrt( 1.0 / norm );
            for ( j = 1; j < 4; ++j )
                Q( j, 2 ) = Q( j, 2 ) * norm;
        }
    }
    else
    {
        // For degenerate eigenvalue, calculate second eigenvector according to
        //   v(1) = v(0) x (A - w(1)).e(i)
        //
        // This would really get to complicated if we could not assume all of A to
        // contain meaningful values.
        A( 2, 1 ) = A( 1, 2 );
        A( 3, 1 ) = A( 1, 3 );
        A( 3, 2 ) = A( 2, 3 );
        A( 1, 1 ) += val( 1 );
        A( 2, 2 ) += val( 1 );
        for ( i = 1; i < 4; ++i )
        {
            A( i, i ) -= val( 2 );
            n0 = A( 1, i ) * A( 1, i ) + A( 2, i ) * A( 2, i ) + A( 3, i ) * A( 3, i );
            if ( n0 > thresh )
            {
                Q( 1, 2 ) = Q( 2, 1 ) * A( 3, i ) - Q( 3, 1 ) * A( 2, i );
                Q( 2, 2 ) = Q( 3, 1 ) * A( 1, i ) - Q( 1, 1 ) * A( 3, i );
                Q( 3, 2 ) = Q( 1, 1 ) * A( 2, i ) - Q( 2, 1 ) * A( 1, i );
                norm = Q( 1, 2 ) * Q( 1, 2 ) + Q( 2, 2 ) * Q( 2, 2 ) + Q( 3, 2 ) * Q( 3, 2 );
                if ( norm > 256.0 * DBL_EPSILON * 256.0 * DBL_EPSILON * n0 ) // Accept cross product only if the angle between
                {                                         // the two vectors was not too small
                    norm = sqrt( 1.0 / norm );
                    for ( j = 1; j < 4; ++j )
                        Q( j, 2 ) = Q( j, 2 ) * norm;
                    break;
                }
            }
        }

        if ( i == 4 )    // This means that any vector orthogonal to v(0) is an EV.
        {
            for ( j = 1; j < 4; ++j )
                if ( Q( j, 1 ) != 0.0 )                                   // Find nonzero element of v(0) ...
                {                                                     // ... and swap it with the next one
                    norm = 1.0 / sqrt( Q( j, 1 ) * Q( j, 1 ) + Q( ( j + 2 ) % 3 + 1, 1 ) * Q( ( j + 1 ) % 3 + 1, 1 ) );
                    Q( j, 2 ) = Q( ( j + 1 ) % 3 + 1, 1 ) * norm;
                    Q( ( j + 1 ) % 3 + 1, 2 ) = -Q( j, 1 ) * norm;
                    Q( ( j + 2 ) % 3 + 1, 2 ) = 0.0;
                    break;
                }
        }
    }

    // Calculate third eigenvector according to
    //   v(2) = v(0) x v(1)
    Q( 1, 3 ) = Q( 2, 1 ) * Q( 3, 2 ) - Q( 3, 1 ) * Q( 2, 2 );
    Q( 2, 3 ) = Q( 3, 1 ) * Q( 1, 2 ) - Q( 1, 1 ) * Q( 3, 2 );
    Q( 3, 3 ) = Q( 1, 1 ) * Q( 2, 2 ) - Q( 2, 1 ) * Q( 1, 2 );

    vec.clear();
    for ( int ii( 1 ); ii < 4; ++ii )
    {
        ColumnVector tmp( 3 );

        tmp( 1 ) = Q( 1, ii );
        tmp( 2 ) = Q( 2, ii );
        tmp( 3 ) = Q( 3, ii );

        vec.push_back( tmp );
    }

}

bool FMath::linePlaneIntersection( QVector3D& contact, QVector3D ray, QVector3D rayOrigin, QVector3D normal, QVector3D coord )
{
    // calculate plane
    float d = QVector3D::dotProduct( normal, coord );

    if ( QVector3D::dotProduct( normal, ray ) == 0 )
    {
        return false; // avoid divide by zero
    }

    // Compute the t value for the directed line ray intersecting the plane
    float t = ( d - QVector3D::dotProduct( normal, rayOrigin ) ) / QVector3D::dotProduct( normal, ray );

    // scale the ray by t
    QVector3D newRay = ray * t;

    // calc contact point
    contact = rayOrigin + newRay;

    if ( t >= 0.0f && t <= 1.0f )
    {
        return true; // line intersects plane
    }
    return false; // line does not
}


#include <optional>

#include <boost/dynamic_bitset.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "decode.hpp"

using namespace std;
using namespace boost::numeric;
using boost::dynamic_bitset;

static const auto parity_mat = [] {
    ublas::matrix<bool> parity_m(3, 7);
    vector<bool> parity_init = {
        1, 0, 1, 0, 1, 0, 1,
        0, 1, 1, 0, 0, 1, 1,
        0, 0, 0, 1, 1, 1, 1
    };

    copy(parity_init.begin(),
         parity_init.end(),
         parity_m.data().begin()
    );

    return parity_m;
}();

static const auto decode_mat = [] {
    ublas::matrix<bool> decode_m(4, 7);
    vector<bool> decode_init = {
        0, 0, 1, 0, 0, 0, 0,
        0, 0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 0, 1
    };

    copy(decode_init.begin(),
         decode_init.end(),
         decode_m.data().begin()
    );

    return decode_m;
}();

optional<uint8_t> parity_check(const ublas::matrix<bool> &code_mat) {
    auto syndrome = ublas::prod(parity_mat, code_mat);

    dynamic_bitset<uint8_t> error_bs;
    for_each(syndrome.begin1(), syndrome.end1(), [&error_bs](auto v) {
        error_bs.push_back(v % 2);
    });

    auto error = error_bs.to_ulong();

    if (error == 0) { return {}; }
    else { return error - 1; }
}

vector<uint8_t> decode(const vector<uint8_t> &bytes) {
    dynamic_bitset<uint8_t> data;
    data.init_from_block_range(bytes.begin(), bytes.end());

    bool back = data[data.size() - 1]; data.pop_back();
    while (!back) {
        back = data[data.size() - 1]; data.pop_back();
    }

    dynamic_bitset<uint8_t> result;

    for (auto p = 0u; p < data.size(); p += 7) {
        ublas::matrix<bool> code_mat(7, 1);
        vector<bool> code_init;

        for (auto i = 0u; i < 7; ++i) {
            code_init.push_back(data[p+i]);
        }

        copy(code_init.begin(),
             code_init.end(),
             code_mat.data().begin()
        );

        if (auto error = parity_check(code_mat); error) {
            code_mat(error.value(), 0) ^= true;
        }

        auto prod = ublas::prod(decode_mat, code_mat);

        for_each(prod.begin1(), prod.end1(), [&result] (auto v) {
            result.push_back(v % 2);
        });
    }

    vector<uint8_t> blocks(result.num_blocks());
    to_block_range(result, blocks.begin());

    return blocks;
}