/*
MIT License

Copyright (c) 2019 Xiaohong Chen

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

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

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

#ifndef BOOST_UBLAS_PRECONDITIONER_HPP
#define BOOST_UBLAS_PRECONDITIONER_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/vector.hpp>

namespace boost { namespace numeric { namespace ublas {

template <class M>
class identity_precond {
public:
    identity_precond() {}
    identity_precond(const M&) {}

    template<class E>
    const vector_expression<E>& operator()(const vector_expression<E>& ve) const {
        return ve;
    }
};

template <class M>
class jacobi_precond {
public:
    typedef typename M::value_type value_type;
    typedef typename M::size_type size_type;
    jacobi_precond(const M& A)
        : diag_(A.size1()) {
        size_type size = A.size1();
        for (size_type i = 0; i < size; ++i) {
            diag_(i) = A(i, i);
        }
    }

    jacobi_precond(const jacobi_precond&) = default;
    jacobi_precond(jacobi_precond&&) = default;
    jacobi_precond& operator=(const jacobi_precond&) = default;
    jacobi_precond& operator=(jacobi_precond&&) = default;

    template<class E>
    decltype(auto) operator()(const vector_expression<E>& ve) const {
        return element_prod(ve, diag_);
    }

private:
    vector<value_type> diag_;
};

} // end namespace linear
} // end namespace solver
} // end namespace math

#endif


#include <iostream>
#include <armadillo.h>
#include <digitRecognition.h>
//#include <mkl.h>

using namespace arma;

constexpr int training_size = 4000;    //m
constexpr int input_layer_size = 784;  //k
constexpr int hidden_layer_size = 500;  //n
constexpr int num_labels = 10;

constexpr double lambda = 0.1;
constexpr int max_iterations = 1000;

int main () {
//	std::cout << "Starting program with " << mkl_get_max_threads() << " threads..." << std::endl;

	//mkl_set_num_threads(4);

    mat predictions(training_size,1,fill::zeros);

    mat x_train(training_size,input_layer_size,fill::zeros);
    //x_train.load("c:\\train_x_4000.csv");
    x_train.load("./mnist_data/train_x_4000.csv");

    mat y_train(training_size,1,fill::randu);
    //y_train.load("c:\\train_y_4000.csv");
    y_train.load("./mnist_data/train_y_4000.csv");

    mat s(10000,10000,fill::randu);
    mat t(10000,10000,fill::randu);
    mat u(10000,10000,fill::zeros);

	wall_clock timer;
	timer.tic();
    u = s * t;
    double n = timer.toc();
	std::cout << "10k x 10k matrix multiplication completed in: " << n << " seconds." << endl;
    pause();

    ////calculate mean of the training data
    //double x_mean = sum(sum(x_train));
    //x_mean /= x_train.n_elem;  //33.5026  TODO: check this
    //
    ////calculate standard devication of the training data
    //mat x_train_mean(training_size,input_layer_size);
    //x_train_mean = x_train;
    //x_train_mean.transform( [x_mean](double val){return (val - x_mean);} );
    //double x_std = sum(sum(x_train_mean));
    //x_std /= (x_train_mean.n_elem);
    //x_std = std::sqrt(x_std);  //7.23942e-06  TODO: check this

	x_train = x_train / 255.0;
	//x_train.transform([](double val){return (val/255.0); });
	//std::cout << x_train.row(0) << endl;

	mat initial_theta1(hidden_layer_size, input_layer_size + 1, fill::ones);  //784 26   n k+1
	//initial_theta1.load("c:\\theta1.csv");
	initial_theta1.load("./mnist_data/theta1.csv");
	mat initial_theta2(num_labels, hidden_layer_size + 1, fill::ones);        //25 11     numlabels n+1
	//initial_theta2.load("c:\\theta2.csv");
	initial_theta2.load("./mnist_data/theta2.csv");

	initial_theta1.reshape(hidden_layer_size*(input_layer_size + 1),1);
	initial_theta2.reshape(num_labels*(hidden_layer_size + 1),1);

    mat combined_theta = join_cols(initial_theta1,initial_theta2);
    //std::cout << "combined_theta rows: " << combined_theta.n_rows << ", cols: " << combined_theta.n_cols << endl;

	//test section----------------------
	mat gradient1 = combined_theta;
	double cost=0.0;

	    //costfunction(cost, gradient1, combined_theta, input_layer_size, hidden_layer_size, num_labels, x_train, y_train, lambda);
	    fmincg(cost, max_iterations,combined_theta,input_layer_size,hidden_layer_size,num_labels,x_train,y_train,lambda);

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

	std::cout << "Final cost: " << cost << endl;

    initial_theta1 = combined_theta.submat(0, 0, initial_theta1.n_cols-1, 0);
	initial_theta1.reshape(hidden_layer_size, input_layer_size + 1);
    //std::cout << "initial_theta1 rows: " << initial_theta1.n_rows << ", cols: " << initial_theta1.n_cols << endl;

    initial_theta2 = combined_theta.submat(initial_theta1.n_cols, 0,initial_theta1.n_cols+initial_theta2.n_cols-1, 0);
	initial_theta2.reshape(num_labels, hidden_layer_size + 1);
    //std::cout << "initial_theta2 rows: " << initial_theta2.n_rows << ", cols: " << initial_theta2.n_cols << endl;

    predict(initial_theta1,initial_theta2,x_train,predictions);

    //std::cout << "predictions: " << predictions.row(0) << endl;

	//pauseJNS();

    return 0;
}

//         real function costandgradient(gradient, nn_params,input_layer_size,hidden_layer_size,num_labels, inputdata, y, lambda)

//         real, allocatable :: inputdata(:,:), X(:,:), y(:,:), y_representative(:,:), gradient(:,:), ones(:,:)
//         real, allocatable :: a_1(:,:),z_2(:,:),a_2(:,:),z_3(:,:),a_3(:,:),hofx(:,:)
//         real, allocatable :: delta_2(:,:), temp_delta_2(:,:), delta_3(:,:), gradient_1(:,:), gradient_2(:,:)
//         real, allocatable :: Theta1_grad(:,:), Theta2_grad(:,:)
//         integer :: input_layer_size,hidden_layer_size,num_labels,m,l,K,i
//         integer :: a1,a2,b1,b2,c1,c2
//         real :: lambda, temp, J
//         real, allocatable :: nn_params(:,:), Theta1(:,:), Theta2(:,:), temptheta1(:,:),temptheta2(:,:)
//         real, allocatable :: a_2_ones(:,:), summation(:,:), summation2(:,:), summation3(:,:),z_2_ones(:,:), sigmoid_z_2(:,:)

//         real :: timer1, timer2, holder1, holder2
//         double precision :: average1 = 0.0
//         double precision :: average2 = 0.0
//         double precision :: average3 = 0.0
//         double precision :: counter = 0.0

//         m = size(inputdata,1)
//         l = size(y,1)
//         K = num_labels

//         allocate(Theta1(1:hidden_layer_size,1:(input_layer_size+1)))
//         allocate(Theta2(1:num_labels,1:(hidden_layer_size+1)))
//         allocate(ones(m,1))
//         allocate(X(size(inputdata,1),(size(inputdata,2)+1)))
//         allocate(y_representative(l,num_labels))
//         allocate(z_2(m,hidden_layer_size))
//         allocate(a_1(size(X,1),size(X,2)))
//         allocate(a_2(size(z_2,1),size(z_2,2)))
//         allocate(a_2_ones(size(a_2,1),(size(a_2,2)+1)))
//         allocate(z_3(size(a_2_ones,1),size(Theta2,1)))  !a_2 * Theta2'
//         allocate(temptheta2(size(Theta2,2),size(Theta2,1)))
//         allocate(a_3(size(z_3,1),size(z_3,2)))
//         allocate(summation(size(z_3,1),size(z_3,2)))
//         allocate(summation2(size(z_3,1),size(z_3,2)))
//         allocate(summation3(size(z_3,1),size(z_3,2)))
//         allocate(delta_3(size(a_3,1),size(a_3,2)))
//         allocate(z_2_ones(size(z_2,1),(size(z_2,2)+1)))
//         allocate(sigmoid_z_2(size(z_2_ones,1),size(z_2_ones,2)))
//         allocate(temp_delta_2(size(delta_3,1),size(Theta2,2)))
//         allocate(delta_2(size(temp_delta_2,1),size(sigmoid_z_2,2)))
//         allocate(gradient_2(size(delta_3,2),size(a_2_ones,2)))
//         allocate(gradient_1(size(delta_2,2)-1,size(a_1,2)))
//         allocate(Theta2_grad(1:size(gradient_2),1))
//         allocate(Theta1_grad(1:size(gradient_1),1))

//         allocate(temptheta1(size(Theta1,2),size(Theta1,1)))

//         Theta1 = reshape(nn_params(1:hidden_layer_size*(input_layer_size + 1),1), (/ hidden_layer_size, (input_layer_size + 1) /))
//         Theta2 = reshape(nn_params((1+hidden_layer_size*(input_layer_size + 1)):,1), (/ num_labels, (hidden_layer_size + 1) /))

//         !print *,shape(Theta1)   !500 x 785
//         !print *,shape(Theta2)   !10  x 501

//         ones = 1.0

//         !timer1=secnds(0.0)

//         do i=1,m
//             temp = y(i,1)
//             do j=1,K
//                 if(temp == j)then
//                     y_representative(i,j) = 1
//                 else
//                     y_representative(i,j) = 0
//                 end if
//             end do
//         end do

//         X(:,1:1) = ones
//         X(:,2:) = inputdata
//         a_1 = X

//         z_2 = 0.0

//         call matrix_multiply(a_1,0,Theta1,1,z_2)

//         a_2 = 0.0
//         call sigmoid(z_2,a_2)

//         a_2_ones(:,1:1) = ones
//         a_2_ones(:,2:) = a_2

//         z_3 = 0.0
//         call matrix_multiply(a_2_ones,0,Theta2,1,z_3)

//         call sigmoid(z_3,a_3)

//         summation = 0.0
//         summation = -y_representative * log(a_3) - (1 - y_representative) * log(1 - a_3)

//         J = sum(summation)
//         J = J / m

//         !do concurrent (i = 1:size(delta_3,1))
//         !    delta_3(i,:) = a_3(i,:) - y(1,:)
//         !end do

//         delta_3 = a_3 - y_representative

//         z_2_ones(:,1:1) = ones
//         z_2_ones(:,2:) = z_2

//         sigmoid_z_2 = 0.0

//         call sigmoidgradient(z_2_ones,sigmoid_z_2)

//         !temp_delta_2 = matmul(delta_3,Theta2)

// !timer1=secnds(0.0)
//         !a1 = size(delta_3,1)
//         !a2 = size(delta_3,2)
//         !b1 = size(Theta2,1)
//         !b2 = size(Theta2,2)
//         !call sGEMM('N','N',a1,b2,b1,1.0,delta_3,a1,Theta2,b1,0.0,temp_delta_2,a1)
//         call matrix_multiply(delta_3,0,Theta2,0,temp_delta_2)
// !timer2=secnds(timer1)
// !average1 = average1 + timer2
// !counter = counter + 1
// !print '(a, f8.4)','matrix_multiply timer: ', average1/counter

//         delta_2 =0.0
//         !delta_2 = temp_delta_2 * sigmoid_z_2   !elemental multiplication
//         call vsMul( size(temp_delta_2), temp_delta_2, sigmoid_z_2, delta_2 )   !elemental multiplication

//         gradient_2 = 0.0
//         gradient_1 = 0.0

//         call matrix_multiply(delta_3,1,a_2_ones,0,gradient_2)

//         call matrix_multiply(delta_2(:,2:),1,a_1,0,gradient_1)

//         gradient_2 = gradient_2 / m
//         gradient_1 = gradient_1 / m

//         gradient_2(:,2:) = gradient_2(:,2:) + Theta2(:,2:) * (lambda / m)
//         gradient_1(:,2:) = gradient_1(:,2:) + Theta1(:,2:) * (lambda / m)

//         Theta2_grad = reshape(gradient_2, (/size(gradient_2),1/))
//         Theta1_grad = reshape(gradient_1, (/size(gradient_1),1/))

//         gradient(1:size(Theta1_grad),1:1) = Theta1_grad
//         gradient((size(Theta1_grad)+1):,1:1) = Theta2_grad

//         costandgradient = J

//         !print '(f8.4, a, f8.4)',average1/counter,' ',average2/counter
//         !timer2=secnds(timer1)
//         !average = average*0.9 + timer2*0.1
//         !print '(a, f8.4)','costandgradient timer: ', timer2
//     end function costandgradient


#ifndef UTILITIES
#define UTILITIES

#include <math.h>
#include <stdlib.h>
#include <string>
#include <vector>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <map>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <complex>
#include "gsl/gsl_sf_gamma.h"
#include "gsl/gsl_sf_result.h"
#include "gsl/gsl_complex_math.h"

namespace utilities {
  using namespace boost::numeric::ublas;

  const double PI = 3.14159265359;
  const double C = 299792458;//m/s
  const double EMASSC2 = 510.9989461;//keV
  const double PMASSC2 = 938272.046;//keV
  const double NMASSC2 = 939565.4133;//keV
  const double ALPHAMASSC2 = 3727379.508;//keV
  const double FINESTRUCTURE = 0.0072973525664;
  const double E = 2.718281828459045;
  const double HBAR = 6.58211889e-16;//ev*s
  const double a_CORR = -1.0;

  const std::string atoms[] = {"H", "He", "Li", "Be", "B", "C", "N", "O", "F", "Ne", "Na", "Mg", "Al", "Si", "P", "S", "Cl", "Ar", "K", "Ca", "Sc", "Ti", "V", "Cr", "Mn", "Fe", "Co", "Ni", "Cu", "Zn", "Ga", "Ge", "As", "Se", "Br", "Kr", "Rb", "Sr", "Y", "Zr", "Nb", "Mo", "Tc", "Ru", "Rh", "Pd", "Ag", "Cd", "In", "Sn", "Sb", "Te", "I", "Xe", "Cs", "Ba", "La", "Ce", "Pr", "Nd", "Pm", "Sm", "Eu", "Gd", "Tb", "Dy", "Ho", "Er", "Tm", "Yb", "Lu", "Hf", "Ta", "W", "Re", "Os", "Ir", "Pt", "Au", "Hg", "Tl", "Pb", "Bi", "Po", "At", "Rn", "Fr", "Ra", "Ac", "Th", "Pa", "U", "Np", "Pu", "Am", "Cm", "Bk", "Cf", "Es", "Fm", "Md", "No", "Lr", "Rf", "Db", "Sg", "Bh", "Hs", "Mt"};

  inline vector<double> RandomDirection () {
    double z, phi;
    vector<double> v(3);

    z = rand() / (double)RAND_MAX * 2. - 1.;
    phi = rand() / (double)RAND_MAX * 2. * PI;

    v(0) = std::sqrt(1.-z*z)*std::cos(phi);
    v(1) = std::sqrt(1.-z*z)*std::sin(phi);
    v(2) = z;

    return v;
  };

  inline double Random(double begin, double end) { return rand() / (double)RAND_MAX * (end - begin) + begin; };

  inline double RandomFromDistribution(std::vector<std::vector<double> >& pd) {
    double begin = pd[0][0];
    double end = pd[pd.size()-1][0];
    double stepSize = pd[1][0] - pd[0][0];
    double max = 0.;

    for (std::vector<std::vector<double> >::size_type i = 0; i != pd.size(); i++) {
      if (pd[i][1] > max) {
        max = pd[i][1];
      }
    }

    double r = 0.;
    double q = 0.;
    do {
      r = rand() / (double)RAND_MAX;
      q = rand() / (double)RAND_MAX * max;
    } while (q > pd[r*pd.size()][1]);
    return r*(end - begin) + begin;
  }

  inline double GetSpeed(double energy, double massc2) {
    double gamma = energy/massc2 + 1.;
    return std::sqrt(1.-1./(gamma*gamma))*C;
  }

  inline double GetNorm(const vector<double>& v) {
    double sum = 0.;
    for (vector<double>::size_type i = 0; i != v.size(); i++) {
      sum+=v[i]*v[i];
    }
    return std::sqrt(sum);
  }

  inline double FourDimDot(vector<double>& a, vector<double>& b) {
    //Mostly minus convention
    return a[0]*b[0]-(a[1]*b[1]+a[2]*b[2]+a[3]*b[3]);
  }

  inline double CalculateXiBetaDecay(double& cs, double& ct, double& cv, double& ca, double& mf, double& mgt) {
    return mf*mf*(2.*cs*cs+2.*cv*cv)+mgt*mgt*(2.*ct*ct+2.*ca*ca);
  }

  inline double CalculateBetaNeutrinoAsymmetry(double cs, double ct, double cv, double ca, double mf, double mgt) {
    return (mf*mf*(-2.*cs*cs+2.*cv*cv)+mgt*mgt/3.*(2.*ct*ct-2.*ca*ca))/CalculateXiBetaDecay(cs, ct, cv, ca, mf, mgt);
  }

  inline vector<double> NormaliseVector(const vector<double>& v) {
    vector<double> newV (v);
    double norm = GetNorm(v);
    if (norm != 0) {
      return newV/norm;
    }
    return v;
  }

  inline double LambdaKinematic(double x, double y, double z) {
    return x*x+y*y+z*z-2*x*y-2*y*z-2*x*z;
  }

  inline double PhaseSpace(double W, double W0) {
    return std::sqrt(W*W-1.)*W*std::pow(W0-W, 2.);
  }

  inline double SimpleFermiFunction(double Z, double v) {
    double nu = Z*FINESTRUCTURE/v*C;
    return 2.*PI*nu/(1.-std::pow(E, -2.*PI*nu));
  }

  inline double FermiFunction(double Z, double W, double R) {
    double gamma = std::sqrt(1.-(FINESTRUCTURE*Z)*(FINESTRUCTURE*Z));
    double p = std::sqrt(W*W-1);
    double first = 2*(gamma+1);
    //the second term will be incorporated in the fifth
    //double second = 1/pow(gsl_sf_gamma(2*gamma+1),2);
    double third = std::pow(2*p*R,2*(gamma-1));
    double fourth = std::exp(M_PI*FINESTRUCTURE*Z*W/p);

    //the fifth is a bit tricky
    //we use the complex gamma function from GSL
    gsl_sf_result magn;
    gsl_sf_result phase;
    gsl_sf_lngamma_complex_e(gamma, FINESTRUCTURE*Z*W/p,&magn,&phase);
    //now we have what we want in magn.val

    //but we incorporate the second term here as well
    double fifth = std::exp(2*(magn.val-gsl_sf_lngamma(2*gamma+1)));

    return first*third*fourth*fifth;
  }

  inline double ApproximateRadius(double A) {
    return (1.15+1.8*std::pow(A, -2./3.)-1.2*std::pow(A, -4./3.))*EMASSC2*1000./HBAR/C*std::pow(A, 1./3.);
  }

  inline double GetSpectrumHeight(double Z, double A, double Q, double E, bool advancedFermi) {
    double W = E/EMASSC2+1.;
    double W0 = Q/EMASSC2+1.;
    double R = ApproximateRadius(A);
    if (advancedFermi) {
      return PhaseSpace(W, W0)*FermiFunction(Z, W, R);
    }
    else {
      return PhaseSpace(W, W0)*SimpleFermiFunction(Z, GetSpeed(E, EMASSC2));
    }
  }

  inline std::vector<std::vector<double> >* GenerateBetaSpectrum(double Z, double A, double Q, bool advancedFermi) {
    std::vector<std::vector<double> >* dist = new std::vector<std::vector<double> >();
    double stepSize = 1.0;

    double currentEnergy = stepSize;
    while(currentEnergy <= Q) {
      double s = GetSpectrumHeight(Z, A, Q, currentEnergy, advancedFermi);
      std::vector<double> pair;
      pair.push_back(currentEnergy);
      pair.push_back(s);
      dist->push_back(pair);
      currentEnergy+=stepSize;
    }
    return dist;
  }

  inline vector<double> CrossProduct(vector<double> first, vector<double> second) {
    vector<double> v (3);

    v(0) = first(1)*second(2) - first(2)*second(1);
    v(1) = first(2)*second(0) - first(0)*second(2);
    v(2) = first(0)*second(1) - first(1)*second(0);

    return v;
  }

  inline vector<double> RotateAroundVector(vector<double>& initial, vector<double>& axis, double angle) {
    vector<double> vect(3);
    double u = axis[0];
    double v = axis[1];
    double w = axis[2];
    double x = initial[0];
    double y = initial[1];
    double z = initial[2];

    double c = std::cos(angle);
    double s = std::sin(angle);

    vect(0) = u*(u*x+v*y+w*z)*(1.-c)+x*c+(-w*y+v*z)*s;
    vect(1) = v*(u*x+v*y+w*z)*(1.-c)+y*c+(w*x-u*z)*s;
    vect(2) = w*(u*x+v*y+w*z)*(1.-c)+z*c+(-v*x+u*y)*s;

    return vect;
  }

  inline vector<double> GetParticleDirection(vector<double>& dir2, std::vector<double>& A) {
    dir2 = NormaliseVector(dir2);

    std::vector<std::vector<double> > dist;
    double stepSize = PI/180;
    double currentAngle = 0.;

    while (currentAngle <= PI) {
      std::vector<double> pair;
      pair.push_back(currentAngle);
      double W = 0.;
      for (std::vector<double>::size_type i = 0; i != A.size(); i++) {
        W+=A[i]*boost::math::legendre_p(i, std::cos(currentAngle));
      }
      pair.push_back(W);
      dist.push_back(pair);
      currentAngle+=stepSize;
    }

    double theta = 1.-RandomFromDistribution(dist)/PI;
    theta = std::acos(2*theta-1.);
    //std::cout << theta;
    vector<double> perp = CrossProduct(dir2, RandomDirection());
    vector<double> dir = RotateAroundVector(dir2, perp, theta);
    return dir;
  }

  inline vector<double> GetParticleDirection(std::vector<vector<double> >& dirs, std::vector<std::vector<double> >& A) {
    vector<double> dir (3);
    //TODO
    return dir;
  }

  inline vector<double> LorentzBoost(vector<double>& velocity, vector<double>& v) {
    double speed = GetNorm(velocity);
    double beta = speed;
    double gamma = 1./std::sqrt(1.-std::pow(speed, 2.));

    vector<double> dir = NormaliseVector(velocity);

    symmetric_matrix<double, upper> boost(4,4);
    boost(0, 0) = gamma;
    boost(0, 1) = -gamma*beta*dir[0];
    boost(0, 2) = -gamma*beta*dir[1];
    boost(0, 3) = -gamma*beta*dir[2];
    boost(1, 1) = 1.+(gamma-1.)*dir[0]*dir[0];
    boost(1, 2) = (gamma-1.)*dir[0]*dir[1];
    boost(1, 3) = (gamma-1.)*dir[0]*dir[2];
    boost(2, 2) = 1.+(gamma-1.)*dir[1]*dir[1];
    boost(2, 3) = (gamma-1.)*dir[1]*dir[2];
    boost(3, 3) = 1.+(gamma-1.)*dir[2]*dir[2];

    return prod(boost, v);
  }

  inline double GetApproximateMass(int Z, int A) {
    double b = 15.5*A-16.8*std::pow(A, 2.0/3.0)- 0.72*Z*(Z-1)*std::pow(A, -1.0/3.0) - 23.*std::pow(A-2*Z, 2.)/A;
    if (A%2 == 0) {
      double asym = 34.*std::pow(A, -3.0/4.0);
      if (Z%2 == 0) {
        b+=asym;
      }
      else {
        b-=asym;
      }
    }
    return Z*PMASSC2+(A-Z)*NMASSC2-b;
  }

  inline std::vector<std::vector<double> > ReadDistribution(const char * filename) {
    std::ifstream distReader(filename, std::ifstream::in);

    if (!distReader.is_open()) {
      std::cout << "Error: Could not find file " << filename << std::endl;
    }
    std::vector<std::vector<double> > dist;

    double x, y;
    while (distReader >> x >> y) {
      std::vector<double> pair;
      pair.push_back(x);
      pair.push_back(y);
      dist.push_back(pair);
    }
   return dist;
  }
}
#endif


// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "event.h"

#include <algorithm>
#include <cmath>
#include <complex>

#include <boost/program_options/variables_map.hpp>

#include "nucleus.h"

namespace trento {

namespace {

typedef std::complex<double> complex_t;

constexpr double TINY = 1e-12;

// Generalized mean for p > 0.
// M_p(a, b) = (1/2*(a^p + b^p))^(1/p)
inline double positive_pmean(double p, double a, double b) {
  return std::pow(.5*(std::pow(a, p) + std::pow(b, p)), 1./p);
}

// Generalized mean for p < 0.
// Same as the positive version, except prevents division by zero.
inline double negative_pmean(double p, double a, double b) {
  if (a < TINY || b < TINY)
    return 0.;
  return positive_pmean(p, a, b);
}

// Generalized mean for p == 0.
inline double geometric_mean(double a, double b) {
  return std::sqrt(a*b);
}

inline int get_max_eccentricity_m(const std::vector<EventQuantity>& quantities) {
  int max_m = -1;
  bool foundmn = false;

  for (auto qty : quantities) {
    if (EventQuantity_GetClass(qty) == EventEpsilon_mn ||
        EventQuantity_GetClass(qty) == EventEpsilonArg_mn)
    {
      int m = EventQuantity_GetSubscript1(qty);
      int n = EventQuantity_GetSubscript2(qty);

      max_m = std::max(max_m, m);
      foundmn = (foundmn || (m != n));
    }
  }

  return (foundmn ? max_m : -1);
}

inline int get_max_eccentricity_n(const std::vector<EventQuantity>& quantities) {
  int max_n = -1;

  for (auto qty : quantities) {
    if (EventQuantity_GetClass(qty) == EventEpsilon_mn ||
        EventQuantity_GetClass(qty) == EventEpsilonArg_mn)
    {
      int n = EventQuantity_GetSubscript2(qty);
      max_n = std::max(max_n, n);
    }
  }

  return max_n;
}

}  // unnamed namespace

Event::Event(const VarMap& var_map)                                    // this constructor assumes that only quantities being displayed need to
  : Event(var_map, var_map["columns"].as<EventQuantityList>().values)  //   be computed; retained for code compatibility but should not be used
{ }

// Determine the grid parameters like so:
//   1. Read and set step size from the configuration.
//   2. Read grid max from the config, then set the number of steps as
//      nsteps = ceil(2*max/step).
//   3. Set the actual grid max as max = nsteps*step/2.  Hence if the step size
//      does not evenly divide the config max, the actual max will be marginally
//      larger (by at most one step size).
Event::Event(
  const VarMap& var_map,
  const std::vector<EventQuantity>& required_quantities)
    : norm_(var_map["normalization"].as<double>()),
      dxy_(var_map["grid-step"].as<double>()),
      nsteps_(std::ceil(2.*var_map["grid-max"].as<double>()/dxy_)),
      xymax_(.5*nsteps_*dxy_),
      TA_(boost::extents[nsteps_][nsteps_]),
      TB_(boost::extents[nsteps_][nsteps_]),
      TR_(boost::extents[nsteps_][nsteps_]),
      max_eccentricity_m_(get_max_eccentricity_m(required_quantities)),
      max_eccentricity_n_(get_max_eccentricity_n(required_quantities)) {
  // Choose which version of the generalized mean to use based on the
  // configuration.  The possibilities are defined above.  See the header for
  // more information.
  auto p = var_map["reduced-thickness"].as<double>();

  if (std::fabs(p) < TINY) {
    compute_reduced_thickness_ = [this]() {
      compute_reduced_thickness(geometric_mean);
    };
  } else if (p > 0.) {
    compute_reduced_thickness_ = [this, p]() {
      compute_reduced_thickness(
        [p](double a, double b) { return positive_pmean(p, a, b); });
    };
  } else {
    compute_reduced_thickness_ = [this, p]() {
      compute_reduced_thickness(
        [p](double a, double b) { return negative_pmean(p, a, b); });
    };
  }
}

void Event::compute(const Nucleus& nucleusA, const Nucleus& nucleusB,
                    const NucleonCommon& nucleon_common) {
  // Reset npart; compute_nuclear_thickness() increments it.
  npart_ = 0;
  compute_nuclear_thickness(nucleusA, nucleon_common, TA_);
  compute_nuclear_thickness(nucleusB, nucleon_common, TB_);
  compute_reduced_thickness_();
  compute_observables();

  failuresA_ = nucleusA.failures();
  failuresB_ = nucleusB.failures();
}

namespace {

// Limit a value to a range.
// Used below to constrain grid indices.
template <typename T>
inline const T& clip(const T& value, const T& min, const T& max) {
  if (value < min)
    return min;
  if (value > max)
    return max;
  return value;
}

}  // unnamed namespace

void Event::compute_nuclear_thickness(
    const Nucleus& nucleus, const NucleonCommon& nucleon_common, Grid& TX) {
  // Construct the thickness grid by looping over participants and adding each
  // to a small subgrid within its radius.  Compared to the other possibility
  // (grid cells as the outer loop and participants as the inner loop), this
  // reduces the number of required distance-squared calculations by a factor of
  // ~20 (depending on the nucleon size).  The Event unit test verifies that the
  // two methods agree.

  // Wipe grid with zeros.
  std::fill(TX.origin(), TX.origin() + TX.num_elements(), 0.);

  // Deposit each participant onto the grid.
  for (const auto& nucleon : nucleus) {
    if (!nucleon.is_participant())
      continue;

    ++npart_;

    // Get nucleon subgrid boundary {xmin, xmax, ymin, ymax}.
    const auto boundary = nucleon_common.boundary(nucleon);

    // Determine min & max indices of nucleon subgrid.
    int ixmin = clip(static_cast<int>((boundary[0]+xymax_)/dxy_), 0, nsteps_-1);
    int ixmax = clip(static_cast<int>((boundary[1]+xymax_)/dxy_), 0, nsteps_-1);
    int iymin = clip(static_cast<int>((boundary[2]+xymax_)/dxy_), 0, nsteps_-1);
    int iymax = clip(static_cast<int>((boundary[3]+xymax_)/dxy_), 0, nsteps_-1);

    // Add profile to grid.
    for (auto iy = iymin; iy <= iymax; ++iy) {
      for (auto ix = ixmin; ix <= ixmax; ++ix) {
        TX[iy][ix] += nucleon_common.thickness(
          nucleon, (ix+.5)*dxy_ - xymax_, (iy+.5)*dxy_ - xymax_
        );
      }
    }
  }
}

template <typename GenMean>
void Event::compute_reduced_thickness(GenMean gen_mean) {
  double sum = 0.;
  double ixcm = 0.;
  double iycm = 0.;

  for (int iy = 0; iy < nsteps_; ++iy) {
    for (int ix = 0; ix < nsteps_; ++ix) {
      auto t = norm_ * gen_mean(TA_[iy][ix], TB_[iy][ix]);
      TR_[iy][ix] = t;
      sum += t;
      // Center of mass grid indices.
      // No need to multiply by dxy since it would be canceled later.
      ixcm += t * static_cast<double>(ix);
      iycm += t * static_cast<double>(iy);
    }
  }

  multiplicity_ = dxy_ * dxy_ * sum;
  ixcm_ = ixcm / sum;
  iycm_ = iycm / sum;
}

void Event::compute_en(int max_n) {
  // Compute eccentricity.
  if (max_n < MinEccentricityHarmonic || max_n > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_n"};

  // Simple helper class for use in the following loop.
  struct EccentricityAccumulator {
    complex_t z = 0.;
    double wt = 0.;  // weight
    double finish() const  // compute final eccentricity
    { return std::abs(z) / std::fmax(wt, TINY); }
    double arg() const
    { return std::arg(z); }
  } en[NumEccentricityHarmonics];

  for (int iy = 0; iy < nsteps_; ++iy) {
    auto y = static_cast<double>(iy) - iycm_;
    auto y2 = y*y;

    for (int ix = 0; ix < nsteps_; ++ix) {
      const auto& t = TR_[iy][ix];
      if (t < TINY)
        continue;

      // Compute `r` relative to the CM.
      auto x = static_cast<double>(ix) - ixcm_;
      auto r = std::sqrt(x*x + y2);

      // The eccentricity harmonics are weighted averages of r^n*exp(i*n*phi)
      // over the entropy profile (reduced thickness).  Note that:
      //
      //   r^n * exp(i*n*phi)  =  r^n * exp(i*phi)^n
      //                       =  (r*exp(i*phi))^n
      //                       =  (x + i*y)^n
      //
      // The Event unit test verifies that this method agrees with the
      // trigonometric function method.
      auto z    = complex_t{x, y};
      auto enz  = t * z;
      auto enwt = t * r;

      for (int i = MinEccentricityHarmonic; i <= max_n; ++i) {
        en[i - MinEccentricityHarmonic].z  += (enz  *= z);
        en[i - MinEccentricityHarmonic].wt += (enwt *= r);
      }
    }
  }

  for (int n = MinEccentricityHarmonic; n <= max_n; ++n) {
    eccentricity_[n] = eccentricity_mn_[std::make_pair(n,n)] = en[n - MinEccentricityHarmonic].finish();
    eccentricity_mn_arg_[std::make_pair(n,n)] = en[n - MinEccentricityHarmonic].arg();
  }
}

void Event::compute_emn(int max_m, int max_n) {
  // Compute eccentricity.
  if (max_m < MinEccentricityHarmonic || max_m > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_m"};

  if (max_n < MinEccentricityHarmonic || max_n > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_n"};

  // Simple helper class for use in the following loop.
  complex_t emn[NumEccentricityHarmonics][NumEccentricityHarmonics] = { };  // important: initializes to zeroes
  double weights[NumEccentricityHarmonics] = { };

  for (int iy = 0; iy < nsteps_; ++iy) {
    auto y = static_cast<double>(iy) - iycm_;
    auto y2 = y*y;

    for (int ix = 0; ix < nsteps_; ++ix) {
      const auto& t = TR_[iy][ix];
      if (t < TINY)
        continue;

      // Compute `r` relative to the CM.
      auto x = static_cast<double>(ix) - ixcm_;
      auto r = std::sqrt(x*x + y2);

      auto wt = t * r;  // initialize to `(t * r)` and multiply `r` again at top of `m` loop, since `m` starts at 2
      for (int m = MinEccentricityHarmonic; m <= max_m; ++m)       // (note: `MinEccentricityHarmonic = 2` assumed)
        weights[m - MinEccentricityHarmonic] += (wt *= r);

      // The eccentricity harmonics are weighted averages of `r^m*exp(i*n*phi)`
      // over the entropy profile (reduced thickness).  Note that:
      //
      //   r^m * exp(i*n*phi)  =  r^m * exp(i*phi)^n
      //                       =  r^(m-n) * (r*exp(i*phi))^n
      //                       =  r^(m-n) * (x + i*y)^n
      //
      auto z = complex_t{x, y};
      auto zfactor = t * z;  // initialize to `(t * z)` and multiply `z` again at top of `n` loop, since `n` starts at 2

      for (int n = MinEccentricityHarmonic; n <= MaxEccentricityHarmonic; n++) {  // (note: `MinEccentricityHarmonic = 2` assumed)
        zfactor *= z;

        double rfactor;

        if (r > TINY) {  // ignore this pixel if `r ~ 0` here; it would be nugatory in an integral but causes problems for our sum
          rfactor = 1.;  // initialize to 1 and divide `r` at top of `m` loop, since `(m - n)` starts at -1 and counts down

          for (int m = n - 1; m >= MinEccentricityHarmonic; --m) {
            rfactor /= r;
            emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic] += rfactor * zfactor;
          }

          rfactor = 1.;  // now initialize to 1 and multiply `r` at bottom of `m` loop, since `(m - n)` now starts at 0 and counts up

          for (int m = n; m <= MaxEccentricityHarmonic; ++m) {
            emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic] += rfactor * zfactor;
            rfactor *= r;
          }
        }
      }
    }
  }

  for (int m = MinEccentricityHarmonic; m <= max_m; ++m) {
    double wt = std::fmax(weights[m - MinEccentricityHarmonic], TINY);

    for (int n = MinEccentricityHarmonic; n <= max_n; ++n) {
      eccentricity_mn_[std::make_pair(m,n)]     = std::abs(emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic]) / wt;
      eccentricity_mn_arg_[std::make_pair(m,n)] = std::arg(emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic]);
    }

    if (m <= max_n)  // also update `eccentricity_` for `m == n` if applicable
      eccentricity_[m] = std::abs(emn[m - MinEccentricityHarmonic][m - MinEccentricityHarmonic]) / wt;
  }
}

void Event::compute_observables() {
  if (max_eccentricity_n_ >= 0) {
    if (max_eccentricity_m_ >= 0)
      compute_emn(max_eccentricity_m_, max_eccentricity_n_);
    else compute_en(max_eccentricity_n_);
  }
}

}  // namespace trento


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

#include "genotyping/AlleleChecker.hh"

#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/log1p.hpp>

#include <numeric>

namespace ehunter
{

namespace
{
double poissonLogPmf(double lambda, double count)
{
    return count * log(lambda) - lambda - boost::math::lgamma(count + 1);
}

double logBeta(int a, int b) { return boost::math::lgamma(a) + boost::math::lgamma(b) - boost::math::lgamma(a + b); }

double logBinomCoef(int n, int k) { return -boost::math::log1p(n) - logBeta(n - k + 1, k + 1); }

double binomLogPmf(int n, double p, int count)
{
    return logBinomCoef(n, count) + count * log(p) + (n - count) * boost::math::log1p(-p);
}

}

AlleleCheckSummary AlleleChecker::check(double haplotypeDepth, int targetAlleleCount, int otherAlleleCount) const
{
    if (haplotypeDepth <= 0)
    {
        throw std::runtime_error("Haplotype depth must be positive");
    }

    if (targetAlleleCount < 0 || otherAlleleCount < 0)
    {
        throw std::runtime_error("Negative read counts are not allowed");
    }

    const int totalReadCount = targetAlleleCount + otherAlleleCount;
    const double ll0 = (totalReadCount > 0) ? binomLogPmf(totalReadCount, errorRate_, targetAlleleCount) : 0;
    const double ll1 = poissonLogPmf(haplotypeDepth, targetAlleleCount);

    AlleleStatus status = AlleleStatus::kUncertain;
    double logLikelihoodRatio = (ll1 - ll0) / log(10);
    if (logLikelihoodRatio < -log10(likelihoodRatioThreshold_))
    {
        status = AlleleStatus::kAbsent;
    }
    else if (logLikelihoodRatio > log10(likelihoodRatioThreshold_))
    {
        status = AlleleStatus::kPresent;
    }

    return AlleleCheckSummary(status, logLikelihoodRatio);
}

std::ostream& operator<<(std::ostream& out, AlleleStatus status)
{
    switch (status)
    {
    case AlleleStatus::kAbsent:
        out << "Absent";
        break;
    case AlleleStatus::kPresent:
        out << "Present";
        break;
    case AlleleStatus::kUncertain:
        out << "Uncertain";
        break;
    }

    return out;
}

}


#pragma once

#include <deal.II/base/tensor.h>
#include <functional>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <vector>

#include "aux/filtered_range.hpp"
#include "aux/hash_specializations.hpp"
#include "aux/simple_sparse_matrix.hpp"
#include "matrix/assembly/velocity_angular_integrator.hpp"
#include "matrix/assembly/velocity_radial_integrator.hpp"
#include "matrix/assembly/weight.hpp"
#include "spectral/basis/spectral_basis_dimension_accessor.hpp"

namespace boltzmann {
namespace local_ {
/**
 * @brief Compute and cache overlap integrals in radial direction
 */
struct radial_entry_computer_t
{
  // -----------------------------------------------------------------
  radial_entry_computer_t(int nqpts_ = 90)
      : nqpts(nqpts_)
  {
    /* empty */
  }

  /**
   *  @brief I = \f$\int_\bbR b_1(r) k(r), b_2(r) r \ud r \f$
   *
   *  @tparam E spectral element
   *  @param k additional function argument
   *  @param b1 1st basis function
   *  @param b2 2nd basis function
   *  @return I integral
   */
  template <typename E>
  double compute(const std::function<double(double)>& k,
                 const E& b1,
                 const E& b2,
                 const double a = 0 /* inner product weight exp(-a r**2) */)
  {
    const double FUZZY = 1e7;
    const double nu = a + b1.w() + b2.w();
    const long int key = FUZZY * nu;
    if (quad_cache.find(key) == quad_cache.end()) quad_cache[key] = ptr_t(new QMaxwell(nu, nqpts));
    const auto& quad = *(quad_cache[key]);

    double sum = 0;
    for (unsigned int q = 0; q < quad.size(); ++q) {
      sum += b1.evaluate(quad.pts(q)) * b2.evaluate(quad.pts(q)) * k(quad.pts(q)) * quad.wts(q);
    }

    return sum;
  }

 private:
  const int nqpts;
  typedef std::shared_ptr<QMaxwell> ptr_t;
  std::map<long int, ptr_t> quad_cache;
};
}  // end namespace local_

template <int DIM>
class VelocityVarForm
{ };

/**
 * @brief Storage for velocity domain matrix entries
 *
 */
template <>
class VelocityVarForm<2>
{
 private:
  static const int DIM = 2;
  typedef unsigned int index_t;
  typedef dealii::Tensor<1, DIM, double> t1_t;
  typedef dealii::Tensor<2, DIM, double> t2_t;

  typedef SimpleSparseMatrix<double> s0_entries_t;
  typedef SimpleSparseMatrix<t1_t> t1_entries_t;
  typedef SimpleSparseMatrix<t2_t> t2_entries_t;

 public:
  template <typename BASIS>
  void init(const BASIS& test_basis, const BASIS& trial_basis, const double beta = 2);

  template <typename BASIS>
  void init(const BASIS& trial_basis, const double beta = 2);

 private:
  s0_entries_t s0_;
  t1_entries_t t1_;
  t2_entries_t t2_;

  /// helpers
  VelocityAngularIntegrator<2> vai;

  constexpr const static double TOL = 1e-11; // magic number

 public:
  auto get_s0() const -> decltype(s0_.get_vec()) { return s0_.get_vec(); }
  auto get_s1() const -> decltype(t1_.get_vec()) { return t1_.get_vec(); }
  auto get_t1() const -> decltype(t1_.get_vec()) { return t1_.get_vec(); }
  auto get_t2() const -> decltype(t2_.get_vec()) { return t2_.get_vec(); }

  const SimpleSparseMatrix<double>& get_s0m() const { return s0_; }
  const SimpleSparseMatrix<t1_t>& get_s1m()   const { return t1_; }
  const SimpleSparseMatrix<t1_t>& get_t1m()   const { return t1_; }
  const SimpleSparseMatrix<t2_t>& get_t2m()   const { return t2_; }

  /**
   *  @brief \f$\langle \psi_1, \psi_2\rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.

   */
  auto s0() const -> const decltype(s0_) & { return s0_; }

  /**
   *  @brief \f$\langle v_i \psi_1, \psi_2\rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.
   */
  auto s1() const -> const decltype(t1_) & { return t1_; }

  /**
   *   @brief same as s1
   */
  auto t1() const -> const decltype(t1_) & { return t1_; }

  /**
   *   @brief \f$\langle v_i v_j \psi_1, \psi_2 \rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.
   */
  auto t2() const -> const decltype(t2_) & { return t2_; }

  const VelocityAngularIntegrator<2>& get_vai() const { return vai; }

  void print_info() const;
};

// --------------------------------------------------------------------------------
template <typename BASIS>
void
VelocityVarForm<2>::init(const BASIS& test_basis, const BASIS& trial_basis, const double beta)
{
  typedef typename BASIS::elem_t elem_t;

  // angular basis
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type angular_elem_t;
  // radial basis
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type radial_elem_t;
  /// vector of basis functions in dim1
  typedef typename BASIS::DimAcc::template get_vec<angular_elem_t> a1_t;
  typedef typename BASIS::DimAcc::template get_vec<radial_elem_t> a2_t;

  typedef typename radial_elem_t::id_t rid_t;
  typedef typename radial_elem_t::numeric_t numeric_t;

  // element accessors for radial and angular parts
  typename elem_t::Acc::template get<radial_elem_t> acc_rad;
  typename elem_t::Acc::template get<angular_elem_t> acc_ang;

  /// they correspond to rows and columns in R1
  const auto& test_angular_basis = a1_t()(test_basis);
  //  const auto& test_radial_basis  = a2_t()(test_basis);

  const auto& trial_angular_basis = a1_t()(trial_basis);
  // const auto& trial_radial_basis  = a2_t()(trial_basis);

  // matrix assembly
  vai.init(trial_angular_basis);
  // regular L2-inner product
  L2Weight weight(beta);

  // ------------------------------
  // Compute Radial Entries
  local_::radial_entry_computer_t radial_entry_computer;
  // create a cache for the radial entries
  std::unordered_map<std::tuple<rid_t, rid_t>, numeric_t> cache;

  typedef std::pair<unsigned int, unsigned int> index_pair_t;

  // ----------------------------------------------------------------------
  // S0
  s0_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k0 = [](double r __attribute__((unused))) { return 1; };
  // iterate over nonzero entries in s0
  const auto& s0A = vai.get_s0();
  for (auto itA = s0A.begin(); itA != s0A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };

    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k0, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          s0_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  s0_.compress();

  // ----------------------------------------------------------------------
  // T1
  t1_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k1 = [](double r) { return r; };
  // iterate over nonzero entries in t1
  const auto& t1A = vai.get_t1();
  for (auto itA = t1A.begin(); itA != t1A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };
    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k1, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          t1_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  t1_.compress();

  // ----------------------------------------------------------------------
  // T2
  t2_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k2 = [](double r) { return r * r; };
  const auto& t2A = vai.get_t2();
  for (auto itA = t2A.begin(); itA != t2A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };
    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k2, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          t2_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  t2_.compress();
}

// ------------------------------------------------------------
template <typename BASIS>
void
VelocityVarForm<2>::init(const BASIS& trial_basis, const double beta)
{
  this->init(trial_basis, trial_basis, beta);
}

// -------------------------------------------------------------
inline void
VelocityVarForm<2>::print_info() const
{
  std::cout << "s0_.size()"
            << "\t" << s0_.size() << std::endl
            << "t1_.size()"
            << "\t" << t1_.size() << std::endl
            << "t2_.size()"
            << "\t" << t2_.size() << std::endl;
}

}  // end namespace boltzmann


//  Boost common_factor.hpp header file  -------------------------------------//

//  (C) Copyright Daryle Walker, Stephen Cleary, Paul Moore 2001.  Permission
//  to copy, use, modify, sell and distribute this software is granted provided
//  this copyright notice appears in all copies.  This software is provided "as
//  is" without express or implied warranty, and with no claim as to its
//  suitability for any purpose.

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_HPP
#define BOOST_MATH_COMMON_FACTOR_HPP

#include <boost/math_fwd.hpp>  // self include

#include <boost/config.hpp>  // for BOOST_STATIC_CONSTANT, etc.
#include <boost/limits.hpp>  // for std::numeric_limits

namespace boost
{
namespace math
{

//  Forward declarations for function templates  -----------------------------//

template < typename IntegerType >
    IntegerType  gcd( IntegerType const &a, IntegerType const &b );

template < typename IntegerType >
    IntegerType  lcm( IntegerType const &a, IntegerType const &b );

//  Greatest common divisor evaluator class declaration  ---------------------//

template < typename IntegerType >
class gcd_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::gcd_evaluator

//  Least common multiple evaluator class declaration  -----------------------//

template < typename IntegerType >
class lcm_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::lcm_evaluator

//  Implementation details  --------------------------------------------------//

namespace detail
{
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build GCD with Euclid's recursive algorithm
    template < unsigned long Value1, unsigned long Value2 >
    struct static_gcd_helper_t
    {
    private:
        BOOST_STATIC_CONSTANT( unsigned long, new_value1 = Value2 );
        BOOST_STATIC_CONSTANT( unsigned long, new_value2 = Value1 % Value2 );

        #ifndef __BORLANDC__
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  Value
        #else
        typedef static_gcd_helper_t  self_type;
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  (self_type:: Value )
        #endif

        typedef static_gcd_helper_t< BOOST_DETAIL_GCD_HELPER_VAL(new_value1),
         BOOST_DETAIL_GCD_HELPER_VAL(new_value2) >  next_step_type;

        #undef BOOST_DETAIL_GCD_HELPER_VAL

    public:
        BOOST_STATIC_CONSTANT( unsigned long, value = next_step_type::value );
    };

    // Non-recursive case
    template < unsigned long Value1 >
    struct static_gcd_helper_t< Value1, 0UL >
    {
        BOOST_STATIC_CONSTANT( unsigned long, value = Value1 );
    };
#else
    // Use inner class template workaround from Peter Dimov
    template < unsigned long Value1 >
    struct static_gcd_helper2_t
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value
             = static_gcd_helper2_t<Value2>::helper<Value1 % Value2>::value );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = Value1 );
        };
    };

    // Special case
    template <  >
    struct static_gcd_helper2_t< 0UL >
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = Value2 );
        };
    };

    // Build the GCD from the above template(s)
    template < unsigned long Value1, unsigned long Value2 >
    struct static_gcd_helper_t
    {
        BOOST_STATIC_CONSTANT( unsigned long, value
         = static_gcd_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build the LCM from the GCD
    template < unsigned long Value1, unsigned long Value2 >
    struct static_lcm_helper_t
    {
        typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

        BOOST_STATIC_CONSTANT( unsigned long, value = Value1 / gcd_type::value
         * Value2 );
    };

    // Special case for zero-GCD values
    template < >
    struct static_lcm_helper_t< 0UL, 0UL >
    {
        BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
    };
#else
    // Adapt GCD's inner class template workaround for LCM
    template < unsigned long Value1 >
    struct static_lcm_helper2_t
    {
        template < unsigned long Value2 >
        struct helper
        {
            typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

            BOOST_STATIC_CONSTANT( unsigned long, value = Value1
             / gcd_type::value * Value2 );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
        };
    };

    // Special case
    template <  >
    struct static_lcm_helper2_t< 0UL >
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
        };
    };

    // Build the LCM from the above template(s)
    template < unsigned long Value1, unsigned long Value2 >
    struct static_lcm_helper_t
    {
        BOOST_STATIC_CONSTANT( unsigned long, value
         = static_lcm_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

    // Greatest common divisor for rings (including unsigned integers)
    template < typename RingType >
    RingType
    gcd_euclidean
    (
        RingType  a,
        RingType  b
    )
    {
        // Avoid repeated construction
        #ifndef __BORLANDC__
        RingType const  zero = static_cast<RingType>( 0 );
        #else
        RingType  zero = static_cast<RingType>( 0 );
        #endif

        // Reduce by GCD-remainder property [GCD(a,b) == GCD(b,a MOD b)]
        while ( true )
        {
            if ( a == zero )
                return b;
            b %= a;

            if ( b == zero )
                return a;
            a %= b;
        }
    }

    // Greatest common divisor for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    gcd_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = gcd_euclidean( a, b );

        return ( result < zero ) ? -result : result;
    }

    // Least common multiple for rings (including unsigned integers)
    template < typename RingType >
    inline
    RingType
    lcm_euclidean
    (
        RingType const &  a,
        RingType const &  b
    )
    {
        RingType const  zero = static_cast<RingType>( 0 );
        RingType const  temp = gcd_euclidean( a, b );

        return ( temp != zero ) ? ( a / temp * b ) : zero;
    }

    // Least common multiple for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    lcm_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = lcm_euclidean( a, b );

        return ( result < zero ) ? -result : result;
    }

    // Function objects to find the best way of computing GCD or LCM
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_euclidean( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#else
    template < bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper2_t
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return gcd_euclidean( a, b );
            }
        };
    };

    template < >
    struct gcd_optimal_evaluator_helper2_t< true, true >
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return gcd_integer( a, b );
            }
        };
    };

    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
        : gcd_optimal_evaluator_helper2_t<IsSpecialized, IsSigned>
           ::BOOST_NESTED_TEMPLATE helper<T>
    {
    };
#endif

    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef gcd_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#endif

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_euclidean( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#else
    template < bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper2_t
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return lcm_euclidean( a, b );
            }
        };
    };

    template < >
    struct lcm_optimal_evaluator_helper2_t< true, true >
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return lcm_integer( a, b );
            }
        };
    };

    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
        : lcm_optimal_evaluator_helper2_t<IsSpecialized, IsSigned>
           ::BOOST_NESTED_TEMPLATE helper<T>
    {
    };
#endif

    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef lcm_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#endif

    // Functions to find the GCD or LCM in the best way
    template < typename T >
    inline
    T
    gcd_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        gcd_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

    template < typename T >
    inline
    T
    lcm_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        lcm_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

}  // namespace detail

//  Compile-time greatest common divisor evaluator class declaration  --------//

template < unsigned long Value1, unsigned long Value2 >
struct static_gcd
{
    BOOST_STATIC_CONSTANT( unsigned long, value
     = (detail::static_gcd_helper_t<Value1, Value2>::value) );

};  // boost::math::static_gcd

//  Compile-time least common multiple evaluator class declaration  ----------//

template < unsigned long Value1, unsigned long Value2 >
struct static_lcm
{
    BOOST_STATIC_CONSTANT( unsigned long, value
     = (detail::static_lcm_helper_t<Value1, Value2>::value) );

};  // boost::math::static_lcm

//  Greatest common divisor evaluator member function definition  ------------//

template < typename IntegerType >
inline
typename gcd_evaluator<IntegerType>::result_type
gcd_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::gcd_optimal( a, b );
}

//  Least common multiple evaluator member function definition  --------------//

template < typename IntegerType >
inline
typename lcm_evaluator<IntegerType>::result_type
lcm_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::lcm_optimal( a, b );
}

//  Greatest common divisor and least common multiple function definitions  --//

template < typename IntegerType >
inline
IntegerType
gcd
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    gcd_evaluator<IntegerType>  solver;

    return solver( a, b );
}

template < typename IntegerType >
inline
IntegerType
lcm
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    lcm_evaluator<IntegerType>  solver;

    return solver( a, b );
}

}  // namespace math
}  // namespace boost

#endif  // BOOST_MATH_COMMON_FACTOR_HPP


#ifndef INCLUDED_scheme_numeric_euler_angles_HH
#define INCLUDED_scheme_numeric_euler_angles_HH

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

namespace scheme { namespace numeric {

	template<class M> typename M::Scalar const & xx( M const & m ) { return m(0,0); }
	template<class M> typename M::Scalar const & xy( M const & m ) { return m(0,1); }
	template<class M> typename M::Scalar const & xz( M const & m ) { return m(0,2); }
	template<class M> typename M::Scalar const & yx( M const & m ) { return m(1,0); }
	template<class M> typename M::Scalar const & yy( M const & m ) { return m(1,1); }
	template<class M> typename M::Scalar const & yz( M const & m ) { return m(1,2); }
	template<class M> typename M::Scalar const & zx( M const & m ) { return m(2,0); }
	template<class M> typename M::Scalar const & zy( M const & m ) { return m(2,1); }
	template<class M> typename M::Scalar const & zz( M const & m ) { return m(2,2); }
	template<class M> typename M::Scalar       & xx( M       & m ) { return m(0,0); }
	template<class M> typename M::Scalar       & xy( M       & m ) { return m(0,1); }
	template<class M> typename M::Scalar       & xz( M       & m ) { return m(0,2); }
	template<class M> typename M::Scalar       & yx( M       & m ) { return m(1,0); }
	template<class M> typename M::Scalar       & yy( M       & m ) { return m(1,1); }
	template<class M> typename M::Scalar       & yz( M       & m ) { return m(1,2); }
	template<class M> typename M::Scalar       & zx( M       & m ) { return m(2,0); }
	template<class M> typename M::Scalar       & zy( M       & m ) { return m(2,1); }
	template<class M> typename M::Scalar       & zz( M       & m ) { return m(2,2); }

	template<class M> struct get_scalar { typedef typename M::Scalar type; };

	template<class T> T sin_cos_range(T const & t) { return t; }

	/// @brief COPIED FROM ROSETTA
	/// Return the three euler angles (in radians) that describe this HomogeneousTransform as the series
	/// of a Z axis rotation by the angle phi (returned in position 1 of the output vector), followed by
	/// an X axis rotation by the angle theta (returned in position 3 of the output vector), followed by another
	/// Z axis rotation by the angle psi (returned in position 2 of the output vector).
	/// This code is a modified version of Alex Z's code from r++.
	///
	/// @details
	/// The range of phi is [ -pi, pi ];
	/// The range of psi is [ -pi, pi ];
	/// The range of theta is [ 0, pi ];
	///
	/// The function pretends that this HomogeneousTransform is the result of these three transformations;
	/// if it were, then the rotation matrix would be
	///
	/// FIGURE 1:
	/// R = [
	///       cos(psi)cos(phi)-cos(theta)sin(phi)sin(psi)        cos(psi)sin(phi)+cos(theta)cos(phi)sin(psi)      sin(psi)sin(theta)
	///      -sin(psi)cos(phi)-cos(theta)sin(phi)cos(psi)       -sin(psi)sin(phi)+cos(theta)cos(phi)cos(psi)      cos(psi)sin(theta)
	///                   sin(theta)sin(phi)                                 -sin(theta)cos(phi)                        cos(theta)
	/// ]
	///
	/// where each axis above is represented as a ROW VECTOR (to be distinguished from the
	/// HomogeneousTransform's representation of axes as COLUMN VECTORS).
	///
	/// The zz_ coordinate gives away theta.
	/// Theta may be computed as acos( zz_ ), or, as Alex does it, asin( sqrt( 1 - zz^2))
	/// Since there is redundancy in theta, this function chooses a theta with a positive
	/// sin(theta): i.e. quadrants I and II.  Assuming we have a positive sin theta
	/// pushes phi and psi into conforming angles.
	///
	/// NOTE on theta: asin returns a value in the range [ -pi/2, pi/2 ], and we have artificially
	/// created a positive sin(theta), so we will get a asin( pos_sin_theta ), we have a value
	/// in the range [ 0, pi/2 ].  To convert this into the actual angle theta, we examine the zz sign.
	/// If zz is negative, we chose the quadrant II theta.
	/// That is, asin( pos_sin_theta) returned an angle, call it theta'.  Now, if cos( theta ) is negative,
	/// then we want to choose the positive x-axis rotation that's equivalent to -theta'.  To do so,
	/// we reflect q through the y axis (See figure 2 below) to get p and then measure theta as pi - theta'.
	///
	/// FIGURE 2:
	///
	///  II        |         I
	///            |
	///    p.      |      .q (cos(-theta'), abs(sin(theta')))
	///       .    |    .
	/// theta'( .  |  .  )  theta' = asin( abs(sin(theta))
	/// -----------------------
	///            |
	///            |
	///            |
	///  III       |        IV
	///            |
	///  The angle between the positive x axis and p is pi - theta'.
	///
	///
	///
	/// Since zx and zy contain only phi terms and a constant sin( theta ) term,
	/// phi is given by atan2( sin_phi, cos_phi ) = atan2( c*sin_phi, c*cos_phi ) = atan2( zx, -zy )
	/// for c positive and non-zero.  If sin_theta is zero, or very close to zero, we're at gimbal lock.
	///
	/// Moreover, since xz and yz contain only psi terms, psi may also be deduced using atan2.
	///
	/// There are 2 degenerate cases (gimbal lock)
	/// 1. theta close to 0  (North Pole singularity), or
	/// 2. theta close to pi (South Pole singularity)
	/// For these, we take: phi=acos(xx), theta = 0 (resp. Pi/2), psi = 0
	template<class M,class E> void
	euler_angles(M const & m, E & euler){

		typedef typename get_scalar<M>::type T;
		static T const pi = boost::math::constants::pi<T>();
		static T const pi_2 = boost::math::constants::pi<T>() * (T)2;
		static T const epsilon = (T)10*std::numeric_limits<T>::epsilon();
		static T const FLOAT_PRECISION = std::sqrt(epsilon);
		if ( zz(m) >= (T)1 - FLOAT_PRECISION ){
			euler[0] = std::atan2( sin_cos_range( yx(m) ), sin_cos_range( xx(m) ) );
			euler[1] = 0.0;
			euler[2] = 0.0;
		} else if ( zz(m) <= (T)-1 + FLOAT_PRECISION ){
			euler[0] = std::atan2( sin_cos_range( yx(m) ), sin_cos_range( xx(m) ) );
			euler[1] = 0.0;
			euler[2] = M_PI;
		} else {
			T pos_sin_theta = std::sqrt( (T)1 - zz(m)*zz(m) ); // sin2theta = 1 - cos2theta.
			euler[2] = std::asin( pos_sin_theta );
			if ( zz(m) < 0 ) {
				euler[2] = pi - euler[2];
			}
			euler[0] = std::atan2( xz(m), -yz(m) );
			euler[1] = std::atan2( zx(m),  zy(m) );
		}
		euler[0] += euler[0]<0.0 ? pi_2 : 0.0;
		euler[1] += euler[1]<0.0 ? pi_2 : 0.0;

		euler[0] = std::min(  std::max( 0.0, euler[0] ),  pi_2-epsilon  );
		euler[1] = std::min(  std::max( 0.0, euler[1] ),  pi_2-epsilon  );
		euler[2] = std::min(  std::max( 0.0, euler[2] ),  pi  -epsilon  );

		assert( 0 <= euler[0] ); assert( euler[0] <  pi_2 );
		assert( 0 <= euler[1] ); assert( euler[1] <  pi_2 );
		assert( 0 <= euler[2] ); assert( euler[2] <= pi   );
	}

	///@brief euler to matrix
	template<class M, class E> void
	from_euler_angles( E const & euler, M & m ){
		typedef typename get_scalar<M>::type T;
		T const ce1( std::cos( euler[0] ) ),  se1( std::sin( euler[0] ) );
		T const ce2( std::cos( euler[1] ) ),  se2( std::sin( euler[1] ) );
		T const ce3( std::cos( euler[2] ) ),  se3( std::sin( euler[2] ) );
		xx(m) =  ce2 * ce1 - ce3 * se1 * se2;  yx(m) =  ce2 * se1 + ce3 * ce1 * se2;  zx(m) =  se2 * se3;
		xy(m) = -se2 * ce1 - ce3 * se1 * ce2;  yy(m) = -se2 * se1 + ce3 * ce1 * ce2;  zy(m) =  ce2 * se3;
		xz(m) =                    se3 * se1;  yz(m) =                   -se3 * ce1;  zz(m) =        ce3;
	}

	template<class M, class E> void
	euler_angles_deg(M const & m, E & euler) {
		typedef typename get_scalar<M>::type T;
		static T const rad_to_deg = 180.0 / boost::math::constants::pi<T>();
		euler_angles(m,euler);
		euler[0] = euler[0] * rad_to_deg;
		euler[1] = euler[1] * rad_to_deg;
		euler[2] = euler[2] * rad_to_deg;
	}
	template<class M, class E> void
	from_euler_angles_deg(E const & erad, M & m) {
		typedef typename get_scalar<M>::type T;
		static T const deg_to_rad = boost::math::constants::pi<T>() / 180.0;
		E euler; euler[0];
		euler[0] = erad[0] * deg_to_rad;
		euler[1] = erad[1] * deg_to_rad;
		euler[2] = erad[2] * deg_to_rad;
		euler_angles(m,euler);
	}

}}

#endif


#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "./left.png";
string right_file = "./right.png";

void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    double b=0.573;

    cv::Mat left=cv::imread(left_file);
    cv::Mat right=cv::imread(right_file);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left,right,disparity_sgbm);
    disparity_sgbm.convertTo(disparity,CV_32F,1.0/16.0f);//signed short 2byte -> float 4byte

    vector<Vector4d,Eigen::aligned_allocator<Vector4d>> pointcloud;

    cout<<left.rows<<" "<<left.cols<<" "<<left.step[0]<<" "<<left.step[1]<<" "<<left.channels()<<endl;
    cout<<disparity_sgbm.rows<<" "<<disparity_sgbm.cols<<" "<<disparity_sgbm.step[0]<<" "<<disparity_sgbm.step[1]<<" "<<disparity_sgbm.channels()<<endl;
    cout<<disparity.rows<<" "<<disparity.cols<<" "<<disparity.step[0]<<" "<<disparity.step[1]<<" "<<disparity.channels()<<endl;

    cout<<disparity_sgbm.type()<<" "<<disparity.type()<<" "<<disparity_sgbm.at<short>(10,0)<<" "<< disparity.at<float>(10,0)<<endl;
/*
+--------+----+----+----+----+------+------+------+------+
|        | C1 | C2 | C3 | C4 | C(5) | C(6) | C(7) | C(8) |
+--------+----+----+----+----+------+------+------+------+
| CV_8U  |  0 |  8 | 16 | 24 |   32 |   40 |   48 |   56 |
| CV_8S  |  1 |  9 | 17 | 25 |   33 |   41 |   49 |   57 |
| CV_16U |  2 | 10 | 18 | 26 |   34 |   42 |   50 |   58 |
| CV_16S |  3 | 11 | 19 | 27 |   35 |   43 |   51 |   59 |
| CV_32S |  4 | 12 | 20 | 28 |   36 |   44 |   52 |   60 |
| CV_32F |  5 | 13 | 21 | 29 |   37 |   45 |   53 |   61 |
| CV_64F |  6 | 14 | 22 | 30 |   38 |   46 |   54 |   62 |
+--------+----+----+----+----+------+------+------+------+
*/
    for(int v=0;v<left.rows;v++)
    {
        for(int u=0;u<left.cols;u++)
        {
            if(disparity.at<float>(v,u)<=0||disparity.at<float>(v,u)>=96.0) continue;

            Vector4d point(0,0,0,left.at<uchar>(v,u)/255.0);

            double depth=b*fx/(disparity.at<float>(v,u));//disparity in pixel
            double x=depth*(u-cx)/fx;
            double y=depth*(v-cy)/fy;

            point[0]=x;
            point[1]=y;
            point[2]=depth;

            pointcloud.push_back(point);
            //cout<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[0]<<" "<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[1]<<" "<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[2]<<" "<<endl;
            //cout<<point<<endl;

        }

    }
    cv::imshow("disparity",disparity/255.0);
    cv::waitKey(0);
    showPointCloud(pointcloud);
    return 0;

}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100903    K. Kumar          File created.
 *      100916    L. Abdulkadir     File checked.
 *      100929    K. Kumar          Checked code by D. Dirkx added.
 *      101110    K. Kumar          Added raiseToExponentPower( ) function.
 *      102410    D. Dirkx          Minor comment changes as code check.
 *      101213    K. Kumar          Bugfix raiseToIntegerExponent( ); renamed raiseToIntegerPower( ).
 *                                  Added computeAbsoluteValue( ) functions.
 *      110202    K. Kumar          Added overload for State* for computeLinearInterpolation( ).
 *      110111    J. Melman         Added computeModulo( ) function.
 *      110411    K. Kumar          Added convertCartesianToSpherical( ) function.
 *      110606    J. Melman         Removed possible singularity from
 *                                  convertCartesianToSpherical.
 *      110707    K. Kumar          Added computeSampleMean( ), computeSampleVariance( ) functions.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120202    K. Kumar          Moved linear interpolation functions into new Interpolators
 *                                  sub-directory.
 *      120716    D. Dirkx          Updated with interpolator architecture.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 *    Notes
 *
 */

#include <boost/multi_array.hpp>

#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"

namespace tudat
{
namespace interpolators
{

//! Compute linear interpolation.
double computeLinearInterpolation( const Eigen::VectorXd& sortedIndependentVariables,
                                   const Eigen::VectorXd& associatedDependentVariables,
                                   const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestNeighbor;
    double locationTargetIndependentVariableValueInInterval;

    // Compute nearest neighbor in sorted vector of independent variables.
    // Result is always to the left of the target independent variable value.
    nearestNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
            sortedIndependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - sortedIndependentVariables[ nearestNeighbor ] )
             / ( sortedIndependentVariables[ nearestNeighbor + 1 ]
                 - sortedIndependentVariables[ nearestNeighbor ] );

    // Return the computed value of the dependent variable.
    return ( associatedDependentVariables[ nearestNeighbor ]
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + associatedDependentVariables[ nearestNeighbor + 1 ]
             * locationTargetIndependentVariableValueInInterval );
}

//! Compute linear interpolation.
Eigen::VectorXd computeLinearInterpolation(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestLeftNeighbor;

    // Declare location of target independent variable value in interval.
    double locationTargetIndependentVariableValueInInterval;

    // Declare map iterators
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalLeft;
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalRight;

    // Compute nearest neighbor in map of data.
    // Result is always to the left of the target independent variable value.
    nearestLeftNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
                sortedIndepedentAndDependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    mapIteratorIntervalLeft = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalLeft, nearestLeftNeighbor );
    mapIteratorIntervalRight = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalRight, nearestLeftNeighbor + 1 );
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - mapIteratorIntervalLeft->first )
             / ( mapIteratorIntervalRight->first
                 - mapIteratorIntervalLeft->first );

    // Return the computed value of the dependent variable.
    return ( mapIteratorIntervalLeft->second
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + mapIteratorIntervalRight->second
             * locationTargetIndependentVariableValueInInterval );
}

} // namespace interpolators
} // mamespace tudat


//  Copyright (c) 2006 Xiaogang Zhang
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it slightly to fit into the
//  Boost.Math conceptual framework better.

#ifndef BOOST_MATH_ELLINT_RD_HPP
#define BOOST_MATH_ELLINT_RD_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>

// Carlson's elliptic integral of the second kind
// R_D(x, y, z) = R_J(x, y, z, z) = 1.5 * \int_{0}^{\infty} [(t+x)(t+y)]^{-1/2} (t+z)^{-3/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rd_imp(T x, T y, T z, const Policy& pol)
{
    T value, u, lambda, sigma, factor, tolerance;
    T X, Y, Z, EA, EB, EC, ED, EE, S1, S2;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_rd<%1%>(%1%,%1%,%1%)";

    if (x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be >= 0, but got %1%", x, pol);
    }
    if (y < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must be >= 0, but got %1%", y, pol);
    }
    if (z <= 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument z must be > 0, but got %1%", z, pol);
    }
    if (x + y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "At most one argument can be zero, but got, x + y = %1%", x+y, pol);
    }

    // error scales as the 6th power of tolerance
    tolerance = pow(tools::epsilon<T>() / 3, T(1)/6);

    // duplication
    sigma = 0;
    factor = 1;
    k = 1;
    do
    {
        u = (x + y + z + z + z) / 5;
        X = (u - x) / u;
        Y = (u - y) / u;
        Z = (u - z) / u;
        if ((tools::max)(abs(X), abs(Y), abs(Z)) < tolerance)
           break;
        T sx = sqrt(x);
        T sy = sqrt(y);
        T sz = sqrt(z);
        lambda = sy * (sx + sz) + sz * sx; //sqrt(x * y) + sqrt(y * z) + sqrt(z * x);
        sigma += factor / (sz * (z + lambda));
        factor /= 4;
        x = (x + lambda) / 4;
        y = (y + lambda) / 4;
        z = (z + lambda) / 4;
        ++k;
    }
    while(k < policies::get_max_series_iterations<Policy>());

    // Check to see if we gave up too soon:
    policies::check_series_iterations<T>(function, k, pol);

    // Taylor series expansion to the 5th order
    EA = X * Y;
    EB = Z * Z;
    EC = EA - EB;
    ED = EA - 6 * EB;
    EE = ED + EC + EC;
    S1 = ED * (ED * T(9) / 88 - Z * EE * T(9) / 52 - T(3) / 14);
    S2 = Z * (EE / 6 + Z * (-EC * T(9) / 22 + Z * EA * T(3) / 26));
    value = 3 * sigma + factor * (1 + S1 + S2) / (u * sqrt(u));

    return value;
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rd(T1 x, T2 y, T3 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rd_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z), pol), "boost::math::ellint_rd<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rd(T1 x, T2 y, T3 z)
{
   return ellint_rd(x, y, z, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RD_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

//[area_with_strategy
//` Calculate the area of a polygon

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Create spherical polygon
    bg::model::polygon<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > > sph_poly;
    bg::read_wkt("POLYGON((0 0,0 1,1 0,0 0))", sph_poly);

    // Create spherical strategy with mean Earth radius in meters
    bg::strategy::area::spherical<> sph_strategy(6371008.8);

    // Calculate the area of a spherical polygon
    double area = bg::area(sph_poly, sph_strategy);
    std::cout << "Area: " << area << std::endl;

    // Create geographic polygon
    bg::model::polygon<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > > geo_poly;
    bg::read_wkt("POLYGON((0 0,0 1,1 0,0 0))", geo_poly);

    // Create geographic strategy with WGS84 spheroid
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategy::area::geographic<> geo_strategy(spheroid);

    // Calculate the area of a geographic polygon
    area = bg::area(geo_poly, geo_strategy);
    std::cout << "Area: " << area << std::endl;

    return 0;
}

//]

//[area_with_strategy_output
/*`
Output:
[pre
Area: 6.18249e+09
Area: 6.15479e+09
]
*/
//]


/// @file  embed_cmp_with_tau.cpp
/// @brief Implements kernel methods based on Kendall's tau.

#include <ogt/embed/cmp.hpp>
#include <ogt/linalg/linalg.hpp>
#include <Eigen/Sparse>

using std::min;
using std::max;
using std::vector;
using Eigen::Index;
using Eigen::MatrixXd;
using Eigen::SparseMatrix;
using Eigen::VectorXd;
using OGT_NAMESPACE::linalg::kernelForFeatures;

typedef Eigen::Triplet<double> sptriple;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpMat;

namespace OGT_NAMESPACE {
namespace embed {

/// Finds the index of the specified object pair.
Index pair(size_t a, size_t b, size_t nObj) {
	Index i = min(a, b), j = max(a, b);
	return (nObj * (nObj-1) / 2)
		- ((nObj - i) * (nObj - i - 1) / 2)
		+ (j - i - 1);
}

/// Computes a kernel matrix from a sparse feature matrix.
/// Computes the k1 kernel matrix from the paper cited for embedCmpWithTauForK()
MatrixXd embedCmpWithTauK1(const vector<CmpConstraint>& cons, size_t nObj) {
	vector<sptriple> triplets;
	triplets.reserve(cons.size());
	VectorXd norm = VectorXd::Zero(nObj);
	Index nPairs = nObj * (nObj - 1) / 2;
	for (const auto& con : cons) {
		assert(con.a < nObj && con.b < nObj && con.c < nObj);
		Index pr = pair(con.b, con.c, nObj);
		triplets.emplace_back(con.a, pr, (con.b < con.c) ? +1 : -1);
		norm(con.a)++;
	}
	SpMat phi(nObj, nPairs);
	phi.setFromTriplets(triplets.begin(), triplets.end());
	for (size_t obj = 0; obj < nObj; obj++) {
		if (norm(obj) > 0) {
			phi.row(obj) /= sqrt(norm(obj));
		}
	}
	return kernelForFeatures(phi);
}

/// Computes the k2 kernel matrix from the paper cited for embedCmpWithTauForK()
MatrixXd embedCmpWithTauK2(const vector<CmpConstraint>& cons, size_t nObj) {
	vector<sptriple> triplets;
	triplets.reserve(2 * cons.size());
	VectorXd norm = VectorXd::Zero(nObj);
	Index nPairs = nObj * (nObj - 1) / 2;
	for (const auto& con : cons) {
		assert(con.a < nObj && con.b < nObj && con.c < nObj);
		Index pr = pair(con.a, con.c, nObj);
		triplets.emplace_back(con.b, pr, +1);
		norm(con.b)++;

		pr = pair(con.a, con.b, nObj);
		if (pr >= nPairs) {
			std::cerr << "pair " << pr << " > " << nPairs << std::endl;
		}
		triplets.emplace_back(con.c, pr, -1);
		norm(con.c)++;
	}
	SpMat phi(nObj, nPairs);
	phi.setFromTriplets(triplets.begin(), triplets.end());
	for (size_t obj = 0; obj < nObj; obj++) {
		if (norm(obj) > 0) {
			phi.row(obj) /= sqrt(norm(obj));
		}
	}
	return kernelForFeatures(phi);
}

/// Compute a kernel matrix where K_ij is derived by how similarly points i and
/// j are ranked by the provided comparisons.
EmbedResult embedCmpWithTauForK(vector<CmpConstraint> cons, double k1,
	double k2) {
	size_t nObj = 0;
	for (const auto& con : cons) {
		nObj = max({nObj, con.a, con.b, con.c});
	}
	nObj++;
	EmbedResult result;
	result.K = MatrixXd::Zero(nObj, nObj);
	if (k1 > 0) {
		result.K += k1 * embedCmpWithTauK1(cons, nObj);
	}
	if (k2 > 0) {
		result.K += k2 * embedCmpWithTauK2(cons, nObj);
	}
	return result;
}

} // end namespace embed
} // end namespace OGT_NAMESPACE