/*
 * Copyright (C) 2015, Nils Moehrle
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <map>
#include <set>

#include <util/timer.h>
#include <math/accum.h>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>

#include "texturing.h"
#include "seam_leveling.h"
#include "progress_counter.h"

TEX_NAMESPACE_BEGIN

typedef Eigen::SparseMatrix<float> SpMat;

math::Vec3f
sample_edge(TexturePatch::ConstPtr texture_patch, math::Vec2f p1, math::Vec2f p2) {
    math::Vec2f p12 = p2 - p1;

    std::size_t num_samples = std::max(p12.norm(), 1.0f) * 2.0f;

    math::Accum<math::Vec3f> color_accum(math::Vec3f(0.0f));
    /* Sample the edge with linear weights. */
    for (std::size_t s = 0; s < num_samples; ++s) {
        float fraction = static_cast<float>(s) / (num_samples - 1);
        math::Vec2f sample_point = p1 + p12 * fraction;
        math::Vec3f color(texture_patch->get_pixel_value(sample_point));

        color_accum.add(color, 1.0f - fraction);
    }

    return color_accum.normalized();
}

void
find_seam_edges_for_vertex_label_combination(UniGraph const & graph, mve::TriangleMesh::ConstPtr & mesh,
    mve::MeshInfo const & mesh_info, std::size_t vertex, std::size_t label1, std::size_t label2,
    std::vector<MeshEdge> * seam_edges) {

    assert(label1 != 0 && label2 != 0 && label1 < label2);

    mve::TriangleMesh::VertexList const & vertices = mesh->get_vertices();

    std::vector<std::size_t> const & adj_verts = mesh_info[vertex].verts;
    for (std::size_t i = 0; i < adj_verts.size(); ++i) {
        std::size_t adj_vertex = adj_verts[i];
        if (vertex == adj_vertex) continue;

        std::vector<std::size_t> edge_faces;
        mesh_info.get_faces_for_edge(vertex, adj_vertex, &edge_faces);

        for (std::size_t j = 0; j < edge_faces.size(); ++j) {
            for(std::size_t k = j + 1; k < edge_faces.size(); ++k) {

                std::size_t face_label1 = graph.get_label(edge_faces[j]);
                std::size_t face_label2 = graph.get_label(edge_faces[k]);
                if (!(face_label1 < face_label2)) std::swap(face_label1, face_label2);

                if (face_label1 != label1 || face_label2 != label2) continue;

                math::Vec3f v1 = vertices[vertex];
                math::Vec3f v2 = vertices[adj_vertex];
                float length = (v2 - v1).norm();

                /* Ignore zero length edges. */
                if (length == 0.0f) continue;

                MeshEdge seam_edge = {vertex, adj_vertex};
                seam_edges->push_back(seam_edge);
            }
        }
    }
}

math::Vec3f
calculate_difference(VertexProjectionInfos const & vertex_projection_infos,
    mve::TriangleMesh::ConstPtr & mesh, std::vector<TexturePatch::Ptr> const &  texture_patches,
    std::vector<MeshEdge> const & seam_edges, int label1, int label2) {

    assert(label1 != 0 && label2 != 0 && label1 < label2);
    assert(!seam_edges.empty());

    mve::TriangleMesh::VertexList const & vertices = mesh->get_vertices();

    math::Accum<math::Vec3f> color1_accum(math::Vec3f(0.0f));
    math::Accum<math::Vec3f> color2_accum(math::Vec3f(0.0f));

    for (MeshEdge const & seam_edge : seam_edges) {
        math::Vec3f v1 = vertices[seam_edge.v1];
        math::Vec3f v2 = vertices[seam_edge.v2];
        float length = (v2 - v1).norm();

        assert(length != 0.0f);

        std::vector<EdgeProjectionInfo> edge_projection_infos;
        find_mesh_edge_projections(vertex_projection_infos, seam_edge, &edge_projection_infos);

        std::size_t num_samples = 0;

        for (EdgeProjectionInfo const & edge_projection_info : edge_projection_infos) {
            TexturePatch::Ptr texture_patch = texture_patches[edge_projection_info.texture_patch_id];
            const int texture_patch_label = texture_patch->get_label();
            if (texture_patch_label == label1 || texture_patch_label == label2) {
                if (texture_patch_label == label1)
                    color1_accum.add(sample_edge(texture_patch, edge_projection_info.p1, edge_projection_info.p2), length);

                if (texture_patch_label == label2)
                    color2_accum.add(sample_edge(texture_patch, edge_projection_info.p1, edge_projection_info.p2), length);

                num_samples++;
            }
        }
        assert(num_samples == 2);
    }

    math::Vec3f color1 = color1_accum.normalized();
    math::Vec3f color2 = color2_accum.normalized();

    /* The order is essential. */
    math::Vec3f difference = color2 - color1;

    assert(!std::isnan(difference[0]));
    assert(!std::isnan(difference[1]));
    assert(!std::isnan(difference[2]));

    return difference;
}

void
global_seam_leveling(UniGraph const & graph, mve::TriangleMesh::ConstPtr mesh,
    mve::MeshInfo const & mesh_info,
    std::vector<std::vector<VertexProjectionInfo> > const & vertex_projection_infos,
    std::vector<TexturePatch::Ptr> * texture_patches) {

    mve::TriangleMesh::VertexList const & vertices = mesh->get_vertices();
    std::size_t const num_vertices = vertices.size();

    std::cout << "\tCreate matrices for optimization... " << std::flush;
    std::vector<std::map<std::size_t, std::size_t> > vertlabel2row;
    vertlabel2row.resize(num_vertices);

    std::vector<std::vector<std::size_t> > labels;
    labels.resize(num_vertices);

    /* Assign each vertex for each label a new index(row) within the solution vector x. */
    std::size_t x_row = 0;
    for (std::size_t i = 0; i < num_vertices; ++i) {
        std::set<std::size_t> label_set;

        std::vector<std::size_t> faces = mesh_info[i].faces;
        std::set<std::size_t>::iterator it = label_set.begin();
        for (std::size_t j = 0; j < faces.size(); ++j) {
            std::size_t label = graph.get_label(faces[j]);
            label_set.insert(it, label);
        }

        for (it = label_set.begin(); it != label_set.end(); ++it) {
            std::size_t label = *it;
            if (label == 0) continue;
            vertlabel2row[i][label] = x_row;
            labels[i].push_back(label);
            ++x_row;
        }
    }
    std::size_t x_rows = x_row;
    assert(x_rows < static_cast<std::size_t>(std::numeric_limits<int>::max()));

    float const lambda = 0.1f;

    /* Fill the Tikhonov matrix Gamma(regularization constraints). */
    std::size_t Gamma_row = 0;
    std::vector<Eigen::Triplet<float, int> > coefficients_Gamma;
    coefficients_Gamma.reserve(2 * num_vertices);
    for (std::size_t i = 0; i < num_vertices; ++i) {
        for (std::size_t j = 0; j < labels[i].size(); ++j) {
            std::vector<std::size_t> const & adj_verts = mesh_info[i].verts;
            for (std::size_t k = 0; k < adj_verts.size(); ++k) {
                std::size_t adj_vertex = adj_verts[k];
                for (std::size_t l = 0; l < labels[adj_vertex].size(); ++l) {
                    std::size_t label = labels[i][j];
                    std::size_t adj_vertex_label = labels[adj_vertex][l];
                    if (i < adj_vertex && label == adj_vertex_label) {
                        Eigen::Triplet<float, int> t1(Gamma_row, vertlabel2row[i][label], lambda);
                        Eigen::Triplet<float, int> t2(Gamma_row, vertlabel2row[adj_vertex][adj_vertex_label], -lambda);
                        coefficients_Gamma.push_back(t1);
                        coefficients_Gamma.push_back(t2);
                        Gamma_row++;
                    }
                }
            }
        }
    }
    std::size_t Gamma_rows = Gamma_row;
    assert(Gamma_rows < static_cast<std::size_t>(std::numeric_limits<int>::max()));

    SpMat Gamma(Gamma_rows, x_rows);
    Gamma.setFromTriplets(coefficients_Gamma.begin(), coefficients_Gamma.end());

    /* Fill the matrix A and the coefficients for the Vector b of the linear equation system. */
    std::vector<Eigen::Triplet<float, int> > coefficients_A;
    std::vector<math::Vec3f> coefficients_b;
    std::size_t A_row = 0;
    for (std::size_t i = 0; i < num_vertices; ++i) {
        for (std::size_t j = 0; j < labels[i].size(); ++j) {
            for (std::size_t k = 0; k < labels[i].size(); ++k) {
                std::size_t label1 = labels[i][j];
                std::size_t label2 = labels[i][k];
                if (label1 < label2) {

                    std::vector<MeshEdge> seam_edges;
                    find_seam_edges_for_vertex_label_combination(graph, mesh, mesh_info, i, label1, label2, &seam_edges);

                    if (seam_edges.empty()) continue;

                    Eigen::Triplet<float, int> t1(A_row, vertlabel2row[i][label1], 1.0f);
                    Eigen::Triplet<float, int> t2(A_row, vertlabel2row[i][label2], -1.0f);
                    coefficients_A.push_back(t1);
                    coefficients_A.push_back(t2);

                    coefficients_b.push_back(calculate_difference(vertex_projection_infos, mesh, *texture_patches, seam_edges, label1, label2));

                    ++A_row;
                }
            }
        }
    }

    std::size_t A_rows = A_row;
    assert(A_rows < static_cast<std::size_t>(std::numeric_limits<int>::max()));

    SpMat A(A_rows, x_rows);
    A.setFromTriplets(coefficients_A.begin(), coefficients_A.end());

    //SpMat Lhs = A.transpose() * A + Gamma.transpose() * Gamma;
	SpMat I(x_rows, x_rows);
	I.setIdentity();
	SpMat Lhs = A.transpose() * A + Gamma.transpose() * Gamma + I * 0.0001f;
    /* Only keep lower triangle (CG only uses the lower), prune the rest and compress matrix. */
    Lhs.prune([](const int& row, const int& col, const float& value) -> bool {
            return col <= row && value != 0.0f;
        }); // value != 0.0f is only to suppress a compiler warning

    std::vector<std::map<std::size_t, math::Vec3f> > adjust_values(num_vertices);
    std::cout << " done." << std::endl;
    std::cout << "\tLhs dimensionality: " << Lhs.rows() << " x " << Lhs.cols() << std::endl;

    util::WallTimer timer;
    std::cout << "\tCalculating adjustments:"<< std::endl;
    #pragma omp parallel for
    for (std::int64_t channel = 0; channel < 3; ++channel) {
        /* Prepare solver. */
        Eigen::ConjugateGradient<SpMat, Eigen::Lower> cg;
        cg.setMaxIterations(1000);
        cg.setTolerance(0.0001);
        cg.compute(Lhs);

        /* Prepare right hand side. */
        Eigen::VectorXf b(A_rows);
        for (std::size_t i = 0; i < coefficients_b.size(); ++i) {
            b[i] = coefficients_b[i][channel];
        }
        Eigen::VectorXf Rhs = SpMat(A.transpose()) * b;

        /* Solve for x. */
        Eigen::VectorXf x(x_rows);
        x = cg.solve(Rhs);

        /* Subtract mean because system is underconstrained and we seek the solution with minimal adjustments. */
        x = x.array() - x.mean();

        #pragma omp critical
        std::cout << "\t\tColor channel " << channel << ": CG took "
            << cg.iterations() << " iterations. Residual is " << cg.error() << std::endl;

        #pragma omp critical
        for (std::size_t i = 0; i < num_vertices; ++i) {
            for (std::size_t j = 0; j < labels[i].size(); ++j) {
                std::size_t label = labels[i][j];
                adjust_values[i][label][channel] = x[vertlabel2row[i][label]];
            }
        }
    }
    std::cout << "\t\tTook " << timer.get_elapsed_sec() << " seconds" << std::endl;

    mve::TriangleMesh::FaceList const & mesh_faces = mesh->get_faces();

    ProgressCounter texture_patch_counter("\tAdjusting texture patches", texture_patches->size());
    #pragma omp parallel for schedule(dynamic)
    for (std::int64_t i = 0; i < texture_patches->size(); ++i) {
        texture_patch_counter.progress<SIMPLE>();

        TexturePatch::Ptr texture_patch = texture_patches->at(i);

        int label = texture_patch->get_label();
        std::vector<std::size_t> const & faces = texture_patch->get_faces();
        std::vector<math::Vec3f> patch_adjust_values(faces.size() * 3, math::Vec3f(0.0f));

        /* Only adjust texture_patches originating form input images. */
        if (label == 0) {
            texture_patch->adjust_colors(patch_adjust_values);
            texture_patch_counter.inc();
            continue;
        };

        for (std::size_t j = 0; j < faces.size(); ++j) {
            for (std::size_t k = 0; k < 3; ++k) {
                std::size_t face_pos = faces[j] * 3 + k;
                std::size_t vertex = mesh_faces[face_pos];
                patch_adjust_values[j * 3 + k] = adjust_values[vertex].find(label)->second;
            }
        }

        texture_patch->adjust_colors(patch_adjust_values);
        texture_patch_counter.inc();
    }
}

TEX_NAMESPACE_END


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <vector>
#include <algorithm>
#include <math.h>
#include <iostream>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"

using namespace std;

////////////////////////////////////
// Class CNRGmatrix functions //
////////////////////////////////////

//////////////
int CNRGmatrix::NumMatBlocks(){

  // Needs MatBlockMap updated

  return(MatBlockBegEnd.size()/2);

}

//////////////
int CNRGmatrix::FindMatBlock(int iblock1, int iblock2){

  int iMatBlock=0;

  int imatch[]={iblock1,iblock2};

  vector<int>::iterator it;

  int it_pos=-1;

  // STL search algorithm

//   it =search(MatBlockMap.begin(),MatBlockMap.end(),imatch,imatch+2 );
//   it_pos=int(it-MatBlockMap.begin());

  // Dec 2015: Problems here: if MatBlockMap is
  //MatBlockMap = 0  1  0  2  1  0  1  3  2  0  2  4  3  1  4  2
  //Searching "1 0" in MatBlockMap gives it_pos= 1 !!!
  // HOWEVER, there's another "1 0" appearing later at it_pos=4.
  // Solution: keep searching until it=MatBlockMap.end().

  it=MatBlockMap.begin();
  while ( (it< MatBlockMap.end())&&((it_pos % 2)!=0) ){
    it =search(it,MatBlockMap.end(),imatch,imatch+2 );
    it_pos=int(it-MatBlockMap.begin());
    it++;
  }

//   if ( (it!= MatBlockMap.end())&&((it_pos % 2)==0) ){
  if ( (it< MatBlockMap.end())&&((it_pos % 2)==0) ){
      iMatBlock=it_pos/2;
  }
  else{iMatBlock=-1;}

  return(iMatBlock);

}

//////////////
int CNRGmatrix::GetMatBlockLimit(int imatblock, int whichlimit){

  return(MatBlockBegEnd[2*imatblock+whichlimit]);

}

//////////////
int CNRGmatrix::GetMatBlockLimit(int iblock1, int iblock2, int whichlimit){

  int ii=FindMatBlock(iblock1,iblock2);

  if (ii<0)
    return(0);
  else
  return(MatBlockBegEnd[2*ii+whichlimit]);

}

//////////////
int CNRGmatrix::GetMatBlockSize(int imatblock ){

  return(MatBlockBegEnd[2*imatblock+1]-MatBlockBegEnd[2*imatblock]+1);

}

//////////////
int CNRGmatrix::GetMatBlockSize(int iblock1, int iblock2){

  int ii=FindMatBlock(iblock1,iblock2);

  if (ii<0)
    return(0);
  else
  return(MatBlockBegEnd[2*ii+1]-MatBlockBegEnd[2*ii]+1);

}

//////////////
double CNRGmatrix::GetBlockMatEl(int iblock1, int iblock2, int iel, int jel){

  int iMatBlock=FindMatBlock(iblock1,iblock2);

  //if (iMatBlock<0)
  // Watch out: if BlockBegEnd has "holes", iel or jel can be negative (state is not there)
  if ( (iMatBlock<0)||(iel<0)||(jel<0) )
      return(0.0);
  else{
    int i0=GetMatBlockLimit(iMatBlock, 0);
    int Nbl1=CNRGarray::GetBlockSize(iblock1);
    int Nbl2=CNRGarray::GetBlockSize(iblock2);
    int rbl=ij2rNSq(Nbl1,Nbl2,iel,jel);

    // watch out for this one!!
    // It was commented... WHY???? April 09.
    if ((UpperTriangular)&&(iblock1==iblock2)){
      if (iel>jel)
	rbl=ij2rUpTr(Nbl1,jel,iel);
      else
	rbl=ij2rUpTr(Nbl1,iel,jel);
    }

    return(MatEl[i0+rbl]);
  }
}
// complex
complex<double> CNRGmatrix::cGetBlockMatEl(int iblock1, int iblock2, int iel, int jel){

  int iMatBlock=FindMatBlock(iblock1,iblock2);

  //if (iMatBlock<0)
  // Watch out: if BlockBegEnd has "holes", iel or jel can be negative (state is not there)
  if ( (iMatBlock<0)||(iel<0)||(jel<0) )
      return(ZeroC);
  else{
    int i0=GetMatBlockLimit(iMatBlock, 0);
    int Nbl1=CNRGarray::GetBlockSize(iblock1);
    int Nbl2=CNRGarray::GetBlockSize(iblock2);
    int rbl=ij2rNSq(Nbl1,Nbl2,iel,jel);

    // watch out for this one!!
    // It was commented... WHY???? April 09.
    if ((UpperTriangular)&&(iblock1==iblock2)){
      if (iel>jel)
	rbl=ij2rUpTr(Nbl1,jel,iel);
      else
	rbl=ij2rUpTr(Nbl1,iel,jel);
    }
    return(MatElCplx[i0+rbl]);
  }
}

//////////////
double CNRGmatrix::GetMatEl(int ist, int jst){

  int ibl,jbl;

  int iblock1=CNRGarray::GetBlockFromSt(ist,ibl);
  int iblock2=CNRGarray::GetBlockFromSt(jst,jbl);

  return(GetBlockMatEl(iblock1,iblock2,ibl,jbl));
}
// complex
complex<double> CNRGmatrix::cGetMatEl(int ist, int jst){

  int ibl,jbl;

  int iblock1=CNRGarray::GetBlockFromSt(ist,ibl);
  int iblock2=CNRGarray::GetBlockFromSt(jst,jbl);

  return(cGetBlockMatEl(iblock1,iblock2,ibl,jbl));

}

//////////////
// Calculate the trace of the matrix
//////////////
double CNRGmatrix::CalcTrace(){

  double trace=0.0;

  vector<int>::iterator it;

  it=MatBlockMap.begin();
  int iMatBlock=0;
  while ( it< MatBlockMap.end() ){
    int ibl1=*it;
    int ibl2=*(it+1);
    // Only diagonal blocks
    if (ibl1==ibl2){
      int i0=GetMatBlockLimit(iMatBlock, 0);
      int Nbl1=CNRGarray::GetBlockSize(ibl1);
      for (int iel=0;iel<Nbl1;iel++){
	int rbl=ij2rNSq(Nbl1,Nbl1,iel,iel); // Gets the diagonal element
	if (UpperTriangular){
	  rbl=ij2rUpTr(Nbl1,iel,iel);
	}
	trace+=MatEl[i0+rbl];
      }
      // iel index states within the block (starting from 0)
    }
    // if diagonal, loop over MatBlock
    iMatBlock++;
    it=it+2;
  }
  // end while

  return(trace);
}
////

//////////////
// Replace existing matrix element by El

int CNRGmatrix::GetMatElPosition(int iblock1, int iblock2, int iel, int jel){

  int iMatBlock=FindMatBlock(iblock1,iblock2);

  // Watch out: if BlockBegEnd has "holes", iel or jel can be negative (state is not there)

  // iel,jel index states within the block (starting from 0)

  if ( (iMatBlock<0)||(iel<0)||(jel<0) ){
    // Message is harmless.
    //cout << " CNRGmatrix::GetMatElPosition: iMatBlock not found " << endl;
    return(-1);
  }
  else{
    int i0=GetMatBlockLimit(iMatBlock, 0);
    int Nbl1=CNRGarray::GetBlockSize(iblock1);
    int Nbl2=CNRGarray::GetBlockSize(iblock2);
    int rbl=ij2rNSq(Nbl1,Nbl2,iel,jel);

    // watch out for this one!!
    if ((UpperTriangular)&&(iblock1==iblock2)){
      if (iel>jel)
	rbl=ij2rUpTr(Nbl1,jel,iel);
      else
	rbl=ij2rUpTr(Nbl1,iel,jel);
    }
    return(i0+rbl);
  }

}
///////

void CNRGmatrix::PushBlockMatEl(double El, int iblock1, int iblock2, int iel, int jel){
  // Deprecated by GetMatElPosition(Oct 2013)
  int iMatBlock=FindMatBlock(iblock1,iblock2);

  // Watch out: if BlockBegEnd has "holes", iel or jel can be negative (state is not there)

  // iel,jel index states within the block (starting from 0)

  if ( (iMatBlock<0)||(iel<0)||(jel<0) ){
    // Message is harmless.
    //cout << " CNRGmatrix::PushBlockMatEl: iMatBlock not found " << endl;
    return;
  }
  else{
    int i0=GetMatBlockLimit(iMatBlock, 0);
    int Nbl1=CNRGarray::GetBlockSize(iblock1);
    int Nbl2=CNRGarray::GetBlockSize(iblock2);
    int rbl=ij2rNSq(Nbl1,Nbl2,iel,jel);

    // watch out for this one!!
    if ((UpperTriangular)&&(iblock1==iblock2)){
      if (iel>jel)
	rbl=ij2rUpTr(Nbl1,jel,iel);
      else
	rbl=ij2rUpTr(Nbl1,iel,jel);
    }
//     MatEl[i0+rbl]=El;
  //if (IsComplex) {MatElCplx[i0+rbl].real()=El;MatElCplx[i0+rbl].imag()=0.0;}
    if (IsComplex) MatElCplx[i0+rbl]=(complex<double>)El;
    else MatEl[i0+rbl]=El;
  }

}

//////////////
// Replace EXISTING matrix element ist,jst by El
void CNRGmatrix::PushMatEl(double El, int ist, int jst){
  int ibl,jbl;

  int iblock1=CNRGarray::GetBlockFromSt(ist,ibl);
  int iblock2=CNRGarray::GetBlockFromSt(jst,jbl);

  int iPos=GetMatElPosition(iblock1,iblock2,ibl,jbl);

//   if (iPos>-1) MatEl[iPos]=El;
  if (iPos>-1)
    if (IsComplex){MatElCplx[iPos]=(complex<double>)El;}
    else MatEl[iPos]=El;

  //PushBlockMatEl(El,iblock1,iblock2,ibl,jbl);

}
void CNRGmatrix::PushMatEl(complex<double> El, int ist, int jst){
  int ibl,jbl;

  int iblock1=CNRGarray::GetBlockFromSt(ist,ibl);
  int iblock2=CNRGarray::GetBlockFromSt(jst,jbl);

  int iPos=GetMatElPosition(iblock1,iblock2,ibl,jbl);

  if (iPos>-1) MatElCplx[iPos]=El;
}
///////////////

///////////// TEMPLATE TESTING /////////////
// Needs to be defined in header file. Otherwise won't work
//
///////////// END TEMPLATE TESTING ////////////////////

void CNRGmatrix::FilterMap_SetBegEnd(){

// Original
// [e1 e2 e3 e4 e5]
// [(b1 b2), (b1 b2), (b4 b5), (b4 b5), (b5 b6) ]
//
// End up with
//
// [e1 e2 e3 e4 e5]
//[(b1 b2) (b4 b5) (b5 b6) ]
//[(0 1),(2 3), (4 4)]
//

//
// Note: needs that MatBlockMap entering to have two indexes *per element*
//

  vector<int>::iterator map_iter,map_iter2;
  vector<int>::iterator it;
   int ipos=0;
   int nelblock=0;

   MatBlockBegEnd.clear();

   cout << "Setting up MatBlockBegEnd: " << endl;

   for (map_iter=MatBlockMap.begin(); map_iter<MatBlockMap.end();
       map_iter+=2)
     {
       MatBlockBegEnd.push_back(ipos);

       nelblock=1;
       map_iter2=map_iter+2;
       it=search(map_iter2,MatBlockMap.end(),
		 map_iter,map_iter+1);
       // Search for sequence map_iter,map_iter+1
//        while (it!=MatBlockMap.end())
// 	 {
// 	   cout << "Found repetition. MatBlockMap size =  "
// 		<< MatBlockMap.size() << endl;
// 	   nelblock++;
// 	   MatBlockMap.erase(it,it+2);
// 	   it=search(map_iter2,MatBlockMap.end(),
// 		     map_iter,map_iter+1);
// 	 }

       while (map_iter2<MatBlockMap.end())
	 {
	   if ( (*map_iter2==*map_iter)&&(*(map_iter2+1)==*(map_iter+1))  )
	     {
	       nelblock++;
	       MatBlockMap.erase(map_iter2,map_iter2+2);
	       map_iter2-=2;

	     }
	   map_iter2+=2;
	 }

       ipos+=nelblock;

       MatBlockBegEnd.push_back(ipos-1);

     }

}

//////////////
// double* Block2Array(int imatblock){

//   return();
// }

//////////////
void CNRGmatrix::ClearAll(){

  CNRGarray::ClearAll();
  MatEl.clear();
  MatElCplx.clear();
  MatBlockMap.clear();
  MatBlockBegEnd.clear();

}
//////////////

//////////////
void CNRGmatrix::PrintMatBlockQNumbers(int imatblock){

  int ibl=MatBlockMap[2*imatblock];
  int jbl=MatBlockMap[2*imatblock+1];

  cout << " Block (" << ibl << ") x Block (" << jbl << ") : ";
  for (int ii=0;ii<NQNumbers;ii++) cout << fixed << GetQNumber(ibl,ii) << " ";
  cout << "  x  ";
  for (int ii=0;ii<NQNumbers;ii++) cout << fixed << GetQNumber(jbl,ii) << " ";
  cout << endl;

  cout << "Nsti = " << CNRGarray::GetBlockLimit(ibl,0) << " to " << CNRGarray::GetBlockLimit(ibl,1) << "   ;   ";
  cout << "Nstj = " << CNRGarray::GetBlockLimit(jbl,0) << " to " << CNRGarray::GetBlockLimit(jbl,1) << endl;

  cout << resetiosflags (ios_base::floatfield);

}

//////////////
void CNRGmatrix::PrintMatBlock(int imatblock){

  int iblock1=MatBlockMap[2*imatblock];
  int iblock2=MatBlockMap[2*imatblock+1];

  int SizeBl1=GetBlockSize(iblock1);
  int SizeBl2=GetBlockSize(iblock2);

  int i0=GetMatBlockLimit(imatblock,0);
  int i1=i0;

  double Mij;
  complex<double> Mij_cplx;
  int pos=0;

  if (QNumbers.size()>0)
    PrintMatBlockQNumbers(imatblock);

  for (int ii=0;ii<SizeBl1;ii++){
    for (int jj=0;jj<SizeBl2;jj++){
      if (UpperTriangular){
	if (ii>jj){
	  pos=i0+ij2rUpTr(SizeBl1,jj,ii);
	  if (IsComplex) Mij_cplx=conj(MatElCplx[pos]);
	  }
	else{
	  pos=i0+ij2rUpTr(SizeBl1,ii,jj);
	  if (IsComplex) Mij_cplx=MatElCplx[pos];
	}
	if (!IsComplex) Mij=MatEl[pos];
      }
      else{
	if (!IsComplex)Mij=MatEl[i1];
	else{
	  // Is this correct? I don't think so...
	  // This is NOT upper triangular!!
// 	  if (ii>jj) Mij_cplx=conj(MatElCplx[i1]);
// 	  else Mij_cplx=MatElCplx[i1];
 	  Mij_cplx=MatElCplx[i1];
	}
	i1++;
      }
      if (!IsComplex)
      cout << Mij  << " ";
      else
      cout << Mij_cplx  << " ";
    }
    cout << endl;
  }
  cout << endl;

}

//////////////
void CNRGmatrix::PrintMatBlock(int iblock1, int iblock2){

  int imatblock=FindMatBlock(iblock1,iblock2);

  int SizeBl1=GetBlockSize(iblock1);
  int SizeBl2=GetBlockSize(iblock2);

  int i0=GetMatBlockLimit(imatblock,0);
  int i1=i0;

  double Mij;
  complex<double> Mij_cplx;
  int pos=0;

  if (QNumbers.size()>0)
    PrintMatBlockQNumbers(imatblock);
  for (int ii=0;ii<SizeBl1;ii++){
    for (int jj=0;jj<SizeBl2;jj++){
      if (UpperTriangular){
	if (ii>jj){
	  pos=i0+ij2rUpTr(SizeBl1,jj,ii);
	  if (IsComplex) Mij_cplx=conj(MatElCplx[pos]);
	}
	else{
	  pos=i0+ij2rUpTr(SizeBl1,ii,jj);
	  if (IsComplex) Mij_cplx=MatElCplx[pos];
	}
	if (!IsComplex) Mij=MatEl[pos];
      }
      else{
	if (!IsComplex)Mij=MatEl[i1];
	else{
	  // Is this correct? I don't think so...
	  // This is NOT upper triangular!!
// 	  if (ii>jj) Mij_cplx=conj(MatElCplx[i1]);
// 	  else Mij_cplx=MatElCplx[i1];
 	  Mij_cplx=MatElCplx[i1];
	}
	i1++;
      }
      //cout << Mij  << " ";
      if (!IsComplex)
	cout << Mij  << " ";
      else
	cout << Mij_cplx  << " ";
    }
    cout << endl;
  }
  cout << endl;

}

//////////////
void CNRGmatrix::PrintAllBlocks(){

  for (int ibl=0;ibl<NumMatBlocks();ibl++)
    PrintMatBlock(ibl);

}

//////////////
void CNRGmatrix::DiagBlock(int iblock,
			   vector<double> &eigvalues,
			   vector<double> &eigvectors){
// Regular
//
//   SUBROUTINE DSYEV( JOBZ, UPLO,	N, A, LDA, W, WORK, LWORK, INFO	)
//       CHARACTER	    JOBZ, UPLO
//       INTEGER	    INFO, LDA, LWORK, N
//       DOUBLE	    PRECISION A( LDA, *	), W( *	), WORK( * )
//
// UpperTriangular
//
//   SUBROUTINE DSPEV( JOBZ, UPLO,	N, AP, W, Z, LDZ, WORK,	INFO )
//       CHARACTER	    JOBZ, UPLO
//       INTEGER	    INFO, LDZ, N
//       DOUBLE	    PRECISION AP( * ), W( * ), WORK( * ), Z( LDZ, * )
//
//  AP is DOUBLE PRECISION array, dimension (N*(N+1)/2)
//           On entry, the upper or lower triangle of the symmetric matrix
//           A, packed columnwise in a linear array.  The j-th column of A
//           is stored in the array AP as follows:
//           if UPLO = 'U', AP(i + (j-1)*j/2) = A(i,j) for 1<=i<=j;
//           if UPLO = 'L', AP(i + (j-1)*(2*n-j)/2) = A(i,j) for j<=i<=n.
//
//
// Z is DOUBLE PRECISION array, dimension (LDZ, N)
//           If JOBZ = 'V', then if INFO = 0, Z contains the orthonormal
//           eigenvectors of the matrix A, with the i-th column of Z
//           holding the eigenvector associated with W(i).
//           If JOBZ = 'N', then Z is not referenced.

  // LAPACK variables
  char JOBZ='V'; //'N'
  char UPLO='U';
  int Nst;
  int INFO=0,LDA,LWORK;
  int LDZ;

  // LDA=Nst
  // LWORK=3*Nst-1

  double *eigv;
  double *work;
  double *Zvec;

  // Get Block info
  int iMatBlock=FindMatBlock(iblock,iblock);

  if (iMatBlock<0){
    cout << "Cannot diagonalize block " << iblock << endl;
    return;
  }
  Nst=CNRGarray::GetBlockSize(iblock);
  //    cout << "Size block                 : "
  //         << GetMatBlockSize(iMatBlock)
  //         << " should be = " << CNRGarray::GetBlockSize(iblock) <<"^2"<< endl;
  LDA=Nst;
  LWORK=3*Nst-1;

  // Construct the STL vector. Remember: ().begin is an iterator

  vector<double> A1(MatEl.begin()+GetMatBlockLimit(iMatBlock,0),
		    MatEl.begin()+GetMatBlockLimit(iMatBlock,1)+1);

  eigv = new double [Nst];
  work = new double [LWORK];

  // Let me try this:
  if (UpperTriangular){
    cout << "Upper triangular" << endl;
    UPLO='L'; // LAPACK stores by cols, I store by lines: my 'U' is their 'L'
    LDZ=Nst;

    Zvec= new double [Nst*Nst];

    dspev_(&JOBZ,&UPLO,&Nst,&A1[0],eigv,Zvec,&LDZ,work,&INFO);

    int i1=0;
    for (int ii=0;ii<Nst;ii++){
      eigvalues.push_back(eigv[ii]);
      for (int jj=0;jj<Nst;jj++){
	eigvectors.push_back(Zvec[i1]);
	i1++;
      }
    }

    delete[] Zvec;

  }else{

    cout << "Regular block" << endl;

    // Diagonalization works on the STL vector directly!

    dsyev_(&JOBZ,&UPLO,&Nst,&A1[0],&LDA,eigv,work,&LWORK,&INFO);

    int i1=0;
    for (int ii=0;ii<Nst;ii++){
      eigvalues.push_back(eigv[ii]);
      for (int jj=0;jj<Nst;jj++){
	eigvectors.push_back(A1[i1]);
	i1++;
      }
    }

  }
  //else if UpTriang

  //cout << "Diag Block " << iblock << " Size: " << Nst << endl;
  //PrintMatBlockQNumbers(iMatBlock);
  //cout << "Lowest States :" << endl;
  //for (int ii=0;ii<( (20 < Nst)?20:Nst );ii++)
  //cout << eigv[ii] << endl;

  delete[] eigv;
  delete[] work;

  A1.clear();

  //Not needed
  //double *A;
  //A = new double [Nst*Nst];
  //   for (int ii=0;ii < A1.size();ii++) A[ii]=A1[ii];
  //dsyev_(&JOBZ,&UPLO,&Nst,A,&LDA,eigv,work,&LWORK,&INFO);
  //delete[] A;

}

//////////////
//
//  Adding complex matrix! (Oct 2013)
//////////////
void CNRGmatrix::DiagBlock(int iblock,
			   vector<double> &eigvalues,
			   vector<complex<double> > &eigvectors){
  // OLD

// Regular
//
// SUBROUTINE ZHEEV( JOBZ, UPLO,N, A, LDA, W, WORK, LWORK,RWORK, INFO)
//       CHARACTER	    JOBZ, UPLO
//       INTEGER	    INFO, LDA, LWORK, N
//       DOUBLE	    PRECISION RWORK( * ), W( * )
//       COMPLEX*16    A( LDA, * ), WORK( * )
//
// UpperTriangular
//
//   SUBROUTINE ZHPEV( JOBZ, UPLO,N, AP, W, Z, LDZ, WORK,RWORK, INFO )
//       CHARACTER	    JOBZ, UPLO
//       INTEGER	    INFO, LDZ, N
//       DOUBLE	    PRECISION RWORK( * ), W( * )
//       COMPLEX*16    AP(	* ), WORK( * ),	Z( LDZ,	* )
//
//AP is COMPLEX*16 array, dimension (N*(N+1)/2)
//           On entry, the upper or lower triangle of the Hermitian matrix
//           A, packed columnwise in a linear array.  The j-th column of A
//           is stored in the array AP as follows:
//           if UPLO = 'U', AP(i + (j-1)*j/2) = A(i,j) for 1<=i<=j;
//           if UPLO = 'L', AP(i + (j-1)*(2*n-j)/2) = A(i,j) for j<=i<=n.
// Z is COMPLEX*16 array, dimension (LDZ, N)
//           If JOBZ = 'V', then if INFO = 0, Z contains the orthonormal
//           eigenvectors of the matrix A, with the i-th column of Z
//           holding the eigenvector associated with W(i).

  // LAPACK variables
  char JOBZ='V'; //'N'
  char UPLO='U';
  int Nst;
  int INFO=0,LDA,LWORK,LRWORK;
  int LDZ;

  // LDA=Nst
  // LWORK=3*Nst-1

  double *eigv; //W
  complex<double> *work;
  double *rwork;
  complex<double> *Zvec;

  // Get Block info
  int iMatBlock=FindMatBlock(iblock,iblock);

  if (iMatBlock<0){
    cout << "Cannot diagonalize block " << iblock << endl;
    return;
  }
  Nst=CNRGarray::GetBlockSize(iblock);
  //    cout << "Size block                 : "
  //         << GetMatBlockSize(iMatBlock)
  //         << " should be = " << CNRGarray::GetBlockSize(iblock) <<"^2"<< endl;
  LDA=Nst;
  //LWORK=3*Nst-1;
  LWORK=std::max(1,2*Nst-1);
  LRWORK=std::max(1,3*Nst-2);

  // Construct the STL vector. Remember: ().begin is an iterator

  vector<complex<double> > A1(MatElCplx.begin()+GetMatBlockLimit(iMatBlock,0),
		    MatElCplx.begin()+GetMatBlockLimit(iMatBlock,1)+1);

  eigv = new double [Nst];
  rwork = new double [LRWORK];
  work = new complex<double> [LWORK];

  // Let me try this:
  if (UpperTriangular){
    cout << "Upper triangular" << endl;
    UPLO='L'; // LAPACK stores by cols, I store by lines: my 'U' is their 'L'
              // HOWEVER, I need to conjugte the off-diagonal elements first
              // Example: 4x4 matrix
              // Mine 'U': A11 A12 A13 A22 A23 A33
              // LAPACK 'L': A11 A12* A13* A22 A23* A33
              // LAPACK 'U' (wrong): A11 A12 A22 A13 A23 A33
    LDZ=Nst;

    Zvec= new complex<double> [Nst*Nst];

    complex<double> caux=ZeroC;
    for (int ii=0; ii<A1.size();ii++){
      caux=std::conj(A1[ii]);
      A1[ii]=caux;
    }

//     cout << " Block : " << iblock;
//     for (int ii=0; ii<A1.size();ii++)
//       cout << " - A1(" << ii <<")= " << A1[ii] << endl;

    //dspev_(&JOBZ,&UPLO,&Nst,&A1[0],eigv,Zvec,&LDZ,work,&INFO);
    zhpev_(&JOBZ,&UPLO,&Nst,&A1[0],eigv,Zvec,&LDZ,work,rwork,&INFO);

    int i1=0;
    for (int ii=0;ii<Nst;ii++){
      eigvalues.push_back(eigv[ii]);
      for (int jj=0;jj<Nst;jj++){
 	eigvectors.push_back(Zvec[i1]);
///
// 	cout << " Block : " << iblock
// 	     << " - Z("<< i1 <<")= " << Zvec[i1] << endl;
///
	i1++;
      }
    }

    delete[] Zvec;

  }else{

    cout << "Regular block" << endl;

    // Diagonalization works on the STL vector directly!

    zheev_(&JOBZ,&UPLO,&Nst,&A1[0],&LDA,eigv,work,&LWORK,rwork,&INFO);

    int i1=0;
    for (int ii=0;ii<Nst;ii++){
      eigvalues.push_back(eigv[ii]);
      for (int jj=0;jj<Nst;jj++){
	eigvectors.push_back(A1[i1]);
	i1++;
      }
    }

  }
  //else if UpTriang

  //cout << "Diag Block " << iblock << " Size: " << Nst << endl;
  //PrintMatBlockQNumbers(iMatBlock);
  //cout << "Lowest States :" << endl;
  //for (int ii=0;ii<( (20 < Nst)?20:Nst );ii++)
  //cout << eigv[ii] << endl;

  delete[] eigv;
  delete[] rwork;
  delete[] work;

  A1.clear();

}

//////////////

//////////////
//
// New April 08- But not going to use it...
void CNRGmatrix::PutInRegularForm(int iblock){

  // Get block info
  int iMatBlock=FindMatBlock(iblock,iblock);

  if (iMatBlock<0)
    {
      cout << "PutInDiagForm: Cannot find block "
	   << iblock << endl;
      return;
    }
  int NstSq=GetMatBlockSize(iMatBlock);
  int Nst=GetBlockSize(iblock);
  int iblBeg=GetMatBlockLimit(iMatBlock,0);

  // Set "unscrambled" array

  double *BlArray = new double [NstSq];
  int i1,j1,ipos;

  for (int ii=0;ii<NstSq;ii++)
    {
      // Get corresponding i,j (new)
      r2ij(Nst,ii,i1,j1);
      // Get Pmn
      ipos=Pmn(Nst,i1,j1); //(old)
      BlArray[ii]=MatEl[iblBeg+ipos];
    }

  // Unscramble
   for (int ii=0;ii<NstSq;ii++)
     MatEl[iblBeg+ii]=BlArray[ii];

  delete[] BlArray;

}

//////////////
//
// New May 08- Save matrix elements in binary file

void CNRGmatrix::SaveBinary(char filename[]){

  //
  // Saves actually: < c+ + c > (Hermitian)
  //

  FILE *pFile;
  int i,j;
  double auxEl=0.0;

  pFile=fopen(filename,"wb");
  for (i=0;i<Nstates();i++)
    {
      for (j=0;j<Nstates();j++)
	{
	  auxEl=GetMatEl(i,j);
	  fwrite (&auxEl, sizeof(double), 1 ,pFile);
	}
    }
  fclose(pFile);

}

//////////////
//

void CNRGmatrix::SaveInOldFormat(){

  char arqname[32];
  char CNsites[8];

  sprintf(CNsites,"%d",Nshell);

  strcpy(arqname,"DataN");
  strcat(arqname,CNsites);
  strcat(arqname,".bin");

  cout << " Matrix file is " << arqname << endl;

  SaveBinary(arqname);

}

//////////////

/////////////
/// Save object (Mar. 09)
/////////////

void CNRGmatrix::SaveBin(char arqname[]){

  CNRGarray::SaveBin(arqname);

  vector<double>::iterator dit;
  vector<int>::iterator iit;

  vector<complex<double> >::iterator cit;

  ofstream OutFile(arqname, ios::out | ios::binary | ios::app);
  double daux;
  int iaux;
  complex<double> caux;

  if (!OutFile){cout << "SaveBin: Cannot save data in " << arqname << endl; return;}

  // Save MatEl
  iaux=MatEl.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(dit=MatEl.begin();dit<MatEl.end();dit++){
    daux=*dit;
    OutFile.write(reinterpret_cast<char*>(&daux), sizeof(double));
  }
  // Save MatElCplx
  iaux=MatElCplx.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(cit=MatElCplx.begin();cit<MatElCplx.end();cit++){
    caux=*cit;
    OutFile.write(reinterpret_cast<char*>(&caux), sizeof(complex<double>));
  }
  // Save MatBlockMap
  iaux=MatBlockMap.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=MatBlockMap.begin();iit<MatBlockMap.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }
  // Save MatBlockBegEnd
  iaux=MatBlockBegEnd.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=MatBlockBegEnd.begin();iit<MatBlockBegEnd.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }
  // Save NeedOld
  OutFile.write((char*)&NeedOld, sizeof(bool));
  // Save UpperTriangular
  OutFile.write((char*)&UpperTriangular, sizeof(bool));
  // Save IsComplex
  OutFile.write((char*)&IsComplex, sizeof(bool));
  // Save WignerEckartL
  OutFile.write((char*)&WignerEckartL, sizeof(double));

  OutFile.close();

  if (!OutFile.good()){cout << "SaveBin: error saving" << endl;}

}
///

void CNRGmatrix::ReadBin(char arqname[]){

  // ifstream
  ifstream InFile;

  InFile.open(arqname, ios::in | ios::binary);
  //InFile.seekg(0,ios::beg);

  if (!InFile){cout << "ReadBin: Cannot read data in " << arqname << endl; return;}

  CNRGmatrix::ReadBinInStream(InFile);

  InFile.close();

  if (!InFile.good()){cout << "ReadBin: overall error reading" << endl;}

}

/////
void CNRGmatrix::ReadBinInStream(ifstream &InFile){

  int isize,iaux, iaux2;
  double daux;
  complex<double> caux;

  CNRGarray::ReadBinInStream(InFile);

  // Read QNumbers_stcf
  MatEl.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&daux), sizeof(double));
    MatEl.push_back(daux);
  }
  // Read MatElCplx
  MatElCplx.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&caux), sizeof(complex<double>));
    MatElCplx.push_back(caux);
  }
  // Read MatBlockMap
  MatBlockMap.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    MatBlockMap.push_back(iaux2);
  }
  // Read MatBlockBegEnd
  MatBlockBegEnd.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    MatBlockBegEnd.push_back(iaux2);
  }
  // Read NeedOld
  InFile.read((char*)&NeedOld, sizeof(bool));
  // Read UpperTriangular
  InFile.read((char*)&UpperTriangular, sizeof(bool));
  // Read IsComplex
  InFile.read((char*)&IsComplex, sizeof(bool));
  // Read WignerEckartL
  InFile.read((char*)&WignerEckartL, sizeof(double));

}
/////////////
void CNRGmatrix::GetConnectingBlocks(int ibl_base,
				     vector < vector<int> > &Connect_BlockMatBlock,
				     bool ToBase){

  // Given a block ibl_base, and stores the connecting blocks
  // "From" (<ibl_base|Op|ibl>) or "To" (<ibl|Op|ibl_base>)
  // and respective BlockMat in the two-column array
  // Connect_BlockMatBlock

  Connect_BlockMatBlock.clear();
  int ifound=0;
  int iblFrom=ibl_base;
  int iblTo=0;
  for (int iblOp=0;iblOp<NumBlocks();iblOp++){
    iblFrom=ibl_base;
    iblTo=iblOp;
    if (ToBase){
      iblFrom=iblOp;
      iblTo=ibl_base;
    }
//     cout << "NRGmatrix::GetConnectingBlocks: ib_base = " << ibl_base
//  	 << " ibOp = " << iblOp
//  	 << " iblFrom = " << iblFrom
//  	 << " iblTo = " << iblTo
//     	 << " -> <From|Op|To> ? " << FindMatBlock(iblFrom,iblTo)
//  	 << endl;

    if (FindMatBlock(iblFrom,iblTo)>=0){
      Connect_BlockMatBlock.push_back( vector<int>() );
      Connect_BlockMatBlock[ifound].push_back(iblOp);
      Connect_BlockMatBlock[ifound].push_back(FindMatBlock(iblFrom,iblTo));
      ifound++;
    }
  }
  // end loop in blocks

}
//////////////

void CNRGmatrix::GetBlocksFromMatBlock(int iMatBlock, int &ibl1, int &ibl2){

  vector<int>::iterator iit=MatBlockMap.begin()+2*iMatBlock;

  ibl1=(*iit);
  ibl2=(*(iit+1));

}

//////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::MatBlock2BLAS(int iMatBlock){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  return(MatBlock2BLAS(ibl1,ibl2));

}
//////////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::MatBlock2BLAS(int ibl1, int ibl2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int pos=GetMatBlockLimit(ibl1,ibl2,0);

  boost::numeric::ublas::matrix<double> dMatAux(Nst1,Nst2);

  for (int ii=0;ii<Nst1;ii++){
    for (int jj=0;jj<Nst2;jj++){
      dMatAux(ii,jj)=GetBlockMatEl(ibl1,ibl2,ii,jj);
    }
  }

  return(dMatAux);

}

/// complex
boost::numeric::ublas::matrix<complex<double> > CNRGmatrix::cMatBlock2BLAS(int ibl1, int ibl2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int pos=GetMatBlockLimit(ibl1,ibl2,0);

  boost::numeric::ublas::matrix<complex<double> > cMatAux(Nst1,Nst2);

  for (int ii=0;ii<Nst1;ii++){
    for (int jj=0;jj<Nst2;jj++){
      cMatAux(ii,jj)=cGetBlockMatEl(ibl1,ibl2,ii,jj);
    }
  }

  return(cMatAux);

}

//////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::MatBlock2BLAS(int iMatBlock,bool kpi, bool kpj){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  return(MatBlock2BLAS(ibl1,ibl2,kpi,kpj));

}
//////////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::MatBlock2BLAS(int ibl1, int ibl2,bool kp1, bool kp2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nstkp1=CNRGarray::GetBlockSize(ibl1,kp1);
  int Nstkp2=CNRGarray::GetBlockSize(ibl2,kp2);

  //int pos=GetMatBlockLimit(ibl1,ibl2,0);

  boost::numeric::ublas::matrix<double> dMatAux(Nstkp1,Nstkp2);

  int istkp=0;
  for (int ii=0;ii<Nst1;ii++){
    int jstkp=0;
    for (int jj=0;jj<Nst2;jj++){
      if ((CheckKept(ibl1,ii,kp1))&&(CheckKept(ibl2,jj,kp2))){
	dMatAux(istkp,jstkp)=GetBlockMatEl(ibl1,ibl2,ii,jj);
	jstkp++;
      }
    }
    //end jj
    if (CheckKept(ibl1,ii,kp1)) istkp++;
  }
  //end ii

  return(dMatAux);

}
//////////////////
boost::numeric::ublas::matrix<complex<double> > CNRGmatrix::cMatBlock2BLAS(int ibl1, int ibl2,bool kp1, bool kp2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nstkp1=CNRGarray::GetBlockSize(ibl1,kp1);
  int Nstkp2=CNRGarray::GetBlockSize(ibl2,kp2);

  //int pos=GetMatBlockLimit(ibl1,ibl2,0);

  boost::numeric::ublas::matrix<complex<double> > cMatAux(Nstkp1,Nstkp2);

  int istkp=0;
  for (int ii=0;ii<Nst1;ii++){
    int jstkp=0;
    for (int jj=0;jj<Nst2;jj++){
      if ((CheckKept(ibl1,ii,kp1))&&(CheckKept(ibl2,jj,kp2))){
	cMatAux(istkp,jstkp)=cGetBlockMatEl(ibl1,ibl2,ii,jj);
	jstkp++;
      }
    }
    //end jj
    if (CheckKept(ibl1,ii,kp1)) istkp++;
  }
  //end ii

  return(cMatAux);

}
///////////

//////////////
void CNRGmatrix::MatBlock2STL(vector<double>& Mout, int iMatBlock){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  MatBlock2STL(Mout,ibl1,ibl2);

}
//////////////////
void CNRGmatrix::MatBlock2STL(vector<double>& Mout,
			       int ibl1, int ibl2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  Mout.clear();

  int ist=0;
  for (int ii=0;ii<Nst1;ii++){
    int jst=0;
    for (int jj=0;jj<Nst2;jj++){
      Mout.push_back(GetBlockMatEl(ibl1,ibl2,ii,jj));
      jst++;
    }
    //end jj
    ist++;
  }
  //end ii

}
//////////////////

//////////////
void CNRGmatrix::MatBlock2STL(vector<double>& Mout,
			       int iMatBlock,bool kpi, bool kpj){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  MatBlock2STL(Mout,ibl1,ibl2,kpi,kpj);

}
//////////////////
void CNRGmatrix::MatBlock2STL(vector<double>& Mout,
			       int ibl1, int ibl2,bool kp1, bool kp2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nstkp1=CNRGarray::GetBlockSize(ibl1,kp1);
  int Nstkp2=CNRGarray::GetBlockSize(ibl2,kp2);

  Mout.clear();

  int istkp=0;
  for (int ii=0;ii<Nst1;ii++){
    int jstkp=0;
    for (int jj=0;jj<Nst2;jj++){
      if ((CheckKept(ibl1,ii,kp1))&&(CheckKept(ibl2,jj,kp2))){
	Mout.push_back(GetBlockMatEl(ibl1,ibl2,ii,jj));
	jstkp++;
      }
    }
    //end jj
    if (CheckKept(ibl1,ii,kp1)) istkp++;
  }
  //end ii

}
//////////////////

//////////////
void CNRGmatrix::cMatBlock2STL(vector<complex<double> >& cMout,
			       int iMatBlock){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  cMatBlock2STL(cMout,ibl1,ibl2);

}
//////////////////
void CNRGmatrix::cMatBlock2STL(vector<complex<double> >& cMout,
			       int ibl1, int ibl2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  cMout.clear();

  int ist=0;
  for (int ii=0;ii<Nst1;ii++){
    int jst=0;
    for (int jj=0;jj<Nst2;jj++){
      cMout.push_back(cGetBlockMatEl(ibl1,ibl2,ii,jj));
      jst++;
    }
    //end jj
    ist++;
  }
  //end ii

}
//////////////////

//////////////
void CNRGmatrix::cMatBlock2STL(vector<complex<double> >& cMout,
			       int iMatBlock,bool kpi, bool kpj){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  cMatBlock2STL(cMout,ibl1,ibl2,kpi,kpj);

}
//////////////////
void CNRGmatrix::cMatBlock2STL(vector<complex<double> >& cMout,
			       int ibl1, int ibl2,bool kp1, bool kp2){

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nstkp1=CNRGarray::GetBlockSize(ibl1,kp1);
  int Nstkp2=CNRGarray::GetBlockSize(ibl2,kp2);

  cMout.clear();

  int istkp=0;
  for (int ii=0;ii<Nst1;ii++){
    int jstkp=0;
    for (int jj=0;jj<Nst2;jj++){
      if ((CheckKept(ibl1,ii,kp1))&&(CheckKept(ibl2,jj,kp2))){
	cMout.push_back(cGetBlockMatEl(ibl1,ibl2,ii,jj));
	jstkp++;
      }
    }
    //end jj
    if (CheckKept(ibl1,ii,kp1)) istkp++;
  }
  //end ii

}
//////////////////

//////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::RotateBlock2BLAS(CNRGbasisarray* pAcut, int iMatBlock){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  return(RotateBlock2BLAS(pAcut,ibl1,ibl2));

}

boost::numeric::ublas::matrix<double> CNRGmatrix::RotateBlock2BLAS(CNRGbasisarray* pAcut, int iMatBlock, bool kp1, bool kp2){

  int ibl1=0;
  int ibl2=0;

  GetBlocksFromMatBlock(iMatBlock,ibl1,ibl2);

  return(RotateBlock2BLAS(pAcut,ibl1,ibl2));

}

//////////////
boost::numeric::ublas::matrix<double> CNRGmatrix::RotateBlock2BLAS(CNRGbasisarray* pAcut, int ibl1, int ibl2){

  // This routine takes an op in the Aeig basis (Nst_bl1,Nst_bl2) and rotates it back
  // to the "before cut" basis (NstBC_bl1,NstBC_bl2)

  // Boost matrices

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nst_w1=pAcut->GetBlockSize(ibl1);
  int Nst_w2=pAcut->GetBlockSize(ibl2);

  if ( (Nst1!=Nst_w1)||(Nst2!=Nst_w2) ){
    cout << "RotateBlock: Error: Block sizes do not match." << endl;
    return(boost::numeric::ublas::matrix<double> ());
  }

  int Nst_basis1=pAcut->GetBlockSizeBC(ibl1);
  int Nst_basis2=pAcut->GetBlockSizeBC(ibl2);

  boost::numeric::ublas::matrix<double> Z1bl(Nst_w1,Nst_basis1);
  boost::numeric::ublas::matrix<double> Z2bl(Nst_w2,Nst_basis2);
  boost::numeric::ublas::matrix<double> MatInitial(Nst_w1,Nst_w2);
  boost::numeric::ublas::matrix<double> MatFinal(Nst_basis1,Nst_basis2);

  Z1bl=pAcut->EigVecCut2BLAS(ibl1);
  Z2bl=pAcut->EigVecCut2BLAS(ibl2);

  MatInitial=MatBlock2BLAS(ibl1,ibl2);

  noalias(MatFinal)=prod ( trans(Z1bl),
	    boost::numeric::ublas::matrix<double>(prod(MatInitial,Z2bl)) );

  return(MatFinal);

}
/// complex
boost::numeric::ublas::matrix<complex<double> > CNRGmatrix::cRotateBlock2BLAS(CNRGbasisarray* pAcut, int ibl1, int ibl2){

  // This routine takes an op in the Aeig basis (Nst_bl1,Nst_bl2) and rotates it back
  // to the "before cut" basis (NstBC_bl1,NstBC_bl2)
  // Boost matrices

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nst_w1=pAcut->GetBlockSize(ibl1);
  int Nst_w2=pAcut->GetBlockSize(ibl2);

  if ( (Nst1!=Nst_w1)||(Nst2!=Nst_w2) ){
    cout << "RotateBlock: Error: Block sizes do not match." << endl;
    return(boost::numeric::ublas::matrix<complex<double> > ());
  }

  int Nst_basis1=pAcut->GetBlockSizeBC(ibl1);
  int Nst_basis2=pAcut->GetBlockSizeBC(ibl2);

  boost::numeric::ublas::matrix<complex<double> > Z1bl(Nst_w1,Nst_basis1);
  boost::numeric::ublas::matrix<complex<double> > Z2bl(Nst_w2,Nst_basis2);
  boost::numeric::ublas::matrix<complex<double> > MatInitial(Nst_w1,Nst_w2);
  boost::numeric::ublas::matrix<complex<double> > MatFinal(Nst_basis1,Nst_basis2);

  Z1bl=pAcut->cEigVecCut2BLAS(ibl1);
  Z2bl=pAcut->cEigVecCut2BLAS(ibl2);

  MatInitial=cMatBlock2BLAS(ibl1,ibl2);

//   noalias(MatFinal)=prod ( trans(Z1bl),
// 	    boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,Z2bl)) );
// Ops, should be:

  noalias(MatFinal)=prod ( herm(Z1bl),
	    boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,Z2bl)) );

  return(MatFinal);

}

////////

//////////////

// Oct 2011
boost::numeric::ublas::matrix<double> CNRGmatrix::RotateBlock2BLAS_NoCut(CNRGarray* pAeig, int ibl1, int ibl2, bool forward){

  // ONLY WORKS IF MATRIX IS IN SYNC WITH Aeig and NO CUTTING HAS BEEN DONE!!!

  // forward=0
  // Takes an op in the "Aeig" basis (Nst_bl1,Nst_bl2) and rotates it backward
  // to the "Abasis" basis.

  // forward=1
  // Takes an op in the "Abasis" basis  and rotates it forward
  // to the "Aeig" basis.

  // Boost matrices

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nst_w1=pAeig->GetBlockSize(ibl1);
  int Nst_w2=pAeig->GetBlockSize(ibl2);

  if ( (Nst1!=Nst_w1)||(Nst2!=Nst_w2) ){
    cout << "RotateForwardBackwardBlock: Error: Block sizes do not match." << endl;
    return(boost::numeric::ublas::matrix<double> ());
  }

  boost::numeric::ublas::matrix<double> Z1bl(Nst_w1,Nst_w1);
  boost::numeric::ublas::matrix<double> Z2bl(Nst_w2,Nst_w2);
  boost::numeric::ublas::matrix<double> MatInitial(Nst_w1,Nst_w2);
  boost::numeric::ublas::matrix<double> MatFinal(Nst_w1,Nst_w2);

  Z1bl=pAeig->EigVec2BLAS(ibl1);
  Z2bl=pAeig->EigVec2BLAS(ibl2);

  MatInitial=MatBlock2BLAS(ibl1,ibl2);

  if (forward)
    noalias(MatFinal)=prod ( Z1bl,
			   boost::numeric::ublas::matrix<double>(prod(MatInitial,trans(Z2bl))) );
  else
    noalias(MatFinal)=prod ( trans(Z1bl),
			     boost::numeric::ublas::matrix<double>(prod(MatInitial,Z2bl)) );

  return(MatFinal);
}

//////////////

// Nov 2013
boost::numeric::ublas::matrix<complex<double> > CNRGmatrix::cRotateBlock2BLAS_NoCut(CNRGarray* pAeig, int ibl1, int ibl2, bool forward){

  // ONLY WORKS IF MATRIX IS IN SYNC WITH Aeig and NO CUTTING HAS BEEN DONE!!!

  // forward=0
  // Takes an op in the "Aeig" basis (Nst_bl1,Nst_bl2) and rotates it backward
  // to the "Abasis" basis.

  // forward=1
  // Takes an op in the "Abasis" basis  and rotates it forward
  // to the "Aeig" basis.

  // Boost matrices

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nst_w1=pAeig->GetBlockSize(ibl1);
  int Nst_w2=pAeig->GetBlockSize(ibl2);

  if ( (Nst1!=Nst_w1)||(Nst2!=Nst_w2) ){
    cout << "RotateForwardBackwardBlock: Error: Block sizes do not match." << endl;
    return(boost::numeric::ublas::matrix<complex<double> > ());
  }

  boost::numeric::ublas::matrix<complex<double> > Z1bl(Nst_w1,Nst_w1);
  boost::numeric::ublas::matrix<complex<double> > Z2bl(Nst_w2,Nst_w2);
  boost::numeric::ublas::matrix<complex<double> > MatInitial(Nst_w1,Nst_w2);
  boost::numeric::ublas::matrix<complex<double> > MatFinal(Nst_w1,Nst_w2);

  Z1bl=pAeig->cEigVec2BLAS(ibl1);
  Z2bl=pAeig->cEigVec2BLAS(ibl2);

  MatInitial=cMatBlock2BLAS(ibl1,ibl2);

  //cout << "cRotateBlock2BLAS_NoCut: I'm here III." << endl;

  if (forward)
//     noalias(MatFinal)=prod ( Z1bl,
// 			   boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,trans(Z2bl))) );
    noalias(MatFinal)=prod ( Z1bl,
			   boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,herm(Z2bl))) );

  else
//     noalias(MatFinal)=prod ( trans(Z1bl),
// 			     boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,Z2bl)) );
    noalias(MatFinal)=prod ( herm(Z1bl),
			     boost::numeric::ublas::matrix<complex<double> >(prod(MatInitial,Z2bl)) );

  return(MatFinal);

}

////////

////////
boost::numeric::ublas::matrix<double> CNRGmatrix::RotateBlock2BLAS(CNRGbasisarray* pAcut, int ibl1, int ibl2, bool kp1, bool kp2){

  // Boost matrices

  int Nst1=CNRGarray::GetBlockSize(ibl1);
  int Nst2=CNRGarray::GetBlockSize(ibl2);

  int Nstkp1=CNRGarray::GetBlockSize(ibl1,kp1);
  int Nstkp2=CNRGarray::GetBlockSize(ibl2,kp2);

  int Nst_w1=pAcut->GetBlockSize(ibl1);
  int Nst_w2=pAcut->GetBlockSize(ibl2);

  if ( (Nst1!=Nst_w1)||(Nst2!=Nst_w2) ){
    cout << "RotateBlock: Error: Block sizes do not match." << endl;
    return(boost::numeric::ublas::matrix<double> ());
  }

  int Nst_basis1=pAcut->GetBlockSizeBC(ibl1);
  int Nst_basis2=pAcut->GetBlockSizeBC(ibl2);

  if ( (Nst1!=Nst_basis1)||(Nst2!=Nst_basis2) ){
    cout << "RotateBlock kept: Error: states are cut! (size in basis does not match)" << endl;
    return(boost::numeric::ublas::matrix<double> ());
  }

  boost::numeric::ublas::matrix<double> Z1bl(Nstkp1,Nstkp1);
  boost::numeric::ublas::matrix<double> Z2bl(Nstkp2,Nstkp2);
  boost::numeric::ublas::matrix<double> MatInitial(Nstkp1,Nstkp2);
  boost::numeric::ublas::matrix<double> MatFinal(Nstkp1,Nstkp2);

  Z1bl=pAcut->EigVecCut2BLAS(ibl1,kp1);
  Z2bl=pAcut->EigVecCut2BLAS(ibl2,kp2);

  MatInitial=MatBlock2BLAS(ibl1,ibl2,kp1,kp2);

  noalias(MatFinal)=prod ( trans(Z1bl),
	    boost::numeric::ublas::matrix<double>(prod(MatInitial,Z2bl)) );

  return(MatFinal);

}

//////////////////////////

void CNRGmatrix::SetDiagonalMatrix(double El){
  // Set matrix as the identity matrix (UpperTriangular==false only)
  if (UpperTriangular){
    cout << " SetIdentityMatrix for UpperTriangular not set up " << endl;
    return;
  }

  MatBlockMap.clear();
  MatBlockBegEnd.clear();
  MatEl.clear();
  MatElCplx.clear();

  complex<double> CEl (1.0,0.0);
  CEl=OneC*El; // complex version of El

  int i1=0;
  for (int ibl=0; ibl<NumBlocks();ibl++){
    // Diagonal matrix
    MatBlockMap.push_back(ibl);
    MatBlockMap.push_back(ibl);
    MatBlockBegEnd.push_back(i1);

    int blsize=GetBlockSize(ibl);

    for (int istbl=0;istbl<blsize;istbl++){
      for (int jstbl=0;jstbl<blsize;jstbl++){
	if (istbl==jstbl)
	  if (IsComplex) MatElCplx.push_back(CEl);
	  else MatEl.push_back(El);
	else
	  if (IsComplex) MatElCplx.push_back(ZeroC);
	  else MatEl.push_back(0.0);
	i1++;
      }
    }
    // end loop in block states
    MatBlockBegEnd.push_back(i1-1);
  }
  // end loop in block

}
//////////////////////////

void CNRGmatrix::SetIdentityMatrix(){

  SetDiagonalMatrix(1.0);

}
//////////////////////////

bool CNRGmatrix::SelfCheckForMatEl(int iblock1, int iblock2){

  CNRGbasisarray Aempty;

  ReverseSyncNRGarray(Aempty);

  return(CheckForMatEl(&Aempty,iblock1,iblock2));

}

//////////////////////////

void CNRGmatrix::SetZeroMatrix(){

  // Sets the matrix based on CheckForMatEl (Matrix elements are zero)

  MatBlockMap.clear();
  MatBlockBegEnd.clear();
  MatEl.clear();
  MatElCplx.clear();

  complex<double> C1 (0.0,0.0);

  int i1=0;
  for (int ibl1=0;ibl1<NumBlocks();ibl1++){
    for (int ibl2=0;ibl2<NumBlocks();ibl2++){
      if (SelfCheckForMatEl(ibl1,ibl2)){
	// Diagonal matrix
	MatBlockMap.push_back(ibl1);
	MatBlockMap.push_back(ibl2);
	MatBlockBegEnd.push_back(i1);

	int bl1size=GetBlockSize(ibl1);
	int bl2size=GetBlockSize(ibl2);

	for (int istbl=0;istbl<bl1size;istbl++){
	  for (int jstbl=0;jstbl<bl2size;jstbl++){
	    if (IsComplex)
	      MatElCplx.push_back(C1);
	    else
	      MatEl.push_back(0.0);
	    i1++;
	  }
	}
	// end loop in block states
	MatBlockBegEnd.push_back(i1-1);
      }
      // end if check for mat
    }
  }
  // end loops in blocks

}
//////////////////////////

//////////////////////////

void CNRGmatrix::SetMatrix(CNRGbasisarray* pAbasis,
			   CNRGbasisarray* pSingleSite){
  // Just to make sure...
  if (!ChkSync(pAbasis)){SyncNRGarray(*pAbasis);}

  MatEl.clear();
  MatElCplx.clear();
  MatBlockMap.clear();
  MatBlockBegEnd.clear();

  double aux=0.0;
  complex<double> caux=ZeroC;
  int icounter=0;
  // Routine Set matrix!!
  for (int ibl=0; ibl<pAbasis->NumBlocks();ibl++){
    for (int jbl=0; jbl<pAbasis->NumBlocks();jbl++){
      if ( CheckForMatEl(pAbasis,ibl,jbl) ){
	MatBlockMap.push_back(ibl);
	MatBlockMap.push_back(jbl);
	MatBlockBegEnd.push_back(icounter);
	for (int ist=pAbasis->GetBlockLimit(ibl,0);ist<=pAbasis->GetBlockLimit(ibl,1);ist++) {
	  for (int jst=pAbasis->GetBlockLimit(jbl,0);jst<=pAbasis->GetBlockLimit(jbl,1);jst++) {
	    if (!IsComplex){
	      aux=CalcMatEl(pAbasis,pSingleSite,ist,jst);
	      MatEl.push_back(aux);
	    }else{
	      caux=CalcMatElCplx(pAbasis,pSingleSite,ist,jst);
	      MatElCplx.push_back(caux);
	    }
	    icounter++;
	  }
	}
	// end loop in states
	MatBlockBegEnd.push_back(icounter-1);
      }
      // end if check
    }
  }
  // end loop in ibl,jbl

}
//////////////////////////


//
// Created by kerin on 2019-03-01.
//
#include "hyps.hpp"
#include "file_utils.hpp"

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <algorithm>
#include <cmath>
#include <string>
#include <vector>

namespace boost_io = boost::iostreams;

uint getPos(std::vector<std::string> vars, const std::string& var){
	auto it = std::find(vars.begin(), vars.end(), var);
	return it - vars.begin();
}

double Hyps::normL2() const {
	double res = sigma * sigma;
	res += slab_relative_var.square().sum();
	res += spike_relative_var.square().sum();
	res += lambda.square().sum();
	return std::sqrt(res);
}

bool Hyps::domain_is_valid() const {
	bool res = true;
	if (sigma <= 0) res = false;
	if (slab_relative_var.minCoeff() <= 0) res = false;
	if (slab_var.minCoeff() <= 0) res = false;
	if (lambda.minCoeff() <= 0) res = false;
	if (lambda.maxCoeff() >= 1) res = false;
	if(p.mode_mog_prior_beta || p.mode_mog_prior_gam) {
		if (spike_var.minCoeff() <= 0) res = false;
		if (spike_relative_var.minCoeff() <= 0) res = false;
	}
	return res;
}

// Note: Should this return class hyps!? s_x not defined?
Hyps operator+(const Hyps &h1, const Hyps &h2){
	Hyps hyps(h1.p);
	hyps.sigma = h1.sigma + h2.sigma;
	hyps.slab_var = h1.slab_var + h2.slab_var;
	hyps.slab_relative_var = h1.slab_relative_var + h2.slab_relative_var;
	hyps.lambda = h1.lambda + h2.lambda;
	if(h1.p.mode_mog_prior_beta || h1.p.mode_mog_prior_gam) {
		hyps.spike_var = h1.spike_var + h2.spike_var;
		hyps.spike_relative_var = h1.spike_relative_var + h2.spike_relative_var;
	}
	return hyps;
}

Hyps operator-(const Hyps &h1, const Hyps &h2){
	Hyps hyps(h1.p);
	hyps.sigma = h1.sigma - h2.sigma;
	hyps.slab_var = h1.slab_var - h2.slab_var;
	hyps.spike_var = h1.spike_var - h2.spike_var;
	hyps.slab_relative_var = h1.slab_relative_var - h2.slab_relative_var;
	hyps.spike_relative_var = h1.spike_relative_var - h2.spike_relative_var;
	hyps.lambda = h1.lambda - h2.lambda;
	return hyps;
}

Hyps operator*(const double &scalar, const Hyps &h1){
	Hyps hyps(h1.p);
	hyps.sigma = scalar * h1.sigma;
	hyps.slab_var = scalar * h1.slab_var;
	hyps.spike_var = scalar * h1.spike_var;
	hyps.slab_relative_var = scalar * h1.slab_relative_var;
	hyps.spike_relative_var = scalar * h1.spike_relative_var;
	hyps.lambda = scalar * h1.lambda;
	return hyps;
}

void Hyps::init_from_grid(const int& my_n_effects,
                          const int& ii,
                          const long& n_var,
                          const Eigen::Ref<const Eigen::MatrixXd> &hyps_grid) {
	// Unpack
	double my_sigma = hyps_grid(ii, sigma_ind);
	double my_sigma_b = hyps_grid(ii, sigma_b_ind);
	double my_sigma_g = hyps_grid(ii, sigma_g_ind);
	double my_lam_b = hyps_grid(ii, lam_b_ind);
	double my_lam_g = hyps_grid(ii, lam_g_ind);

	// Assign initial hyps
	resize(my_n_effects);
	if(n_effects == 1) {
		sigma = my_sigma;
		slab_var << my_sigma * my_sigma_b;
		spike_var << my_sigma * my_sigma_b / p.beta_spike_diff_factor;
		slab_relative_var << my_sigma_b;
		spike_relative_var << my_sigma_b / p.beta_spike_diff_factor;
		lambda << my_lam_b;
		s_x << n_var;
	} else if(n_effects == 2) {
		sigma = my_sigma;
		slab_var << my_sigma * my_sigma_b, my_sigma * my_sigma_g;
		spike_var << my_sigma * my_sigma_b / p.beta_spike_diff_factor, my_sigma * my_sigma_g / p.gam_spike_diff_factor;
		slab_relative_var << my_sigma_b, my_sigma_g;
		spike_relative_var << my_sigma_b / p.beta_spike_diff_factor, my_sigma_g / p.gam_spike_diff_factor;
		lambda << my_lam_b, my_lam_g;
		// Intentionally not setting s_x here
		// Will get updated when updates env-weights
	} else {
		// Not implemented
		assert(false);
	}
}

void Hyps::update_pve(){
	// Compute heritability

	pve = lambda * slab_relative_var * s_x;
	pve_large = pve;
	if(p.mode_mog_prior_beta) {
		int ee = 0;
		pve[ee] += (1 - lambda[ee]) * spike_relative_var[ee] * s_x[ee];
	}

	if (p.mode_mog_prior_gam && n_effects > 1) {
		int ee = 1;
		pve[ee] += (1 - lambda[ee]) * spike_relative_var[ee] * s_x[ee];
	}

	pve_large /= (pve.sum() + 1.0);
	pve /= (pve.sum() + 1.0);
}

std::ostream& operator<<(std::ostream& os, const Hyps& hyps){
	os << hyps.sigma << std::endl;
	os << hyps.lambda << std::endl;
	os << hyps.slab_var << std::endl;
	os << hyps.spike_var << std::endl;
	return os;
}

void Hyps::to_file(const std::string &path) const {
	boost_io::filtering_ostream outf;
	fileUtils::fstream_init(outf, path);

	std::vector<std::string> effects = {"_beta", "_gam"};
	int n_effects = lambda.rows();
	outf << std::scientific << std::setprecision(7);
	outf << "hyp value" << std::endl;
	outf << "sigma " << sigma << std::endl;
	for (int ii = 0; ii < n_effects; ii++) {
		outf << "lambda" << ii+1 << " " << lambda[ii] << std::endl;
	}
	for (int ii = 0; ii < n_effects; ii++) {
		outf << "sigma" << effects[ii] << "0 " << slab_relative_var[ii] << std::endl;
	}
	for (int ii = 0; ii < n_effects; ii++) {
		if((ii == 0 && p.mode_mog_prior_beta) || (ii == 1 && p.mode_mog_prior_gam)) {
			outf << "sigma" << effects[ii] << "1 " << spike_relative_var[ii] << std::endl;
		}
	}
	boost_io::close(outf);
}

void Hyps::from_file(const std::string &filename){
	boost_io::filtering_istream fg;
	std::string gz_str = ".gz";
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));
	if (!fg) {
		throw std::runtime_error(filename+" could not be opened");
	}

	// Read file twice to acertain number of lines
	std::string line, header, case1 = "hyp value";
	getline(fg, header);
	if (header != case1) {
		throw std::runtime_error("Unexpected header from "+filename);
	}

	std::vector<std::string> variables;
	std::vector<double> values;
	std::string s1, s2;
	while (getline(fg, line)) {
		std::stringstream ss(line);
		ss >> s1;
		ss >> s2;

		if(!s1.empty()) {
			variables.push_back(s1);
			try {
				values.push_back(stod(s2));
			} catch(...) {
				throw std::invalid_argument("stod failed");
			}
		}
	}

	std::vector< std::string > case_g = {"sigma", "lambda1", "sigma_beta0", "sigma_beta1"};
	std::vector< std::string > case_gxe = {"sigma", "lambda1", "lambda2", "sigma_beta0", "sigma_gam0",
		                                   "sigma_beta1", "sigma_gam1"};

	if(variables == case_gxe) {
		resize(2);
		sigma                 = values[getPos(case_gxe,"sigma")];
		lambda[0]             = values[getPos(case_gxe,"lambda1")];
		lambda[1]             = values[getPos(case_gxe,"lambda2")];
		slab_relative_var[0]  = values[getPos(case_gxe,"sigma_beta0")];
		slab_relative_var[1]  = values[getPos(case_gxe,"sigma_gam0")];
		spike_relative_var[0] = values[getPos(case_gxe,"sigma_beta1")];
		spike_relative_var[1] = values[getPos(case_gxe,"sigma_gam1")];
	} else if (variables == case_g) {
		resize(1);
		sigma                 = values[getPos(case_g,"sigma")];
		lambda[0]             = values[getPos(case_g,"lambda1")];
		slab_relative_var[0]  = values[getPos(case_g,"sigma_beta0")];
		spike_relative_var[0] = values[getPos(case_g,"sigma_beta1")];
	} else {
		std::cout << "Unrecognised hyps header:" << std::endl;
		for (auto sss : variables) {
			std::cout << sss << std::endl;
		}
		throw std::runtime_error("Unrecognised variable names in "+filename);
	}
	slab_var = slab_relative_var * sigma;
	spike_var = spike_relative_var * sigma;
}

void Hyps::random_init(int n_effects, long n_var) {
	std::default_random_engine generator(p.random_seed);
	std::uniform_real_distribution<double> unif_hb(0,0.5);
	std::uniform_real_distribution<double> unif_hg(0,0.1);
	std::uniform_real_distribution<double> unif_loglambda(2,std::max(3.0,1-std::log10(n_var)));

	double h_b   = unif_hb(generator);
	double h_g   = unif_hg(generator);
	double lam_b = std::pow(10,-unif_loglambda(generator));
	double lam_g = std::pow(10,-unif_loglambda(generator));

	resize(n_effects);
	if(n_effects == 1) {
		slab_relative_var << h_b / (1 - h_b) / n_var / lam_b;
		spike_relative_var << slab_relative_var / p.beta_spike_diff_factor;
		lambda << lam_b;
		s_x << n_var;
	} else {
		slab_relative_var << h_b / (1-h_b-h_g) / n_var / lam_b, h_b / (1-h_b-h_g) / n_var / lam_g;
		spike_relative_var << slab_relative_var[0] / p.beta_spike_diff_factor, slab_relative_var[1] / p.gam_spike_diff_factor;
		lambda << lam_b, lam_g;
		s_x << n_var, n_var;
	}
	sigma     = 1;
	slab_var << sigma * slab_relative_var;
	spike_var << sigma * spike_relative_var;
}

Eigen::VectorXd Hyps::get_sigmas(double h_b, double lam_b, double f1_b, long n_var) const {
	Eigen::MatrixXd tmp(2, 2);
	Eigen::VectorXd rhs(2);

	double P = n_var;
	tmp << P * lam_b * (1 - h_b), P * (1 - lam_b) * (1 - h_b),
	    lam_b * (f1_b - 1), f1_b * (1 - lam_b);
	rhs << h_b, 0;
	Eigen::VectorXd soln = tmp.inverse() * rhs;
	return soln;
}

Eigen::VectorXd
Hyps::get_sigmas(double h_b, double h_g, double lam_b, double lam_g, double f1_b, double f1_g, long n_var) const {
	Eigen::MatrixXd tmp(4, 4);
	Eigen::VectorXd rhs(4);

	double P = n_var;
	tmp << P * lam_b * (1 - h_b), P * (1 - lam_b) * (1 - h_b), -P * h_b * lam_g, -P * h_b * (1 - lam_g),
	    lam_b * (f1_b - 1), f1_b * (1 - lam_b), 0, 0,
	    -P * h_g * lam_b, -P * h_g * (1 - lam_b), P * lam_g * (1 - h_g), P * (1 - lam_g) * (1 - h_g),
	    0, 0, lam_g * (f1_g - 1), f1_g * (1 - lam_g);
	rhs << h_b, 0, h_g, 0;
	Eigen::VectorXd soln = tmp.inverse() * rhs;
	return soln;
}