#include "setup_skinning_handles.h"
#include "kmeans_clustering.h"
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <iostream>
#include <igl/boundary_conditions.h>
#include <igl/lbs_matrix.h>
#include <igl/bbw.h>
#include <igl/harmonic.h>
#include <igl/biharmonic_coordinates.h>
#include <unsupported/Eigen/KroneckerProduct>
#include <igl/slice_into.h>
#include <igl/remove_unreferenced.h>
#include <igl/normalize_row_sums.h>
#include <igl/writeOBJ.h>
#include <igl/list_to_matrix.h>

using namespace Eigen;
using namespace std;
MatrixXd bbw_strain_skinning_matrix(VectorXi& handles, const MatrixXd& mV, MatrixXi& mT){
    std::set<int> unique_vertex_handles;
    std::set<int>::iterator it;
    //from the tet handle indexes, get the unique verts that can act as handles
    for(int i=0; i<handles.size(); i++){
        unique_vertex_handles.insert(mT(handles[i], 0));
        unique_vertex_handles.insert(mT(handles[i], 1));
        unique_vertex_handles.insert(mT(handles[i], 2));
        unique_vertex_handles.insert(mT(handles[i], 3));
    }

    int i=0;
    it = unique_vertex_handles.end();
    VectorXi map_verts_to_unique_verts = VectorXi::Zero(*(--it)+1).array() -1;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        map_verts_to_unique_verts[*it] = i;
        i++;
    }

    MatrixXi vert_to_tet = MatrixXi::Zero(handles.size(), 4);
    i=0;
    for(i=0; i<handles.size(); i++){
        vert_to_tet.row(i)[0] = map_verts_to_unique_verts[mT.row(handles[i])[0]];
        vert_to_tet.row(i)[1] = map_verts_to_unique_verts[mT.row(handles[i])[1]];
        vert_to_tet.row(i)[2] = map_verts_to_unique_verts[mT.row(handles[i])[2]];
        vert_to_tet.row(i)[3] = map_verts_to_unique_verts[mT.row(handles[i])[3]];
    }

    MatrixXd C = MatrixXd::Zero(unique_vertex_handles.size(), 3);
    VectorXi P = VectorXi::Zero(unique_vertex_handles.size());
    VectorXi HandleIndexes = VectorXi::Zero(unique_vertex_handles.size());
    i=0;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        HandleIndexes[i] =  *it;
        C.row(i) = mV.row(*it);
        P(i) = i;
        i++;
    }

    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    cout<<"---------0--------"<<endl;
    igl::boundary_conditions(mV, mT, C, P, MatrixXi(), MatrixXi(), b, bc);
    // compute BBW weights matrix
    igl::BBWData bbw_data;
    // only a few iterations for sake of demo
    bbw_data.active_set_params.max_iter = 100;
    bbw_data.verbosity = 2;

    MatrixXd W, M;
    cout<<"---------1--------"<<endl;
    if(handles.size()==1){
        return MatrixXd::Ones(mT.rows(), handles.size());
    }

    // if(!igl::bbw(mV, mT, b, bc, bbw_data, W))
    // {
    //     std::cout<<"EXIT: Error here"<<std::endl;
    //     exit(0);
    //     return MatrixXd();
    // }
    // if(!igl::harmonic(mV, mT, b, bc, 1, W))
    // {
    //     std::cout<<"EXIT: Error here"<<std::endl;
    //     exit(0);
    //     return MatrixXd();
    // }

    std::vector<std::vector<int> > S;
    igl::matrix_to_list(HandleIndexes,S);

    cout<<"Computing weights for "<<HandleIndexes.size()<<" handles at "<<mV.rows()<<" vertices..."<<endl;
    // Technically k should equal 3 for smooth interpolation in 3d, but 2 is
    // faster and looks OK
    const int k = 3;
    igl::biharmonic_coordinates(mV,mT,S,k,W);

    cout<<"---------2--------"<<endl;

    // Normalize weights to sum to one
    igl::normalize_row_sums(W,W);
    cout<<"---------3--------"<<endl;

    // precompute linear blend skinning matrix
    igl::lbs_matrix(mV,W,M);
    cout<<"---------4--------"<<endl;

    MatrixXd tW = MatrixXd::Zero(mT.rows(), handles.size());
    for(int t =0; t<mT.rows(); t++){
        VectorXi e = mT.row(t);
        for(int h=0; h<handles.size(); h++){
            if(t==handles[h]){
                tW.row(t) *= 0;
                tW(t,h) = 1;
                break;
            }
            double p0 = 0;
            double p1 = 0;
            double p2 = 0;
            double p3 = 0;
            for(int j=0; j<vert_to_tet.cols(); ++j){
                p0 += W(e[0], vert_to_tet(h, j));
                p1 += W(e[1], vert_to_tet(h, j));
                p2 += W(e[2], vert_to_tet(h, j));
                p3 += W(e[3], vert_to_tet(h, j));
            }
            tW(t, h) = (p0+p1+p2+p3)/4;
        }
    }
    cout<<"---------5--------"<<endl;
    igl::normalize_row_sums(tW, tW);

    cout<<"---------6--------"<<endl;
    return tW;
}

Vector3d tet_center(MatrixXi& T, MatrixXd& V, int ind){
    Vector3d v1 = V.row(T.row(ind)[0]);
    Vector3d v2 = V.row(T.row(ind)[1]);
    Vector3d v3 = V.row(T.row(ind)[2]);
    Vector3d v4 = V.row(T.row(ind)[3]);

    return (v1 + v2 + v3 + v4)/4;
}

MatrixXd setup_skinning_helper(int indx,
    int nsh_on_component,
    MatrixXi& mT,
    MatrixXd& mV,
    SparseMatrix<double>& mC,
    SparseMatrix<double>& mA,
    VectorXd& mx0,
    std::map<int, std::vector<int>>& ms_handle_elem_map){

    //---------BBW Skinning Handles ---------
    VectorXi handles_ind = VectorXi::Zero(nsh_on_component);
    VectorXd CAx0 = mC*mA*mx0;

    for(int k=0; k<nsh_on_component; k++){
        std::vector<int> els = ms_handle_elem_map[indx-k];
        double centx = 0;//= VectorXd::Zero(els.size());
        double centy = 0;//= VectorXd::Zero(els.size());
        double centz = 0;//= VectorXd::Zero(els.size());
        Vector3d avg_cent;
        for(int i=0; i<els.size(); i++){
            centx += CAx0[12*els[i]];
            centy += CAx0[12*els[i]+1];
            centz += CAx0[12*els[i]+2];
        }

        avg_cent<<centx/els.size(), centy/els.size(), centz/els.size();
        int minind = 0;
        double mindist = (avg_cent - tet_center(mT, mV, 0)).norm();
        for(int i=1; i<mT.rows(); i++){
            double dist = (avg_cent - tet_center(mT, mV, i)).norm();
            if(dist<mindist){
                mindist = dist;
                minind = i;
            }
        }

        handles_ind[k] = minind;
    }
    return bbw_strain_skinning_matrix(handles_ind, mV, mT);

}

void setup_skinning_handles(int nsh, bool reduced, const MatrixXi& mT, const MatrixXd& mV, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle,
    SparseMatrix<double>& mC, SparseMatrix<double>& mA, MatrixXd& mG, VectorXd& mx0, VectorXd& mred_s, MatrixXd& msW, std::map<int, std::vector<int>>& ms_handle_elem_map, VectorXd& relStiff){

    std::cout<<"+ Skinning Handles"<<std::endl;
    int handles_per_tendon=2;
    VectorXi skinning_elem_cluster_map;
    std::map<int, std::vector<int>> skinning_cluster_elem_map;

    std::cout<<nsh<<std::endl;
    if(nsh==0){
        nsh = mT.rows();
    }

    if(nsh<(ibones.size()+imuscle.size())){
        std::cout<<"Too few skinning handles, too many components"<<std::endl;
        exit(0);
    }

    //-----------------------------------------------------
    if(nsh==mT.rows() && reduced==false){
        //unreduced
        std::cout<<"here"<<std::endl;
        skinning_elem_cluster_map.resize(mT.rows());
        skinning_elem_cluster_map.setZero();
        for(int i=0; i<mT.rows(); i++){
            skinning_elem_cluster_map[i] = i;
        }
    }else{
        kmeans_clustering(skinning_elem_cluster_map, nsh, handles_per_tendon, ibones, imuscle, mG, mC, mA, mx0, relStiff);
    }

    for(int i=0; i<mT.rows(); i++){
        ms_handle_elem_map[skinning_elem_cluster_map[i]].push_back(i);
    }
    //------------------------------------------------------

    if(reduced==false){
        mred_s.resize(6*mT.rows());
        for(int i=0; i<mT.rows(); i++){
            mred_s[6*i+0] = 1;
            mred_s[6*i+1] = 1;
            mred_s[6*i+2] = 1;
            mred_s[6*i+3] = 0;
            mred_s[6*i+4] = 0;
            mred_s[6*i+5] = 0;
        }
        std::cout<<"- Skinning Handles"<<std::endl;
        return;

    }
    if(nsh==mT.rows()){
        std::cout<<"Too many skinning handles. "<<std::endl;
        exit(0);
    }

    mred_s.resize(6*nsh);
    for(int i=0; i<nsh; i++){
        mred_s[6*i+0] = 1;
        mred_s[6*i+1] = 1;
        mred_s[6*i+2] = 1;
        mred_s[6*i+3] = 0;
        mred_s[6*i+4] = 0;
        mred_s[6*i+5] = 0;
    }

    MatrixXd Id6 = MatrixXd::Identity(6, 6);
    MatrixXd sW = MatrixXd::Zero(mT.rows(), nsh);
    sW.setZero();
    std::cout<<"nsh: "<<nsh<<std::endl;
    std::cout<<ms_handle_elem_map.size()<<std::endl;
    // //Constant weights of 1 for each handle region
    // for(int h=0; h<ms_handle_elem_map.size(); h++){
    //   std::vector<int> mshem =  ms_handle_elem_map[ms_handle_elem_map.size() - h - 1];
    //   MatrixXd ones_vec = VectorXd::Ones(mshem.size());
    //   MatrixXd sWslice = MatrixXd::Zero(mT.rows(), 1);
    //   VectorXi handles_elem_vec;
    //   igl::list_to_matrix(mshem, handles_elem_vec);
    //   igl::slice_into(ones_vec , handles_elem_vec, 1, sWslice);
    //   sW.block(0, h, mT.rows(), 1) = sWslice;
    // }
    // msW =  Eigen::kroneckerProduct(sW, Id6);

    // return;

    //blocked construction of full skinning weights matrix
    int maxnsh = nsh;
    cout<<"----------Bone HANDLES------------"<<maxnsh<<"--"<<ms_handle_elem_map.size()<<endl;
    int insert_index = 0;
    for(int b=0; b<ibones.size(); b++){
        MatrixXi subT(ms_handle_elem_map[maxnsh - 1 - insert_index].size(), 4);
        MatrixXi componentT;
        MatrixXd componentV;
        VectorXi J;

        for(int i=0; i<ms_handle_elem_map[maxnsh -1 - insert_index].size() ; i++){
            subT.row(i) = mT.row(ms_handle_elem_map[maxnsh -1 - insert_index][i]);
        }

        igl::remove_unreferenced(mV, subT, componentV, componentT, J);

        std::string nam = "bone"+to_string(b)+".obj";

        igl::writeOBJ(nam, componentV, componentT);

        MatrixXd sWi = setup_skinning_helper(maxnsh-1- insert_index , 1, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());
        igl::slice_into(sWi , ibones[b], 1, sWslice);
        sW.block(0,insert_index, mT.rows(), sWi.cols()) = sWslice;
        nsh = nsh - 1; //bone skinning handle has been made, so decrease nsh by 1
        insert_index += 1;
    }

    cout<<"----------MUSCLE HANDLES----"<<nsh<<"---"<<insert_index<<"-----"<<endl;
    int number_handles_per_muscle = nsh/imuscle.size();
    for(int m=0; m<imuscle.size(); m++){ //through muscle vector
        // std::vector<int> tendon_elements_list;
        // std::vector<int> muscle_elements_list;
        // for(int i=0; i<imuscle[m].size(); i++){
        //     if(relStiff[imuscle[m][i]]>10){
        //         tendon_elements_list.push_back(imuscle[m][i]);
        //     }else{
        //         muscle_elements_list.push_back(imuscle[m][i]);
        //     }
        // }

        VectorXi muscle_els, tendon_els;
        muscle_els = imuscle[m];
        // igl::list_to_matrix(tendon_elements_list, tendon_els);
        // igl::list_to_matrix(muscle_elements_list, muscle_els);

        // //Tendon skinning handles---------------------------
        // if(tendon_els.size()>0){

        //     MatrixXi TcomponentT;
        //     MatrixXd TcomponentV;
        //     MatrixXi TsubT(tendon_els.size(), 4);
        //     VectorXi TJ;
        //     for(int i=0; i<tendon_els.size() ; i++){
        //         TsubT.row(i) = mT.row(tendon_els[i]);
        //     }
        //     igl::remove_unreferenced(mV, TsubT, TcomponentV, TcomponentT, TJ);
        //     std::string Tnam = "tendon"+to_string(m)+".obj";
        //     igl::writeOBJ(Tnam, TcomponentV, TcomponentT);
        //     MatrixXd TsWi;
        //     if(m==imuscle.size() -1){
        //         TsWi = setup_skinning_helper(maxnsh -1 - insert_index - nsh + handles_per_tendon, handles_per_tendon, TcomponentT, TcomponentV, mC, mA, mx0, ms_handle_elem_map);
        //     }else{
        //         TsWi = setup_skinning_helper(maxnsh -1 - insert_index - number_handles_per_muscle + handles_per_tendon, handles_per_tendon, TcomponentT, TcomponentV, mC, mA, mx0, ms_handle_elem_map);
        //     }
        //     MatrixXd TsWslice = MatrixXd::Zero(mT.rows(), TsWi.cols());

        //     igl::slice_into(TsWi , tendon_els, 1, TsWslice);
        //     sW.block(0,insert_index, mT.rows(), TsWi.cols()) = TsWslice;
        // }
        handles_per_tendon=0;

        //Muscle skinning handles---------------------------
        MatrixXi componentT;
        MatrixXd componentV;
        MatrixXi subT(muscle_els.size(), 4);
        VectorXi J;
        for(int i=0; i<muscle_els.size() ; i++){
            subT.row(i) = mT.row(muscle_els[i]);
        }
        igl::remove_unreferenced(mV, subT, componentV, componentT, J);
        std::string nam = "muscle"+to_string(m)+".obj";
        igl::writeOBJ(nam, componentV, componentT);
        MatrixXd sWi;
        if(m==imuscle.size()-1){
            //Deal with remainder handles
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, nsh - handles_per_tendon, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);
        }else{
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, number_handles_per_muscle - handles_per_tendon, componentT, componentV, mC, mA, mx0, ms_handle_elem_map );
        }

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());

        igl::slice_into(sWi , muscle_els, 1, sWslice);
        sW.block(0,insert_index+handles_per_tendon, mT.rows(), sWi.cols()) = sWslice;
        insert_index += number_handles_per_muscle;
        nsh =  nsh - number_handles_per_muscle;
    }
    assert(nsh==0);

    // MatrixXd Id6 = MatrixXd::Identity(6, 6);
    msW =  Eigen::kroneckerProduct(sW, Id6);

    std::cout<<"- Skinning Handles"<<std::endl;
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Transformation.hpp"
#include "Point3d.hpp"
#include "Vector3d.hpp"
#include "Plane.hpp"
#include "BoundingBox.hpp"
#include "EulerAngles.hpp"
#include "Geometry.hpp"
#include "../core/Assert.hpp"

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

#include <algorithm>

#include <math.h>

using boost::numeric::ublas::identity_matrix;
using std::min;

namespace openstudio{

  /// default constructor creates identity transformation
  Transformation::Transformation()
    : m_storage(identity_matrix<double>(4))
  {}

  /// copy constructor
  Transformation::Transformation(const Transformation& other)
    : m_storage(other.m_storage)
  {}

  /// constructor from storage, asserts matrix is 4x4
  Transformation::Transformation(const Matrix& matrix)
    : m_storage(matrix)
  {
    OS_ASSERT(matrix.size1() == 4);
    OS_ASSERT(matrix.size2() == 4);
  }

  /// constructor from storage, asserts vector is size 16
  Transformation::Transformation(const Vector& vector)
    : m_storage(Matrix(4,4))
  {
    OS_ASSERT(vector.size() == 16);

    m_storage(0,0) = vector[0];
    m_storage(1,0) = vector[1];
    m_storage(2,0) = vector[2];
    m_storage(3,0) = vector[3];
    m_storage(0,1) = vector[4];
    m_storage(1,1) = vector[5];
    m_storage(2,1) = vector[6];
    m_storage(2,1) = vector[7];
    m_storage(0,2) = vector[8];
    m_storage(1,2) = vector[9];
    m_storage(2,2) = vector[10];
    m_storage(3,2) = vector[11];
    m_storage(0,3) = vector[12];
    m_storage(1,3) = vector[13];
    m_storage(2,3) = vector[14];
    m_storage(3,3) = vector[15];
  }

  /// rotation about origin defined by axis and angle (radians)
  Transformation Transformation::rotation(const Vector3d& axis, double radians)
  {
    Matrix storage = identity_matrix<double>(4);

    Vector3d temp = axis;
    if (!temp.normalize()){
      LOG(Error, "Could not normalize axis");
    }
    Vector normalVector = temp.vector();

    // Rodrigues' rotation formula / Rotation matrix from Euler axis/angle
    // I*cos(radians) + I*(1-cos(radians))*axis*axis^T + Q*sin(radians)
    // Q = [0, -axis[2], axis[1]; axis[2], 0, -axis[0]; -axis[1], axis[0], 0]
    Matrix P = outer_prod(normalVector, normalVector);
    Matrix I = identity_matrix<double>(3);
    Matrix Q(3, 3, 0.0);
    Q(0,1) = -normalVector(2);
    Q(0,2) =  normalVector(1);
    Q(1,0) =  normalVector(2);
    Q(1,2) = -normalVector(0);
    Q(2,0) = -normalVector(1);
    Q(2,1) =  normalVector(0);

    // rotation matrix
    Matrix R = I*cos(radians) + (1-cos(radians))*P + Q*sin(radians);

    for (unsigned i = 0; i < 3; ++i){
      for (unsigned j = 0; j < 3; ++j){
        storage(i,j) = R(i,j);
      }
    }

    return Transformation(storage);
  }

  /// rotation about point defined by axis and angle (radians)
  Transformation Transformation::rotation(const Point3d& origin, const Vector3d& axis, double radians)
  {
    Vector3d temp(origin.x(), origin.y(), origin.z());

    // translate point to origin, rotate, and then translate back
    return translation(temp)*rotation(axis, radians)*translation(-temp);
  }

  /// rotation specified by Euler angles
  Transformation Transformation::rotation(const EulerAngles& angles)
  {
    Transformation result = Transformation::rotation(Vector3d(0,0,1), angles.phi()) *
                            Transformation::rotation(Vector3d(0,1,0), angles.theta()) *
                            Transformation::rotation(Vector3d(1,0,0), angles.psi());
    return result;
  }

  /// translation along vector
  Transformation Transformation::translation(const Vector3d& translation)
  {
    Matrix storage = identity_matrix<double>(4);

    storage(0,3) = translation.x();
    storage(1,3) = translation.y();
    storage(2,3) = translation.z();

    return Transformation(storage);
  }

  /// transforms system with z' to regular system
  /// will try to align y' with z, but if that fails will align y' with y
  Transformation Transformation::alignZPrime(const Vector3d& zPrime)
  {
    Vector3d xp;
    Vector3d yp;
    Vector3d zp = zPrime;
    if (!zp.normalize()){
      LOG(Error, "Could not normalize zPrime");
    }

    Vector3d xAxis(1,0,0);
    Vector3d yAxis(0,1,0);
    Vector3d zAxis(0,0,1);
    Vector3d negXAxis(-1,0,0);

    // check if face normal is up or down
    if (fabs(zp.dot(zAxis)) < 0.99){
      // not facing up or down, set yPrime along zAxis
      yp = zAxis - (zp.dot(zAxis)*zp);
      if (!yp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      xp = yp.cross(zp);
    }else{
      // facing up or down, set xPrime along -xAxis
      xp = negXAxis - (zp.dot(negXAxis)*zp);
      if (!xp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      yp = zp.cross(xp);
    }

    Matrix storage = identity_matrix<double>(4);
    storage(0,0) = xp.x();
    storage(1,0) = xp.y();
    storage(2,0) = xp.z();
    storage(0,1) = yp.x();
    storage(1,1) = yp.y();
    storage(2,1) = yp.z();
    storage(0,2) = zp.x();
    storage(1,2) = zp.y();
    storage(2,2) = zp.z();

    return Transformation(storage);
  }

  /// transforms face coordinates to regular system, face normal will be z'
  /// will try to align y' with z, but if that fails will align y' with y
  /// face origin will be minimum point in x', y' and z'=0
  /// will return identity transformation if cannot compute plane for vertices
  Transformation Transformation::alignFace(const std::vector<Point3d>& vertices)
  {
    OptionalVector3d zPrime = getOutwardNormal(vertices);
    if (!zPrime){
      LOG(Error, "Cannot compute outward normal for vertices");
      return Transformation();
    }

    // align z' with outward normal
    Transformation align = alignZPrime(*zPrime);
    Point3dVector alignedVertices = align.inverse()*vertices;

    // compute translation to minimum in aligned system
    double minX = alignedVertices[0].x();
    double minY = alignedVertices[0].y();
    double minZ = alignedVertices[0].z();

    for (const Point3d& vertex : alignedVertices){
      minX = min(minX, vertex.x());
      minY = min(minY, vertex.y());
      minZ = min(minZ, vertex.z());
    }
    Transformation translate = translation(Vector3d(minX, minY, minZ));

    return align*translate;
  }

  /// returns a transformation which is the inverse of this
  Transformation Transformation::inverse() const
  {
    Matrix matrix(4,4);
    bool test = invert(m_storage, matrix);
    if (!test){
      // this should never happen
      LOG_AND_THROW("Matrix inversion failed");
    }
    return Transformation(matrix);
  }

  /// get the matrix representation directly
  Matrix Transformation::matrix() const
  {
    return m_storage;
  }

  /// get the vector representation directly
  Vector Transformation::vector() const
  {
    openstudio::Vector result(16);
    result[0] = m_storage(0,0);
    result[1] = m_storage(1,0);
    result[2] = m_storage(2,0);
    result[3] = m_storage(3,0);
    result[4] = m_storage(0,1);
    result[5] = m_storage(1,1);
    result[6] = m_storage(2,1);
    result[7] = m_storage(3,1);
    result[8] = m_storage(0,2);
    result[9] = m_storage(1,2);
    result[10] = m_storage(2,2);
    result[11] = m_storage(3,2);
    result[12] = m_storage(0,3);
    result[13] = m_storage(1,3);
    result[14] = m_storage(2,3);
    result[15] = m_storage(3,3);
    return result;
  }

  /// get the Euler angles for the transformation, does not include translation
  EulerAngles Transformation::eulerAngles() const
  {
    double psi;
    double theta;
    double phi;
    if (m_storage(2,0) == 1.0){
      phi = 0;
      theta = -boost::math::constants::pi<double>()/2.0;
      psi = atan2(-m_storage(0,1), -m_storage(0,2));
    }else if(m_storage(2,0) == -1.0){
      phi = 0;
      theta = boost::math::constants::pi<double>()/2.0;
      psi = atan2(m_storage(0,1), m_storage(0,2));
    }else{
      theta = -asin(m_storage(2,0));
      // theta = pi + asin(m_storage(2,0)); // alternate solution
      psi = atan2(m_storage(2,1)/cos(theta), m_storage(2,2)/cos(theta));
      phi = atan2(m_storage(1,0)/cos(theta), m_storage(0,0)/cos(theta));

    }
    EulerAngles result(psi, theta, phi);
    return result;
  }

  /// get the rotation matrix for the transformation, does not include translation
  Matrix Transformation::rotationMatrix() const
  {
    Matrix result(3,3);
    for(unsigned i = 0 ; i < 3; ++i){
      for(unsigned j = 0; j < 3; ++j){
        result(i,j) = m_storage(i,j);
      }
    }
    return result;
  }

  /// get the translation for the transformation, does not include rotation
  Vector3d Transformation::translation() const
  {
    Vector3d result(m_storage(0, 3), m_storage(1, 3), m_storage(2, 3));
    return result;
  }

  /// apply the transformation to the point
  Point3d Transformation::operator*(const Point3d& point) const
  {
    Vector temp(4);
    temp(0) = point.x();
    temp(1) = point.y();
    temp(2) = point.z();
    temp(3) = 1.0;
    temp = prod(m_storage, temp);
    return Point3d(temp[0], temp[1], temp[2]);
  }

  /// apply the transformation to the vector
  Vector3d Transformation::operator*(const Vector3d& vector) const
  {
    Vector temp(4);
    temp(0) = vector.x();
    temp(1) = vector.y();
    temp(2) = vector.z();
    temp(3) = 1.0;
    temp = prod(m_storage, temp);
    return Vector3d(temp[0], temp[1], temp[2]);
  }

  /// apply the transformation to the BoundingBox
  BoundingBox Transformation::operator*(const BoundingBox& boundingBox) const
  {
    BoundingBox result;
    std::vector<Point3d> transformedPoints = (*this)*boundingBox.corners();
    result.addPoints(transformedPoints);
    return result;
  }

  /// apply the transformation to the plane
  Plane Transformation::operator*(const Plane& plane) const
  {
    // translate a point on the plane, just project (0,0,0)
    Point3d point = plane.project(Point3d(0,0,0));

    // get a point at outward normal
    Vector3d outwardNormal = plane.outwardNormal();
    Point3d refPoint = point + outwardNormal;

    // translate the two points and recompute the normal
    Point3d newPoint = (*this) * point;
    Point3d newRefPoint = (*this) * refPoint;
    Vector3d newNormal = newRefPoint - newPoint;

    return Plane(newPoint, newNormal);
  }

  /// apply the transformation to a vector of points
  std::vector<Point3d> Transformation::operator*(const std::vector<Point3d>& points) const
  {
    std::vector<Point3d> result(points.size());
    for(unsigned i = 0; i < points.size(); ++i){
      result[i] = (*this)*points[i];
    }
    return result;
  }

  /// apply the transformation to a vector of vector
  std::vector<Vector3d> Transformation::operator*(const std::vector<Vector3d>& vectors) const
  {
    std::vector<Vector3d> result(vectors.size());
    for(unsigned i = 0; i < vectors.size(); ++i){
      result[i] = (*this)*vectors[i];
    }
    return result;
  }

  /// apply the transformation to the other transformation
  Transformation Transformation::operator*(const Transformation& other) const
  {
    return Transformation(prod(m_storage, other.m_storage));
  }

  /// ostream operator
  std::ostream& operator<<(std::ostream& os, const Transformation& t)
  {
    os << t.matrix();
    return os;
  }

  Transformation createRotation(const Vector3d& axis, double radians)
  {
    return Transformation::rotation(axis, radians);
  }

  Transformation createRotation(const Point3d& origin, const Vector3d& axis, double radians)
  {
    return Transformation::rotation(origin, axis, radians);
  }

  Transformation createRotation(const EulerAngles& angles)
  {
    return Transformation::rotation(angles);
  }

  Transformation createTranslation(const Vector3d& translation)
  {
    return Transformation::translation(translation);
  }

} // openstudio


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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.
*/

#include <vector>
#include <limits>
#include <boost/algorithm/minmax_element.hpp>
#include <Core/Math/Histogram.h>
#include <Core/Math/MiscMath.h>

using namespace SCIRun::Core::Math;

namespace
{
  double Nan()
  {
    return std::numeric_limits<double>::quiet_NaN();
  }
}

Histogram::Histogram()
{
  this->min_ = Nan();
  this->max_ = Nan();
  this->bin_start_ = Nan();
  this->bin_size_ = Nan();
  this->histogram_.resize( 0 );
}

Histogram::Histogram( const double* data, size_t size )
{
  this->compute( data, size );
}

// For char and short data we do a single pass over the data to speed up the computation. The
// first pass is used to generate the histogram. In the next step we use this data to compute
// min and max
bool Histogram::compute( const double* data, size_t size )
{
  this->min_ = Nan();
  this->max_ = Nan();
  this->bin_start_ = Nan();
  this->bin_size_ = Nan();
  this->histogram_.resize( 0 );
  if ( size == 0 )
    return false;

  try
  {
    this->min_ = std::numeric_limits<double>::max();
    this->max_ = std::numeric_limits<double>::min();

    for ( size_t j = 0 ; j < size ; j++ )
    {
      double val = data[ j ];
      if ( ! IsFinite( val ) ) continue;
      if ( val < this->min_ ) this->min_ = val;
      if ( val > this->max_ ) this->max_ = val;
    }

    if ( this->min_ > this->max_ )
    {
      // Most likely all the data is NaN
      this->min_ = Nan();
      this->max_ = Nan();
      this->bin_start_ = Nan();
      this->bin_size_ = Nan();
      this->histogram_.resize( 0 );
      return false;
    }

    if ( this->min_ == this->max_ )
    {
      this->bin_size_  = 1.0;
      this->bin_start_ = this->min_ - ( this->bin_size_ * 0.5 );
      this->histogram_.resize( 1, 0 );
    }
    else
    {
      size_t hist_size = 0x100;
      this->histogram_.resize( hist_size, 0 );
      this->bin_size_ = ( this->max_ - this->min_ ) / static_cast<double>( hist_size - 1 );
      this->bin_start_ = this->min_ - ( this->bin_size_ * 0.5 );
    }

    double inv_bin_size = 1.0 / bin_size_;

    for ( size_t j = 1 ; j < size ; j++ )
    {
      double val = data[ j ];
      if ( IsFinite( val ) )
      {
        size_t idx = static_cast<size_t>( ( val - this->min_ ) * inv_bin_size );
        this->histogram_[ idx ]++;
      }
    }

    auto min_max = boost::minmax_element( this->histogram_.begin(), this->histogram_.end() );
    this->min_bin_ = (*min_max.first);
    this->max_bin_ = (*min_max.second);
  }
  catch( ... )
  {
    this->min_ = Nan();
    this->max_ = Nan();
    this->bin_start_ = Nan();
    this->bin_size_ = Nan();
    this->histogram_.resize( 0 );
    return false;
  }

  return true;
}

double Histogram::get_min() const
{
  return this->min_;
}

double Histogram::get_max() const
{
  return this->max_;
}

double Histogram::get_cum_value( double fraction ) const
{
  size_t tot_hist = 0;
  for ( size_t j = 0; j < this->histogram_.size(); j++ )
  {
    tot_hist += this->histogram_[ j ];
  }

  double multiplier = 1.0 / static_cast<double>( tot_hist );
  double jj = 0.0;
  size_t cur_hist = 0;

  for ( size_t j = 0; j < this->histogram_.size(); j++ )
  {
    double frac_start = static_cast<double>( cur_hist ) * multiplier;
    cur_hist += this->histogram_[ j ];
    double frac_end = static_cast<double>( cur_hist ) * multiplier;
    if ( fraction > frac_start && fraction <= frac_end )
    {
      jj = static_cast<double>( j ) + ( frac_end - fraction )/( frac_end - frac_start );
      break;
    }
  }

  return this->bin_start_ + ( jj * this->bin_size_ );
}

size_t Histogram::get_max_bin() const
{
  return this->max_bin_;
}

size_t Histogram::get_min_bin() const
{
  return this->min_bin_;
}

double Histogram::get_bin_size() const
{
  return this->bin_size_;
}

double Histogram::get_bin_start( size_t idx ) const
{
  return this->bin_start_ + idx * this->bin_size_;
}

double Histogram::get_bin_end( size_t idx ) const
{
  return this->bin_start_ + ( idx + 1 ) * this->bin_size_;
}

const std::vector<size_t>& Histogram::get_bins() const
{
  return this->histogram_;
}

size_t Histogram::get_size() const
{
  return this->histogram_.size();
}

bool Histogram::is_valid() const
{
  return !( IsNan( this->min_ ) || IsNan( this->max_ ) );
}


/*
Copyright (c) 2012-2018, Visillect Service LLC. All rights reserved.
Developed for Kharkevich Institute for Information Transmission Problems of the
              Russian Academy of Sciences (IITP RAS).

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.

   2. 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.

THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDERS "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 COPYRIGHT HOLDERS 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.

The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of copyright holders.
*/

#include <cassert>
#include <cmath>
#include <algorithm>

#include <colorseg/color_vertex.h>

THIRDPARTY_INCLUDES_BEGIN
#include <Eigen/Dense>
THIRDPARTY_INCLUDES_END

namespace vi { namespace colorseg {

inline Eigen::Vector3d homographyInv(Eigen::Vector3d const & src, double a, double k)
{
  double EPS = 1.e-5;
  assert(k > -EPS);
  assert(a > -EPS);
  assert(a < 1 + EPS);

  Eigen::Vector4d src_vec(src[0] / 255., src[1] / 255., src[2] / 255., 1);

  Eigen::Matrix<double, 4, 4> H = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    H(3, i) = k;

  Eigen::Matrix<double, 4, 4> S = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    S(i, i) = k + 1;

  Eigen::Matrix<double, 4, 4> A = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
        if (i != j)
            A(i, j) = a;

  Eigen::Matrix<double, 4, 4> P = A * S * H;
  Eigen::Vector4d dst_vec = P.inverse() * src_vec;

  if (std::abs(dst_vec[3]) >= EPS)
  {
    for (int i = 0; i < 3; i++)
      dst_vec[i] = (dst_vec[i] / dst_vec[3]) * 255;
  }

  return Eigen::Vector3d(dst_vec[0], dst_vec[1], dst_vec[2]);
}

inline void homography(double * dst, uint8_t const * src, double a, double k)
{
  double EPS = 1.e-5;
  assert(k > -EPS);
  assert(a > -EPS);
  assert(a < 1 + EPS);

  Eigen::Vector4d src_vec((double)src[0] / 255., (double)src[1] / 255., (double)src[2] / 255., 1);

  Eigen::Matrix<double, 4, 4> H = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    H(3, i) = k;

  Eigen::Matrix<double, 4, 4> S = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    S(i, i) = k + 1;

  Eigen::Matrix<double, 4, 4> A = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
        if (i != j)
            A(i, j) = a;

  Eigen::Matrix<double, 4, 4> P = A * S * H;
  Eigen::Vector4d dst_vec = P * src_vec;

  if (std::abs(dst_vec[3]) >= EPS)
  {
    for (int i = 0; i < 3; i++)
      dst_vec[i] = (dst_vec[i] / dst_vec[3]) * 255;
  }

  dst[0] = dst_vec[0];
  dst[1] = dst_vec[1];
  dst[2] = dst_vec[2];
}

}} // ns vi::colorseg


 #ifndef REDDM_H
#define REDDM_H
#include"basis.hpp"
#include <Eigen/Dense>
#include"diag.h"
namespace Many_Body{
  template<typename T>
  using MatrixD =Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  template<typename T>
  using VectorD =Eigen::Matrix<T,  Eigen::Dynamic, 1>;
  template<typename T>
  inline T conj(T a)
  {return a;}

    template<typename T>
  inline std::complex< T > conj(std::complex< T > a)
  { std::complex< T > b(real(a), -imag(a));
    return b;}

  template<typename TotalBasis, typename Matrix, typename Vector>
  std::vector<Eigen::VectorXd> makeThermalRDMTP(Matrix& H, Vector& ev, TotalBasis& TP, double T, bool isDiag=false, int site=0)
{
  double beta=1./T;
  if(!isDiag)
    {
  diagMat(H, ev);
    }
  std::cout<< "GS "<< ev[0]<<std::endl;
  Eigen::MatrixXd rho=Eigen::MatrixXd::Zero(H.rows(), H.rows());
  double Z{0};
  //  std::cout<< ev << std::endl;
  for(int i=0; i<rho.rows(); i++)
    {
      rho(i, i)=std::exp(-beta*ev(i));
      Z+=std::exp(-beta*ev(i));
    }
  std::cout<< "Z "<<std::setprecision(9)<< Z << std::endl;
  rho/=Z;
  //  std::cout<< rho<< std::endl;

  rho=rho*H.adjoint();
  rho=H*rho;
  std::vector<Eigen::VectorXd> evs;
  auto RDM= makeRedDMTP(TP, TP.rbasis, site, rho);
  Eigen::MatrixXd S=Eigen::MatrixXd::Zero(RDM[0].rows(), RDM[0].rows());
  for(auto& M : RDM){

  Eigen::VectorXd evRDM=Eigen::VectorXd::Zero(M.rows());
  std::cout<< "matri x"<< std::endl;
  std::cout<< M<< std::endl;
  S+=M;
  std::cout<< std::endl;
  diagMat(M, evRDM);
  evs.push_back(evRDM);

  }
    std::cout<< "SUM OF all"<< std::endl;
  std::cout<< S<< std::endl;
    std::cout<< ""<< std::endl;

  return evs;
}
template<typename Basis, typename T>
void makeRedDM(Basis& basis, int site, MatrixD<T>& rho )
{
  size_t dim=basis.maxParticles+1;
  MatrixD<T> DM=MatrixD<T>::Zero(dim, dim);
  for(auto it= basis.begin(); it!=basis.end(); ++it)
    {
      //      std::cout<< " loop 1 "<<std::endl;
      auto currentID1 =Id(*it);
      auto p1=basis.particlesAt(currentID1, site);
      DM(p1, p1)+=rho(Position(*it), Position(*it));
      auto itx=it;
      itx++;
      for(auto it2= itx; it2!=basis.end(); ++it2)
    {
      //            std::cout<< " loop 2 "<<std::endl;

      auto currentID2 =Id(*it2);
       auto state1=GetLattice(*it).makeStateVec();
       auto state2=GetLattice(*it2).makeStateVec();
       auto p2=basis.particlesAt(currentID2, site);
       state1.erase(state1.begin()+site);
       state2.erase(state2.begin()+site);
       if(state1==state2)
	 {
      DM(p1, p2)+=rho(Position(*it), Position(*it2));
      DM(p2, p1)+=rho(Position(*it2), Position(*it));
      // std::cout << currentID1 << " part a 1 " << p1 << std::endl;
      //       std::cout << currentID2 << " part a 2 " << p2 << std::endl;
	 }
    }
    }

  std::cout<< DM << std::endl;
}
  template<typename Basis, typename T>
  void makeRedDM(Basis& basis, int site, VectorD<T>& psi)
{
  size_t dim=basis.maxParticles+1;

  MatrixD<T> DM=MatrixD<T>::Zero(dim, dim);
  for(auto it= basis.begin(); it!=basis.end(); ++it)
    {
      //      std::cout<< " loop 1 "<<std::endl;
      auto currentID1 =Id(*it);
      auto p1=basis.particlesAt(currentID1, site);
      DM(p1, p1)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it)));
	      //rho(Position(*it), Position(*it));
      auto itx=it;
      itx++;
      for(auto it2= itx; it2!=basis.end(); ++it2)
    {
      //            std::cout<< " loop 2 "<<std::endl;

      auto currentID2 =Id(*it2);
       auto state1=GetLattice(*it).makeStateVec();
       auto state2=GetLattice(*it2).makeStateVec();
       auto p2=basis.particlesAt(currentID2, site);
       state1.erase(state1.begin()+site);
       state2.erase(state2.begin()+site);
       if(state1==state2)
	 {
	   DM(p1, p2)+=Many_Body::conj(static_cast<T>((psi(Position(*it2)))))*(psi(Position(*it)));
	//rho(Position(*it), Position(*it2));

	   DM(p2, p1)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it2)));
	//rho(Position(*it2), Position(*it));
      // std::cout << currentID1 << " part a 1 " << p1 << std::endl;
      //       std::cout << currentID2 << " part a 2 " << p2 << std::endl;
	 }
    }
    }

  std::cout<< DM << std::endl;
}

template<typename TotalBasis, class SubBasis, typename T>
std::vector<MatrixD<T>> makeRedDMTP(const TotalBasis& totalBasis, const SubBasis& subBasis, int site, MatrixD<T>& rho )
{
  // making reduced DM for each degree of freedom in subasis L

       std::vector<MatrixD<T>> mats(totalBasis.lbasis.maxParticles+1);
       size_t dim=(totalBasis.rbasis.maxParticles+1);

       std::fill(mats.begin(), mats.end(), MatrixD<T>::Zero(dim, dim));

       //  std::cout<< dim << std::endl;

  for(auto it= totalBasis.begin(); it!=totalBasis.end(); ++it)
    {
      auto itL=totalBasis.lbasis.find(LeftId(*it));
      auto itR=totalBasis.rbasis.find(RightId(*it));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID1L =Id(*itL);
       auto currentID1R =Id(*itR);
       auto p1L=totalBasis.lbasis.particlesAt(currentID1L, site);
       auto p1R=totalBasis.rbasis.particlesAt(currentID1R, site);
       // unsafe way to determine the position in the matrix
       // upper first fix electron then iterate boson
       size_t newPos= p1R;

       //       if(std::abs(rho(Position(*it), Position(*it)))>0.001){std::cout<< " her "<< p1L<< "  " << p1R << std::endl;}
       auto itx=it;
       // itx++;
       for(auto it2= itx; it2!=totalBasis.end(); ++it2)
     {
         auto it2L=totalBasis.lbasis.find(LeftId(*it2));
      auto it2R=totalBasis.rbasis.find(RightId(*it2));

      //      std::cout<< " loop 1 "<<std::endl; // re think this one
       auto currentID2L =Id(*it2L);
       auto currentID2R =Id(*it2R);
       auto p2L=totalBasis.lbasis.particlesAt(currentID2L, site);
       auto p2R=totalBasis.rbasis.particlesAt(currentID2R, site);
       size_t newPos2=p2R;
       // making the delta function
       auto state1L=GetLattice(*itL).makeStateVec();
       auto state1R=GetLattice(*itR).makeStateVec();
       auto state2L=GetLattice(*it2L).makeStateVec();
       auto state2R=GetLattice(*it2R).makeStateVec();
       state1L.erase(state1L.begin()+site);
       state2L.erase(state2L.begin()+site);
       state1R.erase(state1R.begin()+site);
       state2R.erase(state2R.begin()+site);
       if(state1L==state2L and state1R==state2R)
	 {
	   //if(p2L!=p1L){std::cout << "here "<< std::endl;}
	   if(newPos!=newPos2){
       mats[p2L](newPos, newPos2)+=rho(Position(*it), Position(*it2));
       mats[p2L](newPos2, newPos)+=rho(Position(*it2), Position(*it));
	   }
	   else{
	     mats[p1L](newPos, newPos)+=rho(Position(*it), Position(*it2));
	   }
	 }
         }
}
  return mats;
}

  template<typename TotalBasis, class SubBasis, typename T>
std::vector<MatrixD<T>> makeRedDMTP(const TotalBasis& totalBasis, const SubBasis& subBasis, int site, VectorD<T>& psi )
{
  // making reduced DM for each degree of freedom in subasis L

       std::vector<MatrixD<T>> mats(totalBasis.lbasis.maxParticles+1);
       size_t dim=(totalBasis.rbasis.maxParticles+1);

       std::fill(mats.begin(), mats.end(), MatrixD<T>::Zero(dim, dim));

       //  std::cout<< dim << std::endl;

  for(auto it= totalBasis.begin(); it!=totalBasis.end(); ++it)
    {
      auto itL=totalBasis.lbasis.find(LeftId(*it));
      auto itR=totalBasis.rbasis.find(RightId(*it));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID1L =Id(*itL);
       auto currentID1R =Id(*itR);
       auto p1L=totalBasis.lbasis.particlesAt(currentID1L, site);
       auto p1R=totalBasis.rbasis.particlesAt(currentID1R, site);
       // unsafe way to determine the position in the matrix
       // upper first fix electron then iterate boson
       size_t newPos= p1R;
       mats[p1L](newPos, newPos)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it)));

       //       if(std::abs(rho(Position(*it), Position(*it)))>0.001){std::cout<< " her "<< p1L<< "  " << p1R << std::endl;}
       auto itx=it;
       itx++;
       for(auto it2= itx; it2!=totalBasis.end(); ++it2)
     {
         auto it2L=totalBasis.lbasis.find(LeftId(*it2));
      auto it2R=totalBasis.rbasis.find(RightId(*it2));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID2L =Id(*it2L);
       auto currentID2R =Id(*it2R);
       auto p2L=totalBasis.lbasis.particlesAt(currentID2L, site);
       auto p2R=totalBasis.rbasis.particlesAt(currentID2R, site);
       size_t newPos2=p2R;
       // making the delta function
       auto state1L=GetLattice(*itL).makeStateVec();
       auto state1R=GetLattice(*itR).makeStateVec();
       auto state2L=GetLattice(*it2L).makeStateVec();
       auto state2R=GetLattice(*it2R).makeStateVec();
       state1L.erase(state1L.begin()+site);
       state2L.erase(state2L.begin()+site);
       state1R.erase(state1R.begin()+site);
       state2R.erase(state2R.begin()+site);
       if(state1L==state2L and state1R==state2R)
	 {
	   mats[p2L](newPos, newPos2)+=Many_Body::conj(static_cast<T>((psi(Position(*it2)))))*(psi(Position(*it)));

	 //rho(Position(*it), Position(*it2));
	   mats[p2L](newPos2, newPos)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it2)));
	 //rho(Position(*it2), Position(*it));

	 }
         }
}
  return mats;
}

}

#endif /* REDDM_H */


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2008 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Daniel Arndt, Matthias Maier, 2015
 *
 * Based on step-22 by Wolfgang Bangerth and Martin Kronbichler
 */

// 这个例子程序是对 step-22 的轻微修改，使用Trilinos并行运行，以演示交易.II中周期性边界条件的使用。因此我们不讨论大部分的源代码，只对处理周期性约束的部分进行评论。其余的请看 step-22 和底部的完整源代码。

// 为了实现周期性边界条件，只有两个函数需要修改。

// -  StokesProblem<dim>::setup_dofs()  : 用周期性约束来填充AffineConstraints对象

// -  StokesProblem<dim>::create_mesh()  : 为分布式三角形提供周期性信息。

// 程序的其余部分与 step-22 相同，所以让我们跳过这一部分，只在下面显示这两个函数。完整的程序可以在下面的 "普通程序 "部分找到）。

//  @cond  跳过

#include <deal.II/base/conditional_ostream.h>

#include <deal.II/distributed/grid_refinement.h>

#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/lac/trilinos_solver.h>
#include <deal.II/lac/trilinos_precondition.h>
#include <deal.II/lac/trilinos_block_sparse_matrix.h>
#include <deal.II/lac/trilinos_parallel_block_vector.h>
#include <deal.II/lac/block_sparsity_pattern.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>

#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/mapping_q.h>

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

namespace Step45
{
  using namespace dealii;

  template <int dim>
  class StokesProblem
  {
  public:
    StokesProblem(const unsigned int degree);
    void run();

  private:
    void create_mesh();
    void setup_dofs();
    void assemble_system();
    void solve();
    void output_results(const unsigned int refinement_cycle) const;
    void refine_mesh();

    const unsigned int degree;

    MPI_Comm mpi_communicator;

    parallel::distributed::Triangulation<dim> triangulation;
    FESystem<dim>                             fe;
    DoFHandler<dim>                           dof_handler;

    AffineConstraints<double> constraints;
    std::vector<IndexSet>     owned_partitioning;
    std::vector<IndexSet>     relevant_partitioning;

    TrilinosWrappers::BlockSparseMatrix system_matrix;

    TrilinosWrappers::BlockSparseMatrix preconditioner_matrix;

    TrilinosWrappers::MPI::BlockVector solution;
    TrilinosWrappers::MPI::BlockVector system_rhs;

    ConditionalOStream pcout;

    MappingQ<dim> mapping;
  };

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    BoundaryValues()
      : Function<dim>(dim + 1)
    {}

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

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> & /*p*/,
                                    const unsigned int component) const
  {
    (void)component;
    Assert(component < this->n_components,
           ExcIndexRange(component, 0, this->n_components));

    return 0;
  }

  template <int dim>
  void BoundaryValues<dim>::vector_value(const Point<dim> &p,
                                         Vector<double> &  values) const
  {
    for (unsigned int c = 0; c < this->n_components; ++c)
      values(c) = BoundaryValues<dim>::value(p, c);
  }

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide()
      : Function<dim>(dim + 1)
    {}

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

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    const Point<dim> center(0.75, 0.1);
    const double     r = (p - center).norm();

    if (component == 0)
      return std::exp(-100. * r * r);
    return 0;
  }

  template <int dim>
  void RightHandSide<dim>::vector_value(const Point<dim> &p,
                                        Vector<double> &  values) const
  {
    for (unsigned int c = 0; c < this->n_components; ++c)
      values(c) = RightHandSide<dim>::value(p, c);
  }

  template <class MatrixType, class PreconditionerType>
  class InverseMatrix : public Subscriptor
  {
  public:
    InverseMatrix(const MatrixType &        m,
                  const PreconditionerType &preconditioner,
                  const IndexSet &          locally_owned,
                  const MPI_Comm &          mpi_communicator);

    void vmult(TrilinosWrappers::MPI::Vector &      dst,
               const TrilinosWrappers::MPI::Vector &src) const;

  private:
    const SmartPointer<const MatrixType>         matrix;
    const SmartPointer<const PreconditionerType> preconditioner;

    const MPI_Comm *                      mpi_communicator;
    mutable TrilinosWrappers::MPI::Vector tmp;
  };

  template <class MatrixType, class PreconditionerType>
  InverseMatrix<MatrixType, PreconditionerType>::InverseMatrix(
    const MatrixType &        m,
    const PreconditionerType &preconditioner,
    const IndexSet &          locally_owned,
    const MPI_Comm &          mpi_communicator)
    : matrix(&m)
    , preconditioner(&preconditioner)
    , mpi_communicator(&mpi_communicator)
    , tmp(locally_owned, mpi_communicator)
  {}

  template <class MatrixType, class PreconditionerType>
  void InverseMatrix<MatrixType, PreconditionerType>::vmult(
    TrilinosWrappers::MPI::Vector &      dst,
    const TrilinosWrappers::MPI::Vector &src) const
  {
    SolverControl              solver_control(src.size(), 1e-6 * src.l2_norm());
    TrilinosWrappers::SolverCG cg(solver_control,
                                  TrilinosWrappers::SolverCG::AdditionalData());

    tmp = 0.;
    cg.solve(*matrix, tmp, src, *preconditioner);
    dst = tmp;
  }

  template <class PreconditionerType>
  class SchurComplement : public TrilinosWrappers::SparseMatrix
  {
  public:
    SchurComplement(const TrilinosWrappers::BlockSparseMatrix &system_matrix,
                    const InverseMatrix<TrilinosWrappers::SparseMatrix,
                                        PreconditionerType> &  A_inverse,
                    const IndexSet &                           owned_pres,
                    const MPI_Comm &mpi_communicator);

    void vmult(TrilinosWrappers::MPI::Vector &      dst,
               const TrilinosWrappers::MPI::Vector &src) const;

  private:
    const SmartPointer<const TrilinosWrappers::BlockSparseMatrix> system_matrix;
    const SmartPointer<
      const InverseMatrix<TrilinosWrappers::SparseMatrix, PreconditionerType>>
                                          A_inverse;
    mutable TrilinosWrappers::MPI::Vector tmp1, tmp2;
  };

  template <class PreconditionerType>
  SchurComplement<PreconditionerType>::SchurComplement(
    const TrilinosWrappers::BlockSparseMatrix &system_matrix,
    const InverseMatrix<TrilinosWrappers::SparseMatrix, PreconditionerType>
      &             A_inverse,
    const IndexSet &owned_vel,
    const MPI_Comm &mpi_communicator)
    : system_matrix(&system_matrix)
    , A_inverse(&A_inverse)
    , tmp1(owned_vel, mpi_communicator)
    , tmp2(tmp1)
  {}

  template <class PreconditionerType>
  void SchurComplement<PreconditionerType>::vmult(
    TrilinosWrappers::MPI::Vector &      dst,
    const TrilinosWrappers::MPI::Vector &src) const
  {
    system_matrix->block(0, 1).vmult(tmp1, src);
    A_inverse->vmult(tmp2, tmp1);
    system_matrix->block(1, 0).vmult(dst, tmp2);
  }

  template <int dim>
  StokesProblem<dim>::StokesProblem(const unsigned int degree)
    : degree(degree)
    , mpi_communicator(MPI_COMM_WORLD)
    , triangulation(mpi_communicator)
    , fe(FE_Q<dim>(degree + 1), dim, FE_Q<dim>(degree), 1)
    , dof_handler(triangulation)
    , pcout(std::cout, Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
    , mapping(degree + 1)
  {}
// @endcond
// @sect3{Setting up periodicity constraints on distributed triangulations}
  template <int dim>
  void StokesProblem<dim>::create_mesh()
  {
    Point<dim>   center;
    const double inner_radius = .5;
    const double outer_radius = 1.;

    GridGenerator::quarter_hyper_shell(
      triangulation, center, inner_radius, outer_radius, 0, true);

// 在我们可以规定周期性约束之前，我们需要确保位于域的对面但由周期性面连接的单元是幽灵层的一部分，如果其中一个单元存储在本地处理器上。在这一点上，我们需要考虑我们要如何规定周期性。左边边界上的面的顶点 $\text{vertices}_2$ 应该与下面边界上的面的顶点 $\text{vertices}_1$ 相匹配，由 $\text{vertices}_2=R\cdot \text{vertices}_1+b$ 给出，其中旋转矩阵 $R$ 和偏移量 $b$ 由
//  @f{align*}
//  R=\begin{pmatrix}
//  0&1\\-1&0
//  \end{pmatrix},
//  \quad
//  b=\begin{pmatrix}0&0\end{pmatrix}.
//  @f}
//  给出。 我们将所得信息保存到这里的数据结构是基于三角结构的。

    std::vector<GridTools::PeriodicFacePair<
      typename parallel::distributed::Triangulation<dim>::cell_iterator>>
      periodicity_vector;

    FullMatrix<double> rotation_matrix(dim);
    rotation_matrix[0][1] = 1.;
    rotation_matrix[1][0] = -1.;

    GridTools::collect_periodic_faces(triangulation,
                                      2,
                                      3,
                                      1,
                                      periodicity_vector,
                                      Tensor<1, dim>(),
                                      rotation_matrix);

// 现在，只要调用 parallel::distributed::Triangulation::add_periodicity. 就可以告诉三角函数所需的周期性，特别容易。
    triangulation.add_periodicity(periodicity_vector);

    triangulation.refine_global(4 - dim);
  }

  template <int dim>
  void StokesProblem<dim>::setup_dofs()
  {
    dof_handler.distribute_dofs(fe);

    std::vector<unsigned int> block_component(dim + 1, 0);
    block_component[dim] = 1;
    DoFRenumbering::component_wise(dof_handler, block_component);

    const std::vector<types::global_dof_index> dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
    const unsigned int n_u = dofs_per_block[0], n_p = dofs_per_block[1];

    {
      owned_partitioning.clear();
      IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
      owned_partitioning.push_back(locally_owned_dofs.get_view(0, n_u));
      owned_partitioning.push_back(locally_owned_dofs.get_view(n_u, n_u + n_p));

      relevant_partitioning.clear();
      IndexSet locally_relevant_dofs;
      DoFTools::extract_locally_relevant_dofs(dof_handler,
                                              locally_relevant_dofs);
      relevant_partitioning.push_back(locally_relevant_dofs.get_view(0, n_u));
      relevant_partitioning.push_back(
        locally_relevant_dofs.get_view(n_u, n_u + n_p));

      constraints.clear();
      constraints.reinit(locally_relevant_dofs);

      FEValuesExtractors::Vector velocities(0);

      DoFTools::make_hanging_node_constraints(dof_handler, constraints);
      VectorTools::interpolate_boundary_values(mapping,
                                               dof_handler,
                                               0,
                                               BoundaryValues<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));
      VectorTools::interpolate_boundary_values(mapping,
                                               dof_handler,
                                               1,
                                               BoundaryValues<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));

// 在我们为网格提供了周期性约束的必要信息后，我们现在可以实际创建它们。对于描述匹配，我们使用与之前相同的方法，也就是说，左边边界上的一个面的 $\text{vertices}_2$ 应该与下面边界上的一个面的顶点 $\text{vertices}_1$ 匹配，由 $\text{vertices}_2=R\cdot \text{vertices}_1+b$  ]，其中旋转矩阵 $R$ 和偏移量 $b$ 由
// @f{align*}
//  R=\begin{pmatrix}
//  0&1\\-1&0
//  \end{pmatrix},
//  \quad
//  b=\begin{pmatrix}0&0\end{pmatrix}.
//  @f}
//  给出。 这两个对象不仅描述了应该如何匹配面，而且还描述了解决方案应该从 $\text{face}_2$ 转换到 $\text{face}_1$ 的意义。

      FullMatrix<double> rotation_matrix(dim);
      rotation_matrix[0][1] = 1.;
      rotation_matrix[1][0] = -1.;

      Tensor<1, dim> offset;

// 为了设置约束，我们首先将周期性信息存储在一个类型为 std::vector@<GridTools::PeriodicFacePair<typename 的辅助对象中。
// DoFHandler@<dim@>::%cell_iterator@>  。周期性边界的边界指标为2（x=0）和3（y=0）。所有其他的参数我们之前已经设置好了。在这种情况下，方向并不重要。由于 $\text{vertices}_2=R\cdot \text{vertices}_1+b$ 这正是我们想要的。

      std::vector<
        GridTools::PeriodicFacePair<typename DoFHandler<dim>::cell_iterator>>
        periodicity_vector;

      const unsigned int direction = 1;

      GridTools::collect_periodic_faces(dof_handler,
                                        2,
                                        3,
                                        direction,
                                        periodicity_vector,
                                        offset,
                                        rotation_matrix);

// 接下来，我们需要提供关于解决方案中哪些矢量值分量应该被旋转的信息。由于我们在这里选择只约束速度，并且从解决方案矢量的第一个分量开始，我们只需插入一个0。

      std::vector<unsigned int> first_vector_components;
      first_vector_components.push_back(0);

// 在设置了周期性_vector中的所有信息之后，我们要做的就是告诉make_periodicity_constraints来创建所需的约束。

      DoFTools::make_periodicity_constraints<dim, dim>(periodicity_vector,
                                                       constraints,
                                                       fe.component_mask(
                                                         velocities),
                                                       first_vector_components);

      VectorTools::interpolate_boundary_values(mapping,
                                               dof_handler,
                                               0,
                                               BoundaryValues<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));
      VectorTools::interpolate_boundary_values(mapping,
                                               dof_handler,
                                               1,
                                               BoundaryValues<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));
    }

    constraints.close();

    {
      TrilinosWrappers::BlockSparsityPattern bsp(owned_partitioning,
                                                 owned_partitioning,
                                                 relevant_partitioning,
                                                 mpi_communicator);

      Table<2, DoFTools::Coupling> coupling(dim + 1, dim + 1);
      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if (!((c == dim) && (d == dim)))
            coupling[c][d] = DoFTools::always;
          else
            coupling[c][d] = DoFTools::none;

      DoFTools::make_sparsity_pattern(dof_handler,
                                      coupling,
                                      bsp,
                                      constraints,
                                      false,
                                      Utilities::MPI::this_mpi_process(
                                        mpi_communicator));

      bsp.compress();

      system_matrix.reinit(bsp);
    }

    {
      TrilinosWrappers::BlockSparsityPattern preconditioner_bsp(
        owned_partitioning,
        owned_partitioning,
        relevant_partitioning,
        mpi_communicator);

      Table<2, DoFTools::Coupling> preconditioner_coupling(dim + 1, dim + 1);
      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if ((c == dim) && (d == dim))
            preconditioner_coupling[c][d] = DoFTools::always;
          else
            preconditioner_coupling[c][d] = DoFTools::none;

      DoFTools::make_sparsity_pattern(dof_handler,
                                      preconditioner_coupling,
                                      preconditioner_bsp,
                                      constraints,
                                      false,
                                      Utilities::MPI::this_mpi_process(
                                        mpi_communicator));

      preconditioner_bsp.compress();

      preconditioner_matrix.reinit(preconditioner_bsp);
    }

    system_rhs.reinit(owned_partitioning, mpi_communicator);
    solution.reinit(owned_partitioning,
                    relevant_partitioning,
                    mpi_communicator);
  }

// 然后程序的其余部分又与  step-22  相同。我们现在省略它，但和以前一样，你可以在下面的 "普通程序 "部分找到这些部分。

//  @cond  SKIP

  template <int dim>
  void StokesProblem<dim>::assemble_system()
  {
    system_matrix         = 0.;
    system_rhs            = 0.;
    preconditioner_matrix = 0.;

    QGauss<dim> quadrature_formula(degree + 2);

    FEValues<dim> fe_values(mapping,
                            fe,
                            quadrature_formula,
                            update_values | update_quadrature_points |
                              update_JxW_values | update_gradients);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    const unsigned int n_q_points = quadrature_formula.size();

    FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> local_preconditioner_matrix(dofs_per_cell,
                                                   dofs_per_cell);
    Vector<double>     local_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    const RightHandSide<dim>    right_hand_side;
    std::vector<Vector<double>> rhs_values(n_q_points, Vector<double>(dim + 1));

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);

    std::vector<SymmetricTensor<2, dim>> symgrad_phi_u(dofs_per_cell);
    std::vector<double>                  div_phi_u(dofs_per_cell);
    std::vector<double>                  phi_p(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      if (cell->is_locally_owned())
        {
          fe_values.reinit(cell);
          local_matrix                = 0;
          local_preconditioner_matrix = 0;
          local_rhs                   = 0;

          right_hand_side.vector_value_list(fe_values.get_quadrature_points(),
                                            rhs_values);

          for (unsigned int q = 0; q < n_q_points; ++q)
            {
              for (unsigned int k = 0; k < dofs_per_cell; ++k)
                {
                  symgrad_phi_u[k] =
                    fe_values[velocities].symmetric_gradient(k, q);
                  div_phi_u[k] = fe_values[velocities].divergence(k, q);
                  phi_p[k]     = fe_values[pressure].value(k, q);
                }

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                {
                  for (unsigned int j = 0; j <= i; ++j)
                    {
                      local_matrix(i, j) +=
                        (symgrad_phi_u[i] * symgrad_phi_u[j] // diffusion
                         - div_phi_u[i] * phi_p[j]           // pressure force
                         - phi_p[i] * div_phi_u[j])          // divergence
                        * fe_values.JxW(q);

                      local_preconditioner_matrix(i, j) +=
                        (phi_p[i] * phi_p[j]) * fe_values.JxW(q);
                    }

                  const unsigned int component_i =
                    fe.system_to_component_index(i).first;
                  local_rhs(i) += fe_values.shape_value(i, q)  //
                                  * rhs_values[q](component_i) //
                                  * fe_values.JxW(q);
                }
            }

          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = i + 1; j < dofs_per_cell; ++j)
              {
                local_matrix(i, j) = local_matrix(j, i);
                local_preconditioner_matrix(i, j) =
                  local_preconditioner_matrix(j, i);
              }

          cell->get_dof_indices(local_dof_indices);
          constraints.distribute_local_to_global(local_matrix,
                                                 local_rhs,
                                                 local_dof_indices,
                                                 system_matrix,
                                                 system_rhs);
          constraints.distribute_local_to_global(local_preconditioner_matrix,
                                                 local_dof_indices,
                                                 preconditioner_matrix);
        }

    system_matrix.compress(VectorOperation::add);
    system_rhs.compress(VectorOperation::add);

    pcout << "   Computing preconditioner..." << std::endl << std::flush;
  }

  template <int dim>
  void StokesProblem<dim>::solve()
  {
    TrilinosWrappers::PreconditionJacobi A_preconditioner;
    A_preconditioner.initialize(system_matrix.block(0, 0));

    const InverseMatrix<TrilinosWrappers::SparseMatrix,
                        TrilinosWrappers::PreconditionJacobi>
      A_inverse(system_matrix.block(0, 0),
                A_preconditioner,
                owned_partitioning[0],
                mpi_communicator);

    TrilinosWrappers::MPI::BlockVector tmp(owned_partitioning,
                                           mpi_communicator);

      TrilinosWrappers::MPI::Vector schur_rhs(owned_partitioning[1],
                                              mpi_communicator);
      A_inverse.vmult(tmp.block(0), system_rhs.block(0));
      system_matrix.block(1, 0).vmult(schur_rhs, tmp.block(0));
      schur_rhs -= system_rhs.block(1);

      SchurComplement<TrilinosWrappers::PreconditionJacobi> schur_complement(
        system_matrix, A_inverse, owned_partitioning[0], mpi_communicator);

      SolverControl solver_control(solution.block(1).size(),
                                   1e-6 * schur_rhs.l2_norm());
      SolverCG<TrilinosWrappers::MPI::Vector> cg(solver_control);

      TrilinosWrappers::PreconditionAMG preconditioner;
      preconditioner.initialize(preconditioner_matrix.block(1, 1));

      InverseMatrix<TrilinosWrappers::SparseMatrix,
                    TrilinosWrappers::PreconditionAMG>
        m_inverse(preconditioner_matrix.block(1, 1),
                  preconditioner,
                  owned_partitioning[1],
                  mpi_communicator);

      cg.solve(schur_complement, tmp.block(1), schur_rhs, preconditioner);

      constraints.distribute(tmp);
      solution.block(1) = tmp.block(1);
    }

    {
      system_matrix.block(0, 1).vmult(tmp.block(0), tmp.block(1));
      tmp.block(0) *= -1;
      tmp.block(0) += system_rhs.block(0);

      A_inverse.vmult(tmp.block(0), tmp.block(0));

      constraints.distribute(tmp);
      solution.block(0) = tmp.block(0);
    }
  }

  template <int dim>
  void
  StokesProblem<dim>::output_results(const unsigned int refinement_cycle) const
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    Vector<float> subdomain(triangulation.n_active_cells());
    for (unsigned int i = 0; i < subdomain.size(); ++i)
      subdomain(i) = triangulation.locally_owned_subdomain();
    data_out.add_data_vector(subdomain, "subdomain");
    data_out.build_patches(mapping, degree + 1);

    data_out.write_vtu_with_pvtu_record(
      "./", "solution", refinement_cycle, MPI_COMM_WORLD, 2);
  }

  template <int dim>
  void StokesProblem<dim>::refine_mesh()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    FEValuesExtractors::Scalar pressure(dim);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      estimated_error_per_cell,
      fe.component_mask(pressure));

    parallel::distributed::GridRefinement::refine_and_coarsen_fixed_number(
      triangulation, estimated_error_per_cell, 0.3, 0.0);
    triangulation.execute_coarsening_and_refinement();
  }

  template <int dim>
  void StokesProblem<dim>::run()
  {
    create_mesh();

    for (unsigned int refinement_cycle = 0; refinement_cycle < 9;
         ++refinement_cycle)
      {
        pcout << "Refinement cycle " << refinement_cycle << std::endl;

        if (refinement_cycle > 0)
          refine_mesh();

        setup_dofs();

        pcout << "   Assembling..." << std::endl << std::flush;
        assemble_system();

        pcout << "   Solving..." << std::flush;
        solve();

        output_results(refinement_cycle);

        pcout << std::endl;
      }
  }
} // namespace Step45

int main(int argc, char *argv[])
{
  try
    {
      using namespace dealii;
      using namespace Step45;

      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
      StokesProblem<2>                 flow_problem(1);
      flow_problem.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;
}
// @endcond


// CANONICALSTRUCTURE class for General DMPs
//   Let x spread in time
//   D.E Argiropoulos - General DMP - June 2020

#include <iostream>
#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

class CanonicalStructure
{
public:
    double ax , x0, Rx0;
    vec x, dx;
    vec Fx, fx;
    vec Rdx, Rx;
    double t0, tf, T;
    double taf , dtaf ;

    CanonicalStructure(int len);
    ~CanonicalStructure();
    void generateFx(std::string kind, vec timed);
    double getFx(double t);
    void generateRfx(string kind, vec timed);

};

CanonicalStructure::CanonicalStructure(int len)
{

  x.set_size(len);
  dx.set_size(len);
  Fx.set_size(len);
  fx.set_size(len);
  Rx.set_size(len);
  Rdx.set_size(len);

  std::cout << "/* Constructor of CanonicalStructure */" << '\n';
}
CanonicalStructure::~CanonicalStructure()
{
}

void CanonicalStructure::generateFx(string kind, vec timed)
{
  int len = timed.n_rows;
  x0 = 0.01;
  t0 = 0;
  ax = 1/2;
  T = timed[len-1]; // starts from 0sec - (len-1)*ts
  tf = T + t0;
  taf = 1;
  dtaf = 0;
  if ( kind == "discrete")
  {
    //CS for discrete
    for (int i = 0; i<len; i++)
    {
      Fx[i] =  ax*timed[i];
      fx[i] = ax;
      x[i] = x0 + Fx[i]/taf ;
      dx[i] = fx[i]/taf;
    }
  }
}

double CanonicalStructure::getFx(double t)
{
  return ax*t;
}

void CanonicalStructure::generateRfx(string kind, vec timed)
{
  int len = timed.n_rows;
  Rx0 = x[len-1] ;
  if ( kind == "discrete")
  {
    for (int i = 0;i<len;i++)
    {
      Rx[i] = Rx0 - Fx[i]/taf ;
      Rdx[i] = -fx[i]/taf;
    }

  }
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include "tinyply.h"
#include "pca.h"
#include <regex>
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

// list of paths of all files under the directory 'dir' when the extension matches the regex
// file_list<true> searches recursively into sub-directories;
// file_list<false> searches only the specified directory
template <bool RECURSIVE> std::vector<fs::path> file_list(fs::path dir, std::regex ext_pattern)
{
	std::vector<fs::path> result;
	using iterator = std::conditional<RECURSIVE, fs::recursive_directory_iterator, fs::directory_iterator>::type;
	const iterator end;
	for (iterator iter{ dir }; iter != end; ++iter)
	{
		const std::string extension = iter->path().extension().string();
		if (fs::is_regular_file(*iter) && std::regex_match(extension, ext_pattern)) result.push_back(*iter);
	}
	return result;
}

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

	std::cout
		<< std::fixed << std::endl
		<< "Usage            : ./<app.exe> <dir>" << std::endl
		<< "Default          : ./pca_mesh.exe ../../data/" << std::endl
		<< std::endl;

	//
	// Initial parameters
	//
	const fs::path input_dir = (argc > 1) ? argv[1] : "../../data/";
	const std::string output_filename = "output_pca.ply";
	const std::vector<fs::path>& input_files = file_list<false>(input_dir, std::regex("\\.(?:ply)"));

	//
	// Compose output filename
	//
	std::stringstream output_abs_filename;
	output_abs_filename << input_dir << "/Output/";
	fs::create_directory(output_abs_filename.str());
	output_abs_filename << output_filename;

	//
	// Verify the first file and check the number of vertices
	//
	std::ifstream ss(input_files.at(0).string(), std::ios::binary);
	tinyply::PlyFile file(ss);
	std::vector<float> verts_first_file;
	file.request_properties_from_element("vertex", { "x", "y", "z" }, verts_first_file);

	//
	// Create matrix for PCA
	//
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> pca_input_matrix(verts_first_file.size(), input_files.size());

	std::cout << "Matrix size: " << pca_input_matrix.rows() << ' ' << pca_input_matrix.cols() << std::endl << std::endl;

	std::vector<float> verts;
	std::vector<float> norms;
	std::vector<uint8_t> colors;
	std::vector<uint32_t> faces;
	std::vector<float> uvCoords;
	uint32_t file_count = 0;

	for (const auto& filename : input_files)
	{
		try
		{
			std::cout << "Reading file <" << filename.string() << "> ... ";
			//
			// Read source ply file
			//
			std::ifstream ss(filename.string(), std::ios::binary);
			tinyply::PlyFile file(ss);

			uint32_t vertexCount, normalCount, colorCount, faceCount, faceTexcoordCount, faceColorCount;
			vertexCount = normalCount = colorCount = faceCount = faceTexcoordCount = faceColorCount = 0;

			vertexCount = file.request_properties_from_element("vertex", { "x", "y", "z" }, verts);

			if (verts.size() != verts_first_file.size())
				throw("[FAIL] The number of vertices does not match");

			normalCount = file.request_properties_from_element("vertex", { "nx", "ny", "nz" }, norms);
			colorCount = file.request_properties_from_element("vertex", { "red", "green", "blue", "alpha" }, colors);

			faceCount = file.request_properties_from_element("face", { "vertex_indices" }, faces, 3);
			faceTexcoordCount = file.request_properties_from_element("face", { "texcoord" }, uvCoords, 6);

			file.read(ss);

			for (int i = 0; i < verts.size(); ++i)
			{
				pca_input_matrix(i, file_count) = verts[i];
			}

			++file_count;

			std::cout << "verts: " << vertexCount << " [OK]" << std::endl;
		}
		catch (const std::exception & e)
		{
			std::cerr << "Caught exception: " << e.what() << std::endl;
		}
	}

	pca_t<float> pca;
	pca.set_input(pca_input_matrix);
	pca.compute();

	std::cout
		<< "Values: \n" << pca.get_eigen_values() << std::endl << std::endl
		<< "Vectors: \n" << pca.get_eigen_vectors() << std::endl << std::endl;

	const auto& result = pca.reprojection();

	for (int i = 0; i < verts.size(); ++i)
	{
		verts[i] = result(i, 0);
	}

	try
	{

		//
		// Write ply file
		//
		std::filebuf fb;
		fb.open(output_abs_filename.str(), std::ios::out | std::ios::binary);
		std::ostream outputStream(&fb);

		tinyply::PlyFile ply_out_file;

		if (!verts.empty())
			ply_out_file.add_properties_to_element("vertex", { "x", "y", "z" }, verts);
		if (!norms.empty())
			ply_out_file.add_properties_to_element("vertex", { "nx", "ny", "nz" }, norms);
		if (!colors.empty())
			ply_out_file.add_properties_to_element("vertex", { "red", "green", "blue", "alpha" }, colors);
		if (!faces.empty())
			ply_out_file.add_properties_to_element("face", { "vertex_indices" }, faces, 3, tinyply::PlyProperty::Type::UINT8);
		if (!uvCoords.empty())
			ply_out_file.add_properties_to_element("face", { "texcoord" }, uvCoords, 6, tinyply::PlyProperty::Type::UINT8);

		ply_out_file.write(outputStream, true);

		fb.close();
	}
	catch (const std::exception & e)
	{
		std::cerr << "Caught exception: " << e.what() << std::endl;
	}

}


#ifndef ALM_CONVEX_HULL_H
#define ALM_CONVEX_HULL_H

#include <vector>

#include <Eigen/Dense>

namespace math_utils{

typedef double coord_t;   // coordinate type
typedef double coord2_t;  // must be big enough to hold 2*max(|coordinate|)^2

// struct Point {
class Point{
public:
  coord_t x, y;

  Point(){}
  Point(const coord_t x_in, const coord_t y_in){
  	x = x_in;
  	y = y_in;
  }

  bool operator<(const Point &p) const {
    return x < p.x || (x == p.x && y < p.y);
  }
};

std::vector<Point> eigenToPoints(const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>>& P);

std::vector<Point> convexHull(std::vector<Point> P);
bool inConvexHull(
    const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>> &P,
    const Eigen::Ref<const Eigen::Vector2d> &q, double tolerance = 1e-16);

// Returns the perpendicular distance from point q to the convex hull of pts.
// Specifically, if pts form a polytope defined by a_i'x <= b_i where each a_i
// is a unit vector, then this returns:
//
// d* = min [b_i - a_i'q]
//       i
//
// If q is inside the convex hull of pts, then d* will be positive, else it will
// be negative.
double signedDistanceInsideConvexHull(
    const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>> &pts,
    const Eigen::Ref<const Eigen::Vector2d> &q);

}

#endif