#include "ViewerInh.h"
#include <igl/collapse_edge.h>
#include <igl/edge_flaps.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/adjacency_list.h>
#include <Eigen/Core>
#include <set>
#include <math.h>
#include "igl/circulation.h"

void ViewerInh::init_ds_for_data() {
    int num_of_meshes = static_cast<int>(data_list.size());
    data_edges.resize(static_cast<unsigned long>(num_of_meshes));
    for (int i = 0; i < num_of_meshes; i++) {
        auto ds = new edges;                                            // Create new data structure
        data_edges[i] = reset(data_list[i], ds);
    }
}

edges *ViewerInh::reset(igl::opengl::ViewerData obj, edges* ds){
    Eigen::MatrixXd V = obj.V;                                                         // Get mesh's vertexes
    Eigen::MatrixXi F = obj.F;                                                         // Get mesh's faces
    igl::edge_flaps(F,ds->E,ds->EMAP,ds->EF,ds->EI);                                   // Init the edge related field in the data structure
    ds->Qit.resize(static_cast<unsigned long>(ds->E.rows()));
    ds->C.resize(ds->E.rows(),V.cols());
    ds->Q.clear();
    ds->EQ = get_Q(V,F,obj.F_normals);

    for(int e = 0; e < ds->E.rows(); e++){
        double cost;
        Eigen::RowVectorXd p(1,3);
        calc_edges_cost(e,V,F,ds->E,ds->EMAP,ds->EF, ds->EI, cost, p, ds->EQ);         // Calculate cost for each edge
        ds->Qit[e] = ds->Q.insert(std::pair<double,int>(cost,e)).first;                // Save the edge cost in Q and in Q's iterator- Qit
        ds->C.row(e)   = p;                                                            // Save the optimal vertex p in C at index e
    }
    ds->num_collapsed = 0;
    return ds;
}

 std::vector<Eigen::Matrix4d> ViewerInh::get_Q(Eigen::MatrixXd &V, Eigen::MatrixXi &F, Eigen::MatrixXd &F_normals){
    std::vector<std::vector<int> > vertex_to_vertices;
    std::vector<std::vector<int> > vertex_to_faces_index;
    igl::adjacency_list(F, vertex_to_vertices);
    igl::vertex_triangle_adjacency(V,F,vertex_to_vertices, vertex_to_faces_index);     // Get the faces connected to each vertex

    std::vector<Eigen::Matrix4d> Q;                                                    // Create vector Q of error quadrics
    Q.resize(V.rows());                                                                // Set the size of Q to be as the numbers of vertecies

    for(int v = 0; v < V.rows(); v++) {                                                // For each vertex v in V
        std::vector<int> faces_index_of_vector = vertex_to_vertices[v];                // Get the faces connected to v
        Eigen::Vector4d vertex(V.row(v)(0), V.row(v)(1), V.row(v)(2), 1);              // Get v in vector of size 4
        Eigen::Matrix4d sum_of_Kp;
        sum_of_Kp << 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;                                  // Init 4X4 matrix to insert to Q

        for(int f: faces_index_of_vector) {                                            // For each face connected to v
            Eigen::RowVectorXd p = F_normals.row(f).normalized();                      // p = (a, b, c) -> a^2 + b^2 + c^2 = 1
            double d = (-vertex(0) * p(0))+ (-vertex(1) * p(1)) + (-vertex(2) * p(2)); // d = -ax - by - cz
            Eigen::Vector4d p_with_d(p(0), p(1), p(2), d);                             // p_with_d = (a, b, c, d)
            Eigen::Matrix4d Kp = p_with_d * p_with_d.transpose();                      // Kp
            sum_of_Kp += Kp;                                                           // Add Kp to sum_of_KP
        }
        Q[v] = sum_of_Kp;                                                              // Push sum_of_KP to Q in index v
    }
     return Q;
}

 void ViewerInh::calc_edges_cost(const int e,
                                 const Eigen::MatrixXd & V,
                                 const Eigen::MatrixXi & F,
                                 const Eigen::MatrixXi & E,
                                 const Eigen::VectorXi & EMAP,
                                 const Eigen::MatrixXi & EF,
                                 const Eigen::MatrixXi & EI,
                                 double & cost,
                                 Eigen::RowVectorXd & p,
                                 std::vector<Eigen::Matrix4d> & Q){
        Eigen::RowVectorXd p_(1,3);
        int v1 = E(e,0);                    // Index (in V) of the first vertex in edge e
        int v2 = E(e,1);                    // Index (in V) of the second vertex in edge e
        Eigen::Matrix4d Q1 = Q[v1];         // Error-quadric of v1
        Eigen::Matrix4d Q2 = Q[v2];         // Error-quadric of v2
        Eigen::Matrix4d Q_tag = (Q1 + Q2);  // Error-quadric Q

        Eigen::Vector4d vertex1(V.row(v1)(0), V.row(v1)(1), V.row(v1)(2), 1); // V.row(v1) as vector of size 4
        Eigen::Vector4d vertex2(V.row(v2)(0), V.row(v2)(1), V.row(v2)(2), 1); // V.row(v2) as vector of size 4

        double lowest_cost;          // Init the lowest cost of the edge

        // Create matrix of Q(1,1)-Q(3,4) and (0,0,0,1) at the bottom
        Eigen::Matrix4d v_tag_1;
        v_tag_1 <<
                Q_tag(0,0),Q_tag(0,1),Q_tag(0,2),Q_tag(0,3),
                Q_tag(1,0),Q_tag(1,1),Q_tag(1,2),Q_tag(1,3),
                Q_tag(2,0),Q_tag(2,1),Q_tag(2,2),Q_tag(2,3),
                0,           0,           0,        1;

        if(v_tag_1.determinant() != 0) {                            // Check if v_tag_1 is inversable
            Eigen::Vector4d v_tag_2(0, 0, 0, 1);
            Eigen::Vector4d v_tag = v_tag_1.inverse() * v_tag_2;    // Get new vertex v_tag to collapse e into
            lowest_cost = v_tag.transpose() * Q_tag * v_tag;        // Get the cost of e
            p_ << v_tag(0), v_tag(1), v_tag(2);                      // Set the optimal vertex p to be v_tag
        }

        else{                                                       // If v_tag_1 is not inverse
            Eigen::Vector4d midpoint = (vertex1 + vertex2) * 0.5;   // Calculate mitdpoint of e
            double cost1 = vertex1.transpose()  * Q_tag * vertex1;  // Calculate cost of v1
            double cost2 = vertex2.transpose()  * Q_tag * vertex2;  // Calculate cost of v2
            double cost3 = midpoint.transpose() * Q_tag * midpoint; // Calculate cost of mitdpoint

            lowest_cost = std::min(cost1, cost2);                   // Set cost of e as min(cost1,cost2,cost3)
            lowest_cost = std::min(lowest_cost, cost3);             //

            if (lowest_cost == cost1)                               //
                p_ << vertex1(0), vertex1(1), vertex1(2);           //
            else if (lowest_cost == cost2)                          // Set the optimal vertex p to the vertex with lowest cost
                p_ << vertex2(0), vertex2(1), vertex2(2);           //
            else                                                    //
                p_ << midpoint(0), midpoint(1), midpoint(2);        //
        }
     p = p_;
     cost = lowest_cost;
//     std::cout << e << ") cost " << cost << " at point " << p(0) << ", "<< p(1) << ", "<< p(2) << "\n"<< std::endl;
}

bool ViewerInh::my_collapse(Eigen::MatrixXd & V,
                             Eigen::MatrixXi & F,
                             Eigen::MatrixXd &F_normals,
                             edges *ds){
    //a. Takes out the lowest cost edge from queue.
    if(ds->Q.empty())
    {
        return false;
    }
    std::pair<double,int> q = *(ds->Q.begin());
    ds->Q.erase(ds->Q.begin());
    int e = q.second;
    double cost = q.first;
    ds->Qit[e] = ds->Q.end();
    std::vector<int> N  = igl::circulation(e, true,ds->EMAP,ds->EF,ds->EI);
    std::vector<int> Nd = igl::circulation(e,false,ds->EMAP,ds->EF,ds->EI);
    N.insert(N.begin(),Nd.begin(),Nd.end());

    //b. Deletes edge
    const auto & kill_edge = [&ds](const int e)
    {
        ds->E(e,0) = 0;
        ds->E(e,1) = 0;
        ds->EF(e,0) = 0;
        ds->EF(e,1) = 0;
        ds->EI(e,0) = 0;
        ds->EI(e,1) = 0;
    };

    const int eflip = ds->E(e,0)>ds->E(e,1);
    // source and destination
    const int s = eflip?ds->E(e,1):ds->E(e,0);
    const int d = eflip?ds->E(e,0):ds->E(e,1);
    const std::vector<int> nV2Fd = igl::circulation(e,!eflip,ds->EMAP,ds->EF,ds->EI);
    int e1,f1,e2,f2;

    const int m = F.rows();
    for(int side = 0;side<2;side++)
    {
        const int f = ds->EF(e,side);
        const int v = ds->EI(e,side);
        const int sign = (eflip==0?1:-1)*(1-2*side);
        // next edge emanating from d
        const int e1_ = ds->EMAP(f+m*((v+sign*1+3)%3));
        // prev edge pointing to s
        const int e2_ = ds->EMAP(f+m*((v+sign*2+3)%3));
        if(! (ds->E(e1_,0) == d || ds->E(e1_,1) == d))return false;
        if(! (ds->E(e2_,0) == s || ds->E(e2_,1) == s))return false;
        // face adjacent to f on e1, also incident on d
        const bool flip1 = ds->EF(e1_,1)==f;
        const int f1_ = flip1 ? ds->EF(e1_,0) : ds->EF(e1_,1);
        if(! (f1_!=f))return false;
        if(! (F(f1_,0)==d || F(f1_,1)==d || F(f1_,2) == d))return false;
        // across from which vertex of f1_ does e1 appear?
        const int v1 = flip1 ? ds->EI(e1_,0) : ds->EI(e1_,1);
        // Kill e1_
        kill_edge(e1_);
        // Kill f
        F(f,0) = 0;
        F(f,1) = 0;
        F(f,2) = 0;
        // map f1_'s edge on e1 to e2_
        if(! (ds->EMAP(f1_+m*v1) == e1_))return false;
        ds->EMAP(f1_+m*v1) = e2_;
        // side opposite f2, the face adjacent to f on e2_, also incident on s
        const int opp2 = (ds->EF(e2_,0)==f?0:1);
        if(! (ds->EF(e2_,opp2) == f))return false;
        ds->EF(e2_,opp2) =  f1_;
        ds->EI(e2_,opp2) = v1;
        // remap e2 from d to s
        ds->E(e2_,0) = ds->E(e2_,0)==d ? s : ds->E(e2_,0);
        ds->E(e2_,1) = ds->E(e2_,1)==d ? s : ds->E(e2_,1);
        if(side==0)
        {
            e1 = e1_;
            f1 = f;
        }else
        {
            e2 = e1_;
            f2 = f;
        }
    }

    //c. Deletes faces
    for(auto f : nV2Fd)
    {
        for(int v = 0;v<3;v++)
        {
            if(F(f,v) == d)
            {
                const int flip1 = (ds->EF(ds->EMAP(f+m*((v+1)%3)),0)==f)?1:0;
                const int flip2 = (ds->EF(ds->EMAP(f+m*((v+2)%3)),0)==f)?0:1;
                if(! (ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) == d ||
                                ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) == s)) return false;
                ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) = s;
                if(! (ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) == d ||
                                ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) == s)) return false;
                ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) = s;
                F(f,v) = s;
                break;
            }
        }
    }
    // Finally, "remove" this edge and its information
    kill_edge(e);

    //d. Merges vertices to a new vertex v̅
    Eigen::RowVectorXd v = ds->C.row(e);
    V.row(s) = v;
    V.row(d) = v;
//    ds->EQ = get_Q(V,F,F_normals);

    // Erase the two, other collapsed edges
    ds->Q.erase(ds->Qit[e1]);
    ds->Qit[e1] = ds->Q.end();
    ds->Q.erase(ds->Qit[e2]);
    ds->Qit[e2] = ds->Q.end();
    // update local neighbors
    // loop over original face neighbors
    for(auto n : N)
    {
        if(F(n,0) != 0 ||
           F(n,1) != 0 ||
           F(n,2) != 0)
        {
            for(int v = 0;v<3;v++)
            {
                // get edge id
                const int ei = ds->EMAP(v*F.rows()+n);
                // erase old entry
                ds->Q.erase(ds->Qit[ei]);
                // compute cost and potential placement
                double cost;
                Eigen::RowVectorXd place;
                calc_edges_cost(ei,V,F,ds->E,ds->EMAP,ds->EF,ds->EI,cost,place,ds->EQ);
                // Replace in queue
                ds->Qit[ei] = ds->Q.insert(std::pair<double,int>(cost,ei)).first;
                ds->C.row(ei) = place;
            }
        }
    }

    std::cout << "edge " << e << ", cost = " << cost << "  new v position (" << v(0) << ","<< v(1) << ","<< v(2) << ")\n"<< std::endl;
return true;
}

void ViewerInh::simplify(int num_to_collapse){
    edges *ds = data_edges[selected_data_index];
    Eigen::MatrixXd V = data_list[selected_data_index].V;
    Eigen::MatrixXi F = data_list[selected_data_index].F;
    Eigen::MatrixXd F_normals = data_list[selected_data_index].F_normals;
    int curr_collapsed = 0;

    const auto & collapse = [&ds, &V,  &F, &F_normals](){
            return my_collapse(V, F, F_normals,ds);
    };

    bool something_collapsed = false;
    while(curr_collapsed < num_to_collapse)
    {
        if(collapse()){
            ds->num_collapsed++;
            curr_collapsed++;
            something_collapsed = true;
        }else{
           break;
        }
    }

    if(something_collapsed){
        data_list[selected_data_index].clear();
        data_list[selected_data_index].set_mesh(V, F);
        data_list[selected_data_index].set_face_based(true);
    }

    data_edges[selected_data_index] = ds;
}

void ViewerInh::simplification(){
    long num_of_edges = data_edges[selected_data_index]->E.rows() - data_edges[selected_data_index]->num_collapsed;
    if (num_of_edges < 8)
        return;
    const int max_iter = std::ceil(num_of_edges * 0.05);
    simplify(max_iter);
}


#include<iostream>
#include<cstdlib>
#include<complex>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/lapack/geev.hpp>
#include "utils.h"

using std::cout;
using std::endl;
using std::vector;
using std::complex;

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

void geev(int);
template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>& );

int main(){
    cout << "I'm testing uBlas." << endl;

    int n = 5;
    geev(n);

}
void geev(int n){
    cout << "\nCalculating eigenvalues using LAPACK's geev." << endl;
    ublas::matrix<double, ublas::column_major> A(n,n);
    Hessenberg(A);
    print_m(A);

    ublas::vector<complex<double> > values(n);
    ublas::matrix<complex<double>, ublas::column_major>* Vectors_left = 0;
    ublas::matrix<complex<double>, ublas::column_major> Vectors_right(n,n);

    lapack::geev(A, values, Vectors_left, &Vectors_right, lapack::optimal_workspace());
    print_v(values, "values"); cout << endl;
    print_m(Vectors_right, "Vectors_right"); cout << endl;

    Hessenberg(A);
    cout << "A*x = l*x." << endl;
    for( int i = 0; i < Vectors_right.size2(); ++i ){
        ublas::vector<complex<double> > tmp(n);
        tmp = ublas::prod( A, column(Vectors_right, i) );
        cout << tmp - values(i)*column(Vectors_right,i) << endl;
    }

}
template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>& H){
    T k = 1;
    for( unsigned int i = 0; i < H.size1(); ++i ){
        for( unsigned int j = i; j <= H.size2(); ++j ){
            if( j > 0 ){
                H(i,j-1) = k;
                k += 1;
            }
        }
    }
}


#include "vmcsolver.h"
#include "lib.h"

#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

VMCSolver::VMCSolver() :
    nDimensions(3),
    charge(2),
    nParticles(2),
    h(0.001),
    h2(1000000),
    idum(-1),
    alpha(0.5*charge),
    nCycles(1000000),
    timestep(0.05),
    D(0.5)
{
}

void VMCSolver::runMonteCarloIntegration()
{
  rOld = zeros<mat>(nParticles, nDimensions);
  rNew = zeros<mat>(nParticles, nDimensions);
  QForceOld = zeros<mat>(nParticles, nDimensions);
  QForceNew = zeros<mat>(nParticles, nDimensions);

  double waveFunctionOld = 0;
  double waveFunctionNew = 0;

  double energySum = 0;
  double energySquaredSum = 0;

  double deltaE;

  // initial trial positions
  for(int i = 0; i < nParticles; i++) {
    for(int j = 0; j < nDimensions; j++) {
      rOld(i,j) = GaussianDeviate(&idum)*sqrt(timestep);
    }
  }
  rNew = rOld;

  // loop over Monte Carlo cycles
  for(int cycle = 0; cycle < nCycles; cycle++) {

    // Store the current value of the wave function
    waveFunctionOld = waveFunction(rOld);
    QuantumForce(rOld, QForceOld); QForceOld = QForceOld*h/waveFunctionOld;
    // New position to test
    for(int i = 0; i < nParticles; i++) {
      for(int j = 0; j < nDimensions; j++) {
	rNew(i,j) = rOld(i,j) + GaussianDeviate(&idum)*sqrt(timestep)+QForceOld(i,j)*timestep*D;
      }
      //  for the other particles we need to set the position to the old position since
      //  we move only one particle at the time
      for (int k = 0; k < nParticles; k++) {
	if ( k != i) {
	  for (int j=0; j < nDimensions; j++) {
	    rNew(k,j) = rOld(k,j);
	  }
	}
      }
      // Recalculate the value of the wave function and the quantum force
      waveFunctionNew = waveFunction(rNew);
      QuantumForce(rNew,QForceNew) = QForceNew*h/waveFunctionNew;
      //  we compute the log of the ratio of the greens functions to be used in the
      //  Metropolis-Hastings algorithm
      GreensFunction = 0.0;
      for (int j=0; j < nDimensions; j++) {
	GreensFunction += 0.5*(QForceOld(i,j)+QForceNew(i,j))*
	  (D*timestep*0.5*(QForceOld(i,j)-QForceNew(i,j))-rNew(i,j)+rOld(i,j));
      }
      GreensFunction = exp(GreensFunction);

      // The Metropolis test is performed by moving one particle at the time
      if(ran2(&idum) <= GreensFunction*(waveFunctionNew*waveFunctionNew) / (waveFunctionOld*waveFunctionOld)) {
	for(int j = 0; j < nDimensions; j++) {
	  rOld(i,j) = rNew(i,j);
	  QForceOld(i,j) = QForceNew(i,j);
	  waveFunctionOld = waveFunctionNew;
	}
      } else {
	for(int j = 0; j < nDimensions; j++) {
	  rNew(i,j) = rOld(i,j);
	  QForceNew(i,j) = QForceOld(i,j);
	}
      }
      // update energies
      deltaE = localEnergy(rNew);
      energySum += deltaE;
      energySquaredSum += deltaE*deltaE;
    }
  }
  double energy = energySum/(nCycles * nParticles);
  double energySquared = energySquaredSum/(nCycles * nParticles);
  cout << "Energy: " << energy << " Energy (squared sum): " << energySquared << endl;
}

double VMCSolver::localEnergy(const mat &r)
{
    mat rPlus = zeros<mat>(nParticles, nDimensions);
    mat rMinus = zeros<mat>(nParticles, nDimensions);

    rPlus = rMinus = r;

    double waveFunctionMinus = 0;
    double waveFunctionPlus = 0;

    double waveFunctionCurrent = waveFunction(r);

    // Kinetic energy

    double kineticEnergy = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = 0; j < nDimensions; j++) {
            rPlus(i,j) += h;
            rMinus(i,j) -= h;
            waveFunctionMinus = waveFunction(rMinus);
            waveFunctionPlus = waveFunction(rPlus);
            kineticEnergy -= (waveFunctionMinus + waveFunctionPlus - 2 * waveFunctionCurrent);
            rPlus(i,j) = r(i,j);
            rMinus(i,j) = r(i,j);
        }
    }
    kineticEnergy = 0.5 * h2 * kineticEnergy / waveFunctionCurrent;

    // Potential energy
    double potentialEnergy = 0;
    double rSingleParticle = 0;
    for(int i = 0; i < nParticles; i++) {
        rSingleParticle = 0;
        for(int j = 0; j < nDimensions; j++) {
            rSingleParticle += r(i,j)*r(i,j);
        }
        potentialEnergy -= charge / sqrt(rSingleParticle);
    }
    // Contribution from electron-electron potential
    double r12 = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = i + 1; j < nParticles; j++) {
            r12 = 0;
            for(int k = 0; k < nDimensions; k++) {
                r12 += (r(i,k) - r(j,k)) * (r(i,k) - r(j,k));
            }
            potentialEnergy += 1 / sqrt(r12);
        }
    }

    return kineticEnergy + potentialEnergy;
}

double VMCSolver::waveFunction(const mat &r)
{
    double argument = 0;
    for(int i = 0; i < nParticles; i++) {
        double rSingleParticle = 0;
        for(int j = 0; j < nDimensions; j++) {
            rSingleParticle += r(i,j) * r(i,j);
        }
        argument += sqrt(rSingleParticle);
    }
    return exp(-argument * alpha);
}

double VMCSolver::QuantumForce(const mat &r, mat &QForce)
{
    mat rPlus = zeros<mat>(nParticles, nDimensions);
    mat rMinus = zeros<mat>(nParticles, nDimensions);

    rPlus = rMinus = r;

    double waveFunctionMinus = 0;
    double waveFunctionPlus = 0;

    double waveFunctionCurrent = waveFunction(r);

    // Kinetic energy

    double kineticEnergy = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = 0; j < nDimensions; j++) {
            rPlus(i,j) += h;
            rMinus(i,j) -= h;
            waveFunctionMinus = waveFunction(rMinus);
            waveFunctionPlus = waveFunction(rPlus);
            QForce(i,j) =  (waveFunctionPlus-waveFunctionMinus);
            rPlus(i,j) = r(i,j);
            rMinus(i,j) = r(i,j);
        }
    }
}


#ifndef TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP
#define TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP

#include <cmath>

#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/ooura_fourier_integrals.hpp>

#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/london.hpp>
#include <triumf/superconductivity/pippard.hpp>

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

//
namespace superconductivity {

// nonlocal effects on superconductivity in the Meissner-Ochsenfeld state
namespace nonlocal {

//
template <typename T = double>
T B_z_pippard(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for Pippard's model
  auto integrand = [&, K](T q) {
    // need to check if this is a reasaonble cutoff...
    auto kappa = q > 1e-5 ? 1.0
                          : (3.0 / 2.0) * ((1.0 + q * q) * std::atan(q) - q) /
                                (q * q * q);
    return q / (q * q + K * kappa);
  };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

// "extreme anomalous limit"
template <typename T = double>
auto B_z_extreme_anomalous_limit(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for the "extreme anomalous limit"
  auto integrand = [&, K](T q) { return q / (q * q + K / q); };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

// london
template <typename T = double>
auto B_z_london(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for the "extreme anomalous limit"
  auto integrand = [&, K](T q) { return q / (q * q + K); };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

} // namespace nonlocal

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP


#include <cmath>
#include <cstdio>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() {
	int fib = 42;

    MatrixXd m(2, 2);
    m << 0, 0, 0, 0;
    EigenSolver<MatrixXd> mSolver(m);
    cout << mSolver.eigenvectors() << "\n";

	MatrixXcd T;

	T = MatrixXcd(3, 3);

	MatrixXd F(2, 2);
	F(0, 0) = 0;
	F(0, 1) = 1;
	F(1, 0) = 1;
	F(1, 1) = 1;
	EigenSolver<MatrixXd> eigensolver(F);

	MatrixXcd D = eigensolver.eigenvalues().asDiagonal();
	D(0, 0) = pow(D(0, 0), fib);
	D(1, 1) = pow(D(1, 1), fib);

	MatrixXcd P = eigensolver.eigenvectors();
	MatrixXcd Pinv = P.inverse();
	MatrixXcd S = (P * D * Pinv);

	cout << F << endl
		<< P << endl
		<< Pinv << endl
		<< D << endl
		<< S << endl;

	cout << "fib(" << fib << "):\n";
	cout << S << endl;
	printf("%.0f\n", round(real(S(1, 1))));

	return 0;
}


/* Author: Xing Jin, Wolfgang Bangerth, Texas A&M University, 2006 */

/*    $Id: step-24.cc 27657 2012-11-21 13:19:08Z bangerth $ */
/*    Copyright (C) 2006-2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// The following have all been covered previously:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>
#include <iostream>

// This is the only new one: We will need a library function defined in a
// class GridTools that computes the minimal cell diameter.
#include <deal.II/grid/grid_tools.h>

// The last step is as in all previous programs:
namespace Step24
{
  using namespace dealii;

  // @sect3{The "forward problem" class template}

  // The first part of the main class is exactly as in step-23 (except for the
  // name):
  template <int dim>
  class TATForwardProblem
  {
  public:
    TATForwardProblem ();
    void run ();

  private:
    void setup_system ();
    void solve_p ();
    void solve_v ();
    void output_results () const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;

    ConstraintMatrix constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;

    Vector<double>       solution_p, solution_v;
    Vector<double>       old_solution_p, old_solution_v;
    Vector<double>       system_rhs_p, system_rhs_v;

    double time, time_step;
    unsigned int timestep_number;
    const double theta;

    //  Here's what's new: first, we need that boundary mass matrix $B$ that
    //  came out of the absorbing boundary condition. Likewise, since this
    //  time we consider a realistic medium, we must have a measure of the
    //  wave speed $c_0$ that will enter all the formulas with the Laplace
    //  matrix (which we still define as $(\nabla \phi_i,\nabla \phi_j)$):
    SparseMatrix<double> boundary_matrix;
    const double wave_speed;

    // The last thing we have to take care of is that we wanted to evaluate
    // the solution at a certain number of detector locations. We need an
    // array to hold these locations, declared here and filled in the
    // constructor:
    std::vector<Point<dim> > detector_locations;
  };

  // @sect3{Equation data}

  // As usual, we have to define our initial values, boundary conditions, and
  // right hand side functions. Except things are a bit simpler this time: we
  // are to consider a problem that is driven by initial conditions, so there
  // is no right hand side function (though you could look up in step-23 to
  // see how this can be done. Secondly, there are no boundary conditions: the
  // entire boundary of the domain consists of absorbing boundary
  // conditions. That only leaves initial conditions, and there things are
  // simple too since for this particular application only nonzero initial
  // conditions for the pressure are prescribed, not for the velocity (which
  // is zero at the initial time).
  //
  // So this is all we need: a class that specifies initial conditions for the
  // pressure. In the physical setting considered in this program, these are
  // small absorbers, which we model as a series of little circles where we
  // assume that the pressure surplus is one, whereas no absorption and
  // therefore no pressure surplus is anywhere else. This is how we do things
  // (note that if we wanted to expand this program to not only compile but
  // also to run, we would have to initialize the sources with
  // three-dimensional source locations):
  template <int dim>
  class InitialValuesP : public Function<dim>
  {
  public:
    InitialValuesP ()
      :
      Function<dim>()
    {}

    virtual double value (const Point<dim> &p,
                          const unsigned int  component = 0) const;

  private:
    struct Source
    {
      Source (const Point<dim> &l,
              const double      r)
        :
        location (l),
        radius (r)
      {}

      const Point<dim> location;
      const double     radius;
    };
  };

  template <int dim>
  double InitialValuesP<dim>::value (const Point<dim> &p,
                                     const unsigned int /*component*/) const
  {
    static const Source sources[] = {Source (Point<dim> (0, 0),         0.025),
                                     Source (Point<dim> (-0.135, 0),    0.05),
                                     Source (Point<dim> (0.17, 0),      0.03),
                                     Source (Point<dim> (-0.25, 0),     0.02),
                                     Source (Point<dim> (-0.05, -0.15), 0.015)
                                    };
    static const unsigned int n_sources = sizeof(sources)/sizeof(sources[0]);

    for (unsigned int i=0; i<n_sources; ++i)
      if (p.distance(sources[i].location) < sources[i].radius)
        return 1;

    return 0;
  }

  // @sect3{Implementation of the TATForwardProblem class}

  // Let's start again with the constructor. Setting the member variables is
  // straightforward. We use the acoustic wave speed of mineral oil (in
  // millimeters per microsecond, a common unit in experimental biomedical
  // imaging) since this is where many of the experiments we want to compare
  // the output with are made in. The Crank-Nicolson scheme is used again,
  // i.e. theta is set to 0.5. The time step is later selected to satisfy $k =
  // \frac hc$
  template <int dim>
  TATForwardProblem<dim>::TATForwardProblem ()
    :
    fe (1),
    dof_handler (triangulation),
    theta (0.5),
    wave_speed (1.437)
  {
    // The second task in the constructor is to initialize the array that
    // holds the detector locations. The results of this program were compared
    // with experiments in which the step size of the detector spacing is 2.25
    // degree, corresponding to 160 detector locations. The radius of the
    // scanning circle is selected to be half way between the center and the
    // boundary to avoid that the remaining reflections from the imperfect
    // boundary condition spoils our numerical results.
    //
    // The locations of the detectors are then calculated in clockwise
    // order. Note that the following of course only works if we are computing
    // in 2d, a condition that we guard with an assertion. If we later wanted
    // to run the same program in 3d, we would have to add code here for the
    // initialization of detector locations in 3d. Due to the assertion, there
    // is no way we can forget to do this.
    Assert (dim == 2, ExcNotImplemented());

    const double detector_step_angle = 2.25;
    const double detector_radius = 0.5;

    for (double detector_angle = 2*numbers::PI;
         detector_angle >= 0;
         detector_angle -= detector_step_angle/360*2*numbers::PI)
      detector_locations.push_back (Point<dim> (std::cos(detector_angle),
                                                std::sin(detector_angle)) *
                                    detector_radius);
  }

  // @sect4{TATForwardProblem::setup_system}

  // The following system is pretty much what we've already done in step-23,
  // but with two important differences. First, we have to create a circular
  // (or spherical) mesh around the origin, with a radius of 1. This nothing
  // new: we've done so before in step-6, step-10, and step-11, where we also
  // explain how to attach a boundary object to a triangulation to be used
  // whenever the triangulation needs to know where new boundary points lie
  // when a cell is refined. Following this, the mesh is refined a number of
  // times.
  //
  // One thing we had to make sure is that the time step satisfies the CFL
  // condition discussed in the introduction of step-23. Back in that program,
  // we ensured this by hand by setting a timestep that matches the mesh
  // width, but that was error prone because if we refined the mesh once more
  // we would also have to make sure the time step is changed. Here, we do
  // that automatically: we ask a library function for the minimal diameter of
  // any cell. Then we set $k=\frac h{c_0}$. The only problem is: what exactly
  // is $h$? The point is that there is really no good theory on this question
  // for the wave equation. It is known that for uniformly refined meshes
  // consisting of rectangles, $h$ is the minimal edge length. But for meshes
  // on general quadrilaterals, the exact relationship appears to be unknown,
  // i.e. it is unknown what properties of cells are relevant for the CFL
  // condition. The problem is that the CFL condition follows from knowledge
  // of the smallest eigenvalue of the Laplace matrix, and that can only be
  // computed analytically for simply structured meshes.
  //
  // The upshot of all this is that we're not quite sure what exactly we
  // should take for $h$. The function GridTools::minimal_cell_diameter
  // computes the minimal diameter of all cells. If the cells were all squares
  // or cubes, then the minimal edge length would be the minimal diameter
  // divided by std::sqrt(dim). We simply generalize this,
  // without theoretical justification, to the case of non-uniform meshes.
  //
  // The only other significant change is that we need to build the boundary
  // mass matrix. We will comment on this further down below.
  template <int dim>
  void TATForwardProblem<dim>::setup_system ()
  {
    const Point<dim> center;
    GridGenerator::hyper_ball (triangulation, center, 1.);
    static const HyperBallBoundary<dim> boundary_description (center, 1.);
    triangulation.set_boundary (0,boundary_description);
    triangulation.refine_global (7);

    time_step = GridTools::minimal_cell_diameter(triangulation) /
                wave_speed /
                std::sqrt (1.*dim);

    std::cout << "Number of active cells: "
              << triangulation.n_active_cells()
              << std::endl;

    dof_handler.distribute_dofs (fe);

    std::cout << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << std::endl
              << std::endl;

    sparsity_pattern.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress();

    system_matrix.reinit (sparsity_pattern);
    mass_matrix.reinit (sparsity_pattern);
    laplace_matrix.reinit (sparsity_pattern);

    MatrixCreator::create_mass_matrix (dof_handler, QGauss<dim>(3),
                                       mass_matrix);
    MatrixCreator::create_laplace_matrix (dof_handler, QGauss<dim>(3),
                                          laplace_matrix);

    // The second difference, as mentioned, to step-23 is that we need to
    // build the boundary mass matrix that grew out of the absorbing boundary
    // conditions.
    //
    // A first observation would be that this matrix is much sparser than the
    // regular mass matrix, since none of the shape functions with purely
    // interior support contributes to this matrix. We could therefore
    // optimize the storage pattern to this situation and build up a second
    // sparsity pattern that only contains the nonzero entries that we
    // need. There is a trade-off to make here: first, we would have to have a
    // second sparsity pattern object, so that costs memory. Secondly, the
    // matrix attached to this sparsity pattern is going to be smaller and
    // therefore requires less memory; it would also be faster to perform
    // matrix-vector multiplications with it. The final argument, however, is
    // the one that tips the scale: we are not primarily interested in
    // performing matrix-vector with the boundary matrix alone (though we need
    // to do that for the right hand side vector once per time step), but
    // mostly wish to add it up to the other matrices used in the first of the
    // two equations since this is the one that is going to be multiplied with
    // once per iteration of the CG method, i.e. significantly more often. It
    // is now the case that the SparseMatrix::add class allows to add one
    // matrix to another, but only if they use the same sparsity pattern (the
    // reason being that we can't add nonzero entries to a matrix after the
    // sparsity pattern has been created, so we simply require that the two
    // matrices have the same sparsity pattern).
    //
    // So let's go with that:
    boundary_matrix.reinit (sparsity_pattern);

    // The second thing to do is to actually build the matrix. Here, we need
    // to integrate over faces of cells, so first we need a quadrature object
    // that works on dim-1 dimensional objects. Secondly, the
    // FEFaceValues variant of FEValues that works on faces, as its name
    // suggest. And finally, the other variables that are part of the assembly
    // machinery. All of this we put between curly braces to limit the scope
    // of these variables to where we actually need them.
    //
    // The actual act of assembling the matrix is then fairly straightforward:
    // we loop over all cells, over all faces of each of these cells, and then
    // do something only if that particular face is at the boundary of the
    // domain. Like this:
    {
      const QGauss<dim-1>  quadrature_formula(3);
      FEFaceValues<dim> fe_values (fe, quadrature_formula,
                                   update_values  |  update_JxW_values);

      const unsigned int   dofs_per_cell = fe.dofs_per_cell;
      const unsigned int   n_q_points    = quadrature_formula.size();

      FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);

      std::vector<unsigned int> local_dof_indices (dofs_per_cell);

      typename DoFHandler<dim>::active_cell_iterator
      cell = dof_handler.begin_active(),
      endc = dof_handler.end();
      for (; cell!=endc; ++cell)
        for (unsigned int f=0; f<GeometryInfo<dim>::faces_per_cell; ++f)
          if (cell->at_boundary(f))
            {
              cell_matrix = 0;

              fe_values.reinit (cell, f);

              for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
                for (unsigned int i=0; i<dofs_per_cell; ++i)
                  for (unsigned int j=0; j<dofs_per_cell; ++j)
                    cell_matrix(i,j) += (fe_values.shape_value(i,q_point) *
                                         fe_values.shape_value(j,q_point) *
                                         fe_values.JxW(q_point));

              cell->get_dof_indices (local_dof_indices);
              for (unsigned int i=0; i<dofs_per_cell; ++i)
                for (unsigned int j=0; j<dofs_per_cell; ++j)
                  boundary_matrix.add (local_dof_indices[i],
                                       local_dof_indices[j],
                                       cell_matrix(i,j));
            }

    }

    system_matrix.copy_from (mass_matrix);
    system_matrix.add (time_step * time_step * theta * theta *
                       wave_speed * wave_speed,
                       laplace_matrix);
    system_matrix.add (wave_speed * theta * time_step, boundary_matrix);

    solution_p.reinit (dof_handler.n_dofs());
    old_solution_p.reinit (dof_handler.n_dofs());
    system_rhs_p.reinit (dof_handler.n_dofs());

    solution_v.reinit (dof_handler.n_dofs());
    old_solution_v.reinit (dof_handler.n_dofs());
    system_rhs_v.reinit (dof_handler.n_dofs());

    constraints.close ();
  }

  // @sect4{TATForwardProblem::solve_p and TATForwardProblem::solve_v}

  // The following two functions, solving the linear systems for the pressure
  // and the velocity variable, are taken pretty much verbatim (with the
  // exception of the change of name from $u$ to $p$ of the primary variable)
  // from step-23:
  template <int dim>
  void TATForwardProblem<dim>::solve_p ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs_p.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (system_matrix, solution_p, system_rhs_p,
              PreconditionIdentity());

    std::cout << "   p-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  template <int dim>
  void TATForwardProblem<dim>::solve_v ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs_v.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (mass_matrix, solution_v, system_rhs_v,
              PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  // @sect4{TATForwardProblem::output_results}

  // The same holds here: the function is from step-23.
  template <int dim>
  void TATForwardProblem<dim>::output_results () const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution_p, "P");
    data_out.add_data_vector (solution_v, "V");

    data_out.build_patches ();

    const std::string filename =  "solution-" +
                                  Utilities::int_to_string (timestep_number, 3) +
                                  ".gnuplot";
    std::ofstream output (filename.c_str());
    data_out.write_gnuplot (output);
  }

  // @sect4{TATForwardProblem::run}

  // This function that does most of the work is pretty much again like in
  // step-23, though we make things a bit clearer by using the vectors G1 and
  // G2 mentioned in the introduction. Compared to the overall memory
  // consumption of the program, the introduction of a few temporary vectors
  // isn't doing much harm.
  //
  // The only changes to this function are: First, that we do not have to
  // project initial values for the velocity $v$, since we know that it is
  // zero. And second that we evaluate the solution at the detector locations
  // computed in the constructor. This is done using the
  // VectorTools::point_value function. These values are then written to a
  // file that we open at the beginning of the function.
  template <int dim>
  void TATForwardProblem<dim>::run ()
  {
    setup_system();

    VectorTools::project (dof_handler, constraints,
                          QGauss<dim>(3), InitialValuesP<dim>(),
                          old_solution_p);
    old_solution_v = 0;

    std::ofstream detector_data("detectors.dat");

    Vector<double> tmp (solution_p.size());
    Vector<double> G1 (solution_p.size());
    Vector<double> G2 (solution_v.size());

    const double end_time = 0.7;
    for (timestep_number=1, time=time_step;
         time<=end_time;
         time+=time_step, ++timestep_number)
      {
        std::cout << std::endl;
        std::cout<< "time_step " << timestep_number << " @ t=" << time << std::endl;

        mass_matrix.vmult (G1, old_solution_p);
        mass_matrix.vmult (tmp, old_solution_v);
        G1.add(time_step * (1-theta), tmp);

        mass_matrix.vmult (G2, old_solution_v);
        laplace_matrix.vmult (tmp, old_solution_p);
        G2.add (-wave_speed * wave_speed * time_step * (1-theta), tmp);

        boundary_matrix.vmult (tmp, old_solution_p);
        G2.add (wave_speed, tmp);

        system_rhs_p = G1;
        system_rhs_p.add(time_step * theta , G2);

        solve_p ();

        system_rhs_v = G2;
        laplace_matrix.vmult (tmp, solution_p);
        system_rhs_v.add (-time_step * theta * wave_speed * wave_speed, tmp);

        boundary_matrix.vmult (tmp, solution_p);
        system_rhs_v.add (-wave_speed, tmp);

        solve_v ();

        output_results ();

        detector_data << time;
        for (unsigned int i=0 ; i<detector_locations.size(); ++i)
          detector_data << " "
                        << VectorTools::point_value (dof_handler,
                                                     solution_p,
                                                     detector_locations[i])
                        << " ";
        detector_data << std::endl;

        old_solution_p = solution_p;
        old_solution_v = solution_v;
      }
  }
}

// @sect3{The main function}

// What remains is the main function of the program. There is nothing here
// that hasn't been shown in several of the previous programs:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step24;

      deallog.depth_console (0);

      TATForwardProblem<2> forward_problem_solver;
      forward_problem_solver.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * 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 project's author 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.
 *
 */

#ifndef __STATIMO_CORE_PCA_MODEL_BUILDER_HXX_
#define __STATIMO_CORE_PCA_MODEL_BUILDER_HXX_

#include "statismo/core/PCAModelBuilder.h"
#include "statismo/core/CommonTypes.h"
#include "statismo/core/Exceptions.h"

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

namespace statismo
{

template <typename T>
UniquePtrType<typename PCAModelBuilder<T>::StatisticalModelType>
PCAModelBuilder<T>::BuildNewModel(const DataItemListType & sampleDataList,
                                  double                   noiseVariance,
                                  bool                     computeScores,
                                  EigenValueMethod         method) const
{

  STATISMO_LOG_INFO("Building new model");
  STATISMO_LOG_INFO("Noise variance: " + std::to_string(noiseVariance));

  auto n = sampleDataList.size();
  STATISMO_LOG_INFO("Sample count: " + std::to_string(n));

  if (n <= 0)
  {
    throw StatisticalModelException("Provided empty sample set. Cannot build the sample matrix",
                                    Status::BAD_INPUT_ERROR);
  }

  unsigned     p = sampleDataList.front()->GetSampleVector().rows();
  const auto * representer = sampleDataList.front()->GetRepresenter();

  // Compute the mean vector mu
  VectorType mu = VectorType::Zero(p);

  for (const auto & item : sampleDataList)
  {
    assert(item->GetSampleVector().rows() == p);   // all samples must have same number of rows
    assert(item->GetRepresenter() == representer); // all samples have the same representer
    mu += item->GetSampleVector();
  }

  mu /= n;

  // Build the mean free sample matrix X0
  MatrixType matX0(n, p);
  unsigned   i{ 0 };
  for (const auto & item : sampleDataList)
  {
    matX0.row(i++) = item->GetSampleVector() - mu;
  }

  // build the model
  auto model = BuildNewModelInternal(representer, matX0, mu, noiseVariance, method);

  // compute the scores if requested
  MatrixType scores;
  if (computeScores)
  {
    scores = this->ComputeScores(sampleDataList, model.get());
  }

  typename BuilderInfo::ParameterInfoList bi;
  bi.emplace_back(BuilderInfo::KeyValuePair("NoiseVariance ", std::to_string(noiseVariance)));

  typename BuilderInfo::DataInfoList dataInfo;
  i = 0;
  for (const auto & item : sampleDataList)
  {
    STATISMO_LOG_INFO("Adding info about dataset with uri " + item->GetDatasetURI());
    std::ostringstream os;
    os << "URI_" << i++;
    dataInfo.emplace_back(os.str().c_str(), item->GetDatasetURI());
  }

  // finally add meta data to the model info
  ModelInfo::BuilderInfoList biList;
  biList.emplace_back("PCAModelBuilder", dataInfo, bi);

  model->SetModelInfo(ModelInfo{ scores, biList });

  return model;
}

template <typename T>
UniquePtrType<typename PCAModelBuilder<T>::StatisticalModelType>
PCAModelBuilder<T>::BuildNewModelInternal(const Representer<T> * representer,
                                          const MatrixType &     matX0,
                                          const VectorType &     mu,
                                          double                 noiseVariance,
                                          EigenValueMethod       method) const
{
  unsigned n = matX0.rows();
  unsigned p = matX0.cols();

  STATISMO_LOG_INFO("n: " + std::to_string(n));
  STATISMO_LOG_INFO("p: " + std::to_string(p));

  switch (method)
  {
    case EigenValueMethod::JACOBI_SVD:

      STATISMO_LOG_INFO("Using JACOBI_SVD method");

      using SVDType = Eigen::JacobiSVD<MatrixType>;
      using SVDDoublePrecisionType = Eigen::JacobiSVD<MatrixTypeDoublePrecision>;

      // We destinguish the case where we have more variables than samples and
      // the case where we have more samples than variable.
      // In the first case we compute the (smaller) inner product matrix instead of the full covariance matrix.
      // It is known that this has the same non-zero singular values as the covariance matrix.
      // Furthermore, it is possible to compute the corresponding eigenvectors of the covariance matrix from the
      // decomposition.

      if (n < p)
      {
        // we compute the eigenvectors of the covariance matrix by computing an SVD of the
        // n x n inner product matrix 1/(n-1) X0X0^T
        MatrixType             cov = matX0 * matX0.transpose() * 1.0 / (n - 1);
        SVDDoublePrecisionType svd(cov.cast<double>(), Eigen::ComputeThinV);
        VectorType             singularValues = svd.singularValues().cast<ScalarType>();
        MatrixType             matV = svd.matrixV().cast<ScalarType>();

        unsigned numComponentsAboveTolerance =
          ((singularValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();

        // there can be at most n-1 nonzero singular values in this case. Everything else must be due to numerical
        // inaccuracies
        unsigned numComponentsToKeep = std::min(numComponentsAboveTolerance, n - 1);
        // compute the pseudo inverse of the square root of the singular values
        // which is then needed to recompute the PCA basis
        VectorType singSqrt = singularValues.array().sqrt();
        VectorType singSqrtInv = VectorType::Zero(singSqrt.rows());
        for (unsigned i = 0; i < numComponentsToKeep; i++)
        {
          assert(singSqrt(i) > Superclass::sk_tolerance);
          singSqrtInv(i) = 1.0 / singSqrt(i);
        }

        if (numComponentsToKeep == 0)
        {
          STATISMO_LOG_ERROR("No component to keep");
          throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        // we recover the eigenvectors U of the full covariance matrix from the eigenvectors V of the inner product
        // matrix. We use the fact that if we decompose X as X=UDV^T, then we get X^TX = UD^2U^T and XX^T = VD^2V^T
        // (exploiting the orthogonormality of the matrix U and V from the SVD). The additional factor sqrt(n-1) is to
        // compensate for the 1/sqrt(n-1) in the formula for the covariance matrix.

        MatrixType pcaBasis = matX0.transpose() * matV * singSqrtInv.asDiagonal();
        pcaBasis /= sqrt(n - 1.0);
        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);

        auto model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);

        return model;
      }
      else // NOLINT
      {
        // we compute an SVD of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly
        SVDType    svd(matX0.transpose() * matX0, Eigen::ComputeThinU);
        VectorType singularValues = svd.singularValues();
        singularValues /= (n - 1.0);
        unsigned numComponentsToKeep =
          ((singularValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();
        MatrixType pcaBasis = svd.matrixU();

        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        if (numComponentsToKeep == 0)
        {
          STATISMO_LOG_ERROR("No component to keep");
          throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
        auto       model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);
        return model;
      }
      break;

    case EigenValueMethod::SELF_ADJOINT_EIGEN_SOLVER:
    {
      STATISMO_LOG_INFO("Using SELF_ADJOINT_EIGEN_SOLVER method");
      // we compute the eigenvalues/eigenvectors of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly

      using SelfAdjointEigenSolver = Eigen::SelfAdjointEigenSolver<MatrixType>;
      SelfAdjointEigenSolver es;
      es.compute(matX0.transpose() * matX0);
      VectorType eigenValues =
        es.eigenvalues().reverse(); // SelfAdjointEigenSolver orders the eigenvalues in increasing order
      eigenValues /= (n - 1.0);

      unsigned   numComponentsToKeep = ((eigenValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();
      MatrixType pcaBasis = es.eigenvectors().rowwise().reverse();
      pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

      if (numComponentsToKeep == 0)
      {
        throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
      }

      VectorType sampleVarianceVector = eigenValues.topRows(numComponentsToKeep);
      VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
      auto       model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);
      return model;
    }
    break;

    default:
      throw StatisticalModelException("Unrecognized decomposition/eigenvalue solver method.");
  }
  return nullptr;
}

} // namespace statismo

#endif


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/qr.hpp
 *
 * \brief The QR matrix decomposition.
 *
 * Given a matrix \f$A\f$, its QR-decomposition is a matrix decomposition of the
 * form:
 * \f[
 *   A=QR
 * \f]
 * where \f$R\f$ is an m-by-n upper trapezoidal (or, when \f$m \ge n\f$,
 * triangular) matrix and \f$Q\f$ is an m-by-m orthogonal (or unitary) matrix,
 * that is one satisfying:
 * \f[
 *   Q^{T}Q=I,
 * \f]
 * where \f$Q^{T}\f$ is the transpose of \f$Q\f$ and \f$I\f$ is the identity
 * matrix.
 *
 * For the special case of \f$m \ge n\f$, the factorization can be rewritten as:
 * \f[
 *  A=\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     R_1 \\
 *     R_2 \\
 *     \end{pmatrix}
 *   =\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     R_1 \\
 *     0 \\
 *     \end{pmatrix}
 *   = Q_1 R_1
 * \f]
 * where \f$Q_1\f$ is an m-by-n matrix, \f$Q_2\f$ is an m-by-(m-n) matrix,
 * \f$R_1\f$ is an n-by-n lower triangular matrix, and \f$R_2\f$ is an
 * (m-n)-by-n zero matrix.
 *
 * This matrix decomposition can be used to solve linear systems of equations,
 * especially the ones involved in the linear least squares problem.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP

#include <algorithm>
#include <boost/mpl/and.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/numeric/bindings/lapack/computational/geqrf.hpp>
#include <boost/numeric/bindings/lapack/computational/orgqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ungqr.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_same.hpp>
#include <complex>
#include <cstddef>
#include <stdint.h>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

/**
 * \brief Common operations for QR decomposition.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
struct qr_decomposition_impl_common;

/**
 * \brief Type-oriented operations for QR decomposition.
 *
 * \tparam IsComplex Logical parameter telling if the we are doing either a real
 *  or a complex QR decomposition.
 *
 * This class makes distinction between the real and the complex case.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <bool IsComplex>
struct qr_decomposition_impl;

struct qr_decomposition_impl_common
{
    /// Performan QR decomposition of the given input matrix \a A
    /// (row-major case).
    template <typename AMatrixT, typename TauVectorT>
        static void decompose(AMatrixT& A, TauVectorT& tau, row_major_tag)
    {
        matrix<typename matrix_traits<AMatrixT>::value_type, column_major> tmp_A(A);

        decompose(tmp_A, tau, column_major_tag());

        A = tmp_A;
    }

    /// Performan QR decomposition of the given input matrix \a A
    /// (column-major case).
    template <typename AMatrixT, typename TauVectorT>
        static void decompose(AMatrixT& A, TauVectorT& tau, column_major_tag)
    {
        typedef typename matrix_traits<AMatrixT>::size_type size_type;

        size_type m = num_rows(A);
        size_type n = num_columns(A);
        size_type k = ::std::min(m,n);

        if (size(tau) != k)
        {
            tau.resize(k, false);
        }

        ::boost::numeric::bindings::lapack::geqrf(A, tau);
    }

    /**
     * \brief Extract the R matrix from a previously computing QR decomposition
     * (row-major case).
     *
     * Let QR be an m-by-n matrix, then the R matrix is built by taking the
     * min(m,n)-by-n upper trapezoidal (triangular, if m >= n) elements of QR.
     */
    template <typename QRMatrixT, typename RMatrixT>
        static void extract_R(QRMatrixT const& QR, RMatrixT& R, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<RMatrixT>::value_type, column_major> tmp_R(R);

        extract_R(tmp_QR, tmp_R, full, column_major_tag());

        R = tmp_R;
    }

    /**
     * \brief Extract the R matrix from a previously computing QR decomposition
     * (row-major case).
     *
     * Let QR be an m-by-n matrix, then the R matrix is built by taking the
     * min(m,n)-by-n upper trapezoidal (triangular, if m >= n) elements of QR.
     */
    template <typename QRMatrixT, typename RMatrixT>
        static void extract_R(QRMatrixT const& QR, RMatrixT& R, bool full, column_major_tag)
    {
        typedef typename matrix_traits<RMatrixT>::size_type size_type;
        typedef typename matrix_traits<RMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nr = full ? m : ::std::min(m,n);

        if (num_rows(R) != nr && num_columns(R) != n)
        {
            R.resize(nr, n, false);
        }

        if (m >= n)
        {
            // The upper triangle of the submatrix QR(1:n,1:n) contains the
            // min(m,n)-by-n upper triangular matrix R.
            for (size_type row = 0; row < n; ++row)
            {
                for (size_type col = 0; col < n; ++col)
                {
                    if (col >= row)
                    {
                        R(row,col) = QR(row,col);
                    }
                    else
                    {
                        R(row,col) = value_type/*zero*/();
                    }
                }
            }

            // Set to zero the last m-n rows
            if (full)
            {
                subrange(R, n, m, 0, n) = scalar_matrix<value_type>(m-n, n, value_type/*zero*/());
            }
        }
        else
        {
            // The elements on and upper the n-th subdiagonal contain the
            // m-by-n upper trapezoidal matrix R.
            for (size_type row = 0; row < m; ++row)
            {
                for (size_type col = 0; col < n; ++col)
                {
                    if (col >= row)
                    {
                        R(row,col) = QR(row,col);
                    }
                    else
                    {
                        R(row,col) = value_type/*zero*/();
                    }
                }
            }
        }
    }

    /**
     * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
     *  the QR decomposition.
     *
     * \tparam QRMatrixT The type of the \a QR matrix.
     * \tparam TAUMatrixT The type of the \a tau vector.
     * \tparam CMatrixT The type of the \a C matrix.
     *
     * \param QR The matrix obtained by the QR decomposition such that the i-th
     *  column contains the vector which defines the elementary reflector
     *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
     * \param tau The vector obtained by the QR decomposition containing the
     *  scalar factors of the elementary reflectors \f$H(i)\f$, for
     *  \f$i=1,2,\ldots,k\f$.
     * \param left_Q A boolean value indicating which side of the product the
     *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
     *  operand, while a \c false value indicates that \c Q is the right
     *  operand.
     * \param trans_Q A boolean value indicating if the matrix \c Q is to be
     *  transposed. A \c true value indicates that \c Q is to be transposed,
     *  while a \c false value indicates that \c Q is to be taken as-is.
     * \param orientation The matrix orientation fixed to row-major.
     *
     * Let \c Q be the matrix obtained from the QR decomposition represented
     * by the \a QR matrix and the \a tau vector parameters.
     * Then this function computes the following matrix product:
     * \f{equation*}{
     *   \begin{cases}
     *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
     *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
     *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
     *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
     *  \end{cases}
     * \f}
     */
    template <typename QRMatrixT, typename TAUVectorT, typename CMatrixT>
        static void prod(QRMatrixT& QR, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, row_major_tag)
    {
        //NOTE: QR cannot be const since LAPACK::ORMQR modified it, restoring
        //      it at the end of the function.

        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<CMatrixT>::value_type, column_major> tmp_C(C);

        prod(tmp_QR, tau, tmp_C, left_Q, trans_Q, column_major_tag());

        C = tmp_C;
    }

    /**
     * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
     *  the QR decomposition.
     *
     * \tparam QRMatrixT The type of the \a QR matrix.
     * \tparam TAUMatrixT The type of the \a tau vector.
     * \tparam CMatrixT The type of the \a C matrix.
     *
     * \param QR The matrix obtained by the QR decomposition such that the i-th
     *  column contains the vector which defines the elementary reflector
     *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
     * \param tau The vector obtained by the QR decomposition containing the
     *  scalar factors of the elementary reflectors \f$H(i)\f$, for
     *  \f$i=1,2,\ldots,k\f$.
     * \param left_Q A boolean value indicating which side of the product the
     *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
     *  operand, while a \c false value indicates that \c Q is the right
     *  operand.
     * \param trans_Q A boolean value indicating if the matrix \c Q is to be
     *  transposed. A \c true value indicates that \c Q is to be transposed,
     *  while a \c false value indicates that \c Q is to be taken as-is.
     * \param orientation The matrix orientation fixed to column-major.
     *
     * Let \c Q be the matrix obtained from the QR decomposition represented
     * by the \a QR matrix and the \a tau vector parameters.
     * Then this function computes the following matrix product:
     * \f{equation*}{
     *   \begin{cases}
     *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
     *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
     *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
     *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
     *  \end{cases}
     * \f}
     */
    template <typename QRMatrixT, typename TAUVectorT, typename CMatrixT>
        static void prod(QRMatrixT& QR, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, column_major_tag /*orientation*/)
    {
        //NOTE: QR cannot be const since LAPACK::ORMQR modified it, restoring
        //      it at the end of the function.

//      typedef typename matrix_traits<QRMatrixT>::value_type value_type;
//      typedef typename matrix_traits<QRMatrixT>::size_type size_type;
//      typedef typename type_traits<value_type>::real_type real_type;
//
//      const ::fortran_int_t m = num_rows(C);
//      const ::fortran_int_t n = num_columns(C);
//      const ::fortran_int_t k = size(tau);
//      const ::fortran_int_t lda = num_rows(QR);
//      const ::fortran_int_t ldc = m;
//      real_type* work;
//      real_type opt_work_size;
//      ::fortran_int_t lwork;
//      ::std::ptrdiff_t info;

        if (left_Q)
        {
            if (trans_Q)
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::left(),
                    ::boost::numeric::bindings::trans(QR),
                    tau,
                    C
                );
            }
            else
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::left(),
                    QR,
                    tau,
                    C
                );
            }
        }
        else
        {
            if (trans_Q)
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::right(),
                    ::boost::numeric::bindings::trans(QR),
                    tau,
                    C
                );
            }
            else
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::right(),
                    QR,
                    tau,
                    C
                );
            }
        }
    }
};

template <>
struct qr_decomposition_impl<false>: public qr_decomposition_impl_common
{
    /// Extract the Q matrix from a previously computing QR decomposition
    /// (row-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

        extract_Q(tmp_QR, tau, tmp_Q, full, column_major_tag());

        Q = tmp_Q;
    }

    /// Extract the Q matrix from a previously computing QR decomposition
    /// (column-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT& tau, QMatrixT& Q, bool full, column_major_tag)
    {
        typedef typename matrix_traits<QMatrixT>::size_type size_type;
        typedef typename matrix_traits<QMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nc = full ? m : ::std::min(m,n);

        if (num_rows(Q) != m || num_columns(Q) != nc)
        {
            Q.resize(m, nc, false);
        }

        if (m > n)
        {
            if (full)
            {
                subrange(Q, 0, m, 0, n) = QR;
                subrange(Q, 0, m, n, m) = scalar_matrix<value_type>(m, m-n, value_type/*zero*/());
            }
            else
            {
                Q = QR;
            }
        }
        else if (m < n)
        {
            Q = subrange(QR, 0, m, 0, nc);
        }
        else
        {
            Q = QR;
        }

       ::boost::numeric::bindings::lapack::orgqr(Q, tau);
    }
};

template <>
struct qr_decomposition_impl<true>: public qr_decomposition_impl_common
{
    /// Extract the Q matrix from a previously computing QR decomposition
    /// (row-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

        extract_Q(tmp_QR, tau, tmp_Q, full, column_major_tag());

        Q = tmp_Q;
    }

    /// Extract the Q matrix from a previously computing QR decomposition
    /// (column-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, column_major_tag)
    {
        typedef typename matrix_traits<QMatrixT>::size_type size_type;
        typedef typename matrix_traits<QMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nc = full ? m : ::std::min(m,n);

        if (num_rows(Q) != m || num_columns(Q) != nc)
        {
            Q.resize(m, nc, false);
        }

        if (m > n)
        {
            if (full)
            {
                subrange(Q, 0, m, 0, n) = QR;
                subrange(Q, 0, m, n, m) = scalar_matrix<value_type>(m, m-n, value_type/*zero*/());
            }
            else
            {
                Q = QR;
            }
        }
        else if (m < n)
        {
            Q = subrange(QR, 0, m, 0, nc);
        }
        else
        {
            Q = QR;
        }

        ::boost::numeric::bindings::lapack::ungqr(Q, tau);
    }
};

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename QMatrixT, typename RMatrixT, typename OrientationT>
void qr_decompose_impl(matrix_expression<MatrixExprT> const& A, QMatrixT& Q, RMatrixT& R, bool full, OrientationT orientation)
{
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;

    matrix<value_type, typename layout_type<MatrixExprT>::type> tmp_QR(A);
    vector<value_type> tmp_tau;

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template decompose(tmp_QR, tmp_tau, orientation);

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template extract_Q(tmp_QR, tmp_tau, Q, full, orientation);

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template extract_R(tmp_QR, R, full, orientation);
}

} // Namespace detail

/**
 * \brief QR decomposition.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename ValueT>
class qr_decomposition
{
    public: typedef ValueT value_type;
    private: typedef matrix<value_type, column_major> work_matrix_type;
    public: typedef work_matrix_type QR_matrix_type;
    public: typedef work_matrix_type Q_matrix_type;
    public: typedef work_matrix_type R_matrix_type;
    private: typedef vector<value_type> tau_vector_type;

    public: qr_decomposition()
    {
        // empty
    }

    public: template <typename MatrixExprT>
        qr_decomposition(matrix_expression<MatrixExprT> const& A)
        : QR_(A)
    {
        decompose();
    }

    public: template <typename MatrixExprT>
        void decompose(matrix_expression<MatrixExprT> const& A)
    {
        QR_ = A;

        decompose();
    }

    public: Q_matrix_type Q(bool full = true) const
    {
        Q_matrix_type tmp_Q;

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template extract_Q(QR_, tau_, tmp_Q, full, column_major_tag());

        return tmp_Q;
    }

    public: R_matrix_type R(bool full = true) const
    {
        R_matrix_type tmp_R;

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template extract_R(QR_, tmp_R, full, column_major_tag());

        return tmp_R;
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        lprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        rprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        tlprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        trprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$Q C\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type lprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        lprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$C Q\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type rprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        rprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$Q^T C\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type tlprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        tlprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$C Q^T\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type trprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        trprod_inplace(tmp_C);

        return tmp_C;
    }

    private: void decompose()
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template decompose(QR_, tau_, column_major_tag());
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C (column-major
    /// case).
    private: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, C, true, false, column_major_tag());
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C (row-major
    /// case).
    private: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, true, false, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C (column-major
    /// case).
    private: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template prod(QR_, tau_, C, false, false, column_major_tag());
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C (row-major
    /// case).
    private: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, false, false, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C
    /// (column-major case).
    private: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template prod(QR_, tau_, C, true, true, column_major_tag());
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C
    /// (row-major case).
    private: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, true, true, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C
    /// (column-major case).
    private: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, C, false, true, column_major_tag());
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C
    /// (row-major case).
    private: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, false, true, column_major_tag());

        C = tmp_C;
    }

    // NOTE: the 'mutable' keyword is needed in order to make 'const' the
    //       '?prod' methods ('lprod', 'tlprod', 'rprod', 'trprod').
    //       Indeed, these methods call the respective '?prod_inplace' methods
    //       which, in turns, call the LAPACK::ORMQR function which temporarily
    //       changes the QR matrix (and restores it before returning).
    private: mutable QR_matrix_type QR_;
    private: tau_vector_type tau_;
};

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename OutMatrix1T, typename OutMatrix2T>
BOOST_UBLAS_INLINE
void qr_decompose(matrix_expression<MatrixExprT> const& A, OutMatrix1T& Q, OutMatrix2T& R, bool full = true)
{
    typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category1;
    typedef typename matrix_traits<OutMatrix1T>::orientation_category orientation_category2;
    typedef typename matrix_traits<OutMatrix2T>::orientation_category orientation_category3;

    // precondition: same orientation category
    BOOST_MPL_ASSERT(
        (::boost::mpl::and_<
            ::boost::is_same<orientation_category1,orientation_category2>,
            ::boost::is_same<orientation_category1,orientation_category3>
        >)
    );

    detail::qr_decompose_impl(A, Q, R, full, orientation_category1());
}

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT>
BOOST_UBLAS_INLINE
qr_decomposition<typename matrix_traits<MatrixExprT>::value_type> qr_decompose(matrix_expression<MatrixExprT> const& A)
{
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;

    return qr_decomposition<value_type>(A);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP