/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */
#define ARMA_DONT_PRINT_ERRORS
#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <algorithm>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include "../components/ConnectedComponents.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(Graph& G,const count k, const count CB,const double epsilon, const bool doInvert)
    : G(G),k(k),CB(CB),epsilon(epsilon),doInvert(doInvert){
     if (G.isDirected()) throw std::runtime_error("Graph is directed!");
     ConnectedComponents cc(G);
     cc.run();
     if (cc.getPartition().numberOfSubsets() > 1) throw std::runtime_error("Graph has more then one component!");
     if(k>=G.numberOfNodes()) throw std::runtime_error("Size of Group greater then number of nodes!");
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;

     S.resize(k);
     n=G.numberOfNodes();

     end = std::chrono::high_resolution_clock::now();
     diff = end-start;
     std::cout << "Constructor finished in " << diff.count() << "(s)" << "\n";
     std::cout << "Number of Nodes=" << G.numberOfNodes() << "\n";
   }

   void CurrentFlowGroupCloseness::run() {
     bool coarse;
     count ID, minDegree;
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;
     std::vector<node> vecOfChosenNodes;

     ID=0;
     if(CB>0){
       ID++;
       minDegree=1;
       mergePeripheralNodes();
       ID++;
       minDegree=updateMinDegree();
       coarse=true;
       while((minDegree<CB)&&(coarse)){
         vecOfChosenNodes=coarsingIndices(minDegree, false);
         coarseGraph(vecOfChosenNodes,ID);
         ID++;
         minDegree=updateMinDegree();
         if(!(vecOfChosenNodes.size()>1)||!(G.numberOfNodes()>k)){
           coarse=false;
         }
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Coarsening of G finished in: " << diff.count() << "(s)" << "\n";
      std::cout << "Number of nodes after coarsening: " << G.numberOfNodes() << "\n";

      /*
      start = std::chrono::high_resolution_clock::now();
      if(CB>1){
        ID=LevelList[LevelList.size()-1].getID();
        std::cout<<"ID="<<ID<<"\n";
        while(ID>1){
          uncoarseEfffectiveResistanceDistanceMatrix(ID);
          //greedy();
          //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
          ID--;
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Uncoarsening in: " << diff.count() << "(s)" << "\n";
      */
      std::cout << "Starting Greedy-Algorithm\n";
      start = std::chrono::high_resolution_clock::now();
      if(doInvert)
        greedy();
      else{
        greedyLAMG();
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Greedy in: " << diff.count() << "(s)" << "\n\n";
    }
    std::vector<node> CurrentFlowGroupCloseness::getNodesofGroup(){
      return S;
    }
    double CurrentFlowGroupCloseness::getCFGCC() {
      return CFGCC;
    }
    void CurrentFlowGroupCloseness::greedy(){
      count v_i,w_i;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V,W;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs,cutNodes;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, minApprox, bstApprox, marginalGain;

      arma::Mat<double> Pinv;
      Pinv=computePinvOfLaplacian();

      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      V.resize(G.numberOfNodes(),true);
      W.resize(G.numberOfNodes(),true);
      mindst.resize(G.numberOfNodes(),n*n);
      marginalGain.resize(G.numberOfNodes(),CFGCC);

      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(std::find(vecOfPeripheralNodes.begin(), vecOfPeripheralNodes.end(), v) != vecOfPeripheralNodes.end())
          vecOfSamples.push_back(v);
        else{
          vecOfPeriphs.push_back(v);
        }
      }
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          v_i=reverse[v];
          if(V[v_i] && (bestMarginalGain<marginalGain[v_i])){
            dst=mindst;
            centrality = 0.;
            cutNodes.resize(0);
            for (count l = 0; l < vecOfSamples.size(); l++) {
              w=vecOfSamples[l];
              w_i=reverse[w];
              if(W[w_i]){
                distance=Pinv(v_i,v_i)+Pinv(w_i,w_i)-2*Pinv(v_i,w_i);
                if (distance< mindst[w_i]){
                  dst[w_i]=distance;
                  if(distance<bst[w_i])
                    cutNodes.push_back(w_i);
                }
              }
              centrality +=dst[w_i];
            }
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              w_i=reverse[w];
              if(W[w_i]){
                distance=Pinv(v_i,v_i)+Pinv(w_i,w_i)-2*Pinv(v_i,w_i);
                if (distance< mindst[w_i]){
                  dst[w_i]=distance;
                  if(distance<bst[w_i])
                    cutNodes.push_back(w_i);
                }
                centrality += dst[w_i];
              }
            }
            marginalGain[v_i]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v_i];
            }
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    void CurrentFlowGroupCloseness::greedyLAMG(){
      count v_i,w_i;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, minApprox, bstApprox, marginalGain;
      std::cout<<"***************Setup***************\n";
      Lamg<CSRMatrix> lamg;
      CSRMatrix matrix = CSRMatrix::laplacianMatrix(G);
      lamg.setupConnected(matrix);
      std::cout<<"**************Setup***************\n";
      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      mindst.resize(G.numberOfNodes(),n*n);
      marginalGain.resize(G.numberOfNodes(),CFGCC);
      V.resize(G.numberOfNodes(),true);
      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(std::find(vecOfPeripheralNodes.begin(), vecOfPeripheralNodes.end(), v) != vecOfPeripheralNodes.end())
          vecOfSamples.push_back(v);
        else{
          vecOfPeriphs.push_back(v);
        }
      }
      Vector result(vecOfNodes.size());
      Vector rhs(vecOfNodes.size(), 0.);
      Vector zeroVector(vecOfNodes.size(), 0.);
      std::cout<<"********************1***************\n";
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          v_i=reverse[v];
          if(V[v_i] && (bestMarginalGain<marginalGain[v_i])){
            rhs[v_i]=1.;
            dst=mindst;
            centrality = 0.;
            for (count l = 0; l < vecOfSamples.size(); l++) {
              if(v!=w){
                w=vecOfSamples[l];
                w_i=reverse[w];
                rhs[w_i]=-1.;
                result=zeroVector;
                lamg.solve(rhs, result);
                distance=fabs(result[v_i]-result[w_i]);
                if (distance< mindst[w_i])
                  dst[w_i]=distance;
                centrality +=dst[w_i];
                rhs[w_i]=0.;
              }
            }
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              w_i=reverse[w];
              rhs[w_i]=-1.;
              result=zeroVector;
              lamg.solve(rhs, result);
              distance=fabs(result[v_i]-result[w_i]);
              if (distance< mindst[w_i])
                dst[w_i]=distance;
              centrality += dst[w_i];
              rhs[w_i]=0.;
            }
            marginalGain[v_i]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v_i];
            }
            rhs[v_i]=0.;
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    count CurrentFlowGroupCloseness::updateMinDegree(){
      bool search;
      count min;
      count i;
      std::vector<node> vecOfNodes;
      node v;
      search=true;
      min=n;
      i=0;
      vecOfNodes=G.nodes();
      while((i<vecOfNodes.size())&&(search)){
        v=vecOfNodes[i];
        if((G.degree(v)<min) && (G.degree(v)>1)){
          min=G.degree(v);
          if(min==2){
            search=false;
          }
        }
        i++;
      }
      return min;
    }

    std::vector<node> CurrentFlowGroupCloseness::coarsingIndices(count courseningDegree, bool Random){
        bool search;
        count l;
        node c,w;
        std::vector<node> vecOfNodes, vecOfChosenNodes, vecOfNeighbors,vecOfOccupiedNodes,vecOfCandidates;

        vecOfOccupiedNodes.resize(0);
        vecOfCandidates.resize(0);
        vecOfNodes=G.nodes();
        for(count i=0;i<vecOfNodes.size();i++){
          c=vecOfNodes[i];
          if(G.degree(c)==courseningDegree){
            vecOfCandidates.push_back(c);
          }
        }
        if(Random){
          std::random_shuffle (vecOfCandidates.begin(), vecOfCandidates.end());
        }
        for(count i=0;i<vecOfCandidates.size();i++){
          c=vecOfCandidates[i];
          if(std::find(vecOfOccupiedNodes.begin(), vecOfOccupiedNodes.end(), c) == vecOfOccupiedNodes.end()){
            vecOfNeighbors=G.neighbors(c);
            search=true;
            l=0;
            if((search)&&(l<vecOfNeighbors.size())){
              w=vecOfNeighbors[l];
              if(std::find(vecOfOccupiedNodes.begin(), vecOfOccupiedNodes.end(), w) != vecOfOccupiedNodes.end()){
                search=false;
              }
              else{
                l++;
              }
            }
            if(search){
              for(count j=0;j<vecOfNeighbors.size();j++){
                w=vecOfNeighbors[j];
                vecOfOccupiedNodes.push_back(w);
              }
              vecOfChosenNodes.push_back(c);
            }
          }
        }
        return vecOfChosenNodes;
      }
    /**************************************************************************
    void CurrentFlowGroupCloseness::uncoarseEfffectiveResistanceDistanceMatrix(count ID){
      bool search;
      count l,j;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<std::tuple<node,node,count,double,double,double>> vecOfTriangles;
      std::tuple<node,node,count,double,double,double> triangle;

      vecOfTriangles=LevelList[ID-1].getVecOfTriangles();
      for(count i=0;i<vecOfTriangles.size();i++){
        j=vecOfTriangles.size()-1-i;
        triangle=vecOfTriangles[j];
        ERD.uncoarseTriangle(vecOfNodes,triangle);
        c=std::get<0>(triangle);
        s=std::get<1>(triangle);
        w=std::get<2>(triangle);
        edgeWeightcs=std::get<3>(triangle);
        edgeWeightcw=std::get<4>(triangle);
        edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
        edgeWeightsw=1./edgeWeightsw;
        //vecOfNodes.push_back(c);
        //Adj[s].push_back (c);
        //Adj[c].push_back(s);
        if(std::get<4>(triangle)>0){
          //Adj[c].push_back (w);
          //Adj[w].push_back(c);
        }
        if(!(std::get<5>(triangle)==edgeWeightsw)){
          search = true;
          l= 0;
          while((l<Adj[s].size())&&(search)){
            if(Adj[s][l]==w){
              Adj[s].erase(Adj[s].begin()+l);
              search=false;
            }
            else{
              l++;
            }
          }//end while
          search = true;
          l= 0;
          while((l<Adj[w].size())&&(search)){
            if(Adj[w][l]==s){
                Adj[w].erase(Adj[w].begin()+l);
                search=false;
              }
          else{
            l++;
          }
          }//end while
        }//end if
        numberOfCoarsedNodes--;
      }//end for
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::mergePeripheralNodes(){
      bool search;
      count l;
      node c,s,w;
      double edgeWeightcs,edgeWeightsw;
      std::vector<node> vecOfNodes,vecOfSupernodes;
      std::vector<double> vecOfWeights;
      std::vector<std::pair<node,node>> mapping;

      vecOfSupernodes.resize(0);
      mapping.resize(0);
      vecOfNodes=G.nodes();
      vecOfWeights.resize(1);
      for(count i=0;i<vecOfNodes.size();i++){
        c=vecOfNodes[i];
        if(G.degree(c)==1){
          s=G.randomNeighbor(c);
          if(std::find(vecOfSupernodes.begin(), vecOfSupernodes.end(), s) == vecOfSupernodes.end()){
            vecOfSupernodes.push_back(s);
            mapping.push_back (std::make_pair(s,c));
            vecOfPeripheralNodes.push_back(c);
          }
          else{
            search = true;
            l= 0;
            while(search){
              if(vecOfSupernodes[l]==s){
                w=mapping[l].second;
                search=false;
              }
              l++;
            }//end while
            edgeWeightcs=G.weight(c,s);
            edgeWeightsw=G.weight(s,w);
            edgeWeightsw=edgeWeightcs+edgeWeightsw;
            G.setWeight(s,w,edgeWeightsw);
            vecOfWeights[0]=edgeWeightcs;
            ERDLevel coarsed(c,G.neighbors(c),vecOfWeights);
            coarsedNodes.push_back(coarsed);
            G.removeNode(c);
          }//end else
        }
      }
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseGraph(std::vector<node> vecOfChosenNodes,count degree){
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<node> vecOfNeighbors;
      std::vector<double> vecOfWeights;

      vecOfWeights.resize(degree);
      /*Path Coarsenening*/
      if(degree==2){
        for(count i=0;i<vecOfChosenNodes.size();i++){
          c=vecOfChosenNodes[i];
          vecOfNeighbors=G.neighbors(c);
          s=vecOfNeighbors[0];
          w=vecOfNeighbors[1];
          edgeWeightcs=G.weight(c,s);
          edgeWeightcw=G.weight(c,w);
          edgeWeightsw=edgeWeightcs+edgeWeightcw;
          /*Path Merge*/
          if(G.weight(s,w)!=0){
            edgeWeightsw=1./edgeWeightsw;
            edgeWeightsw+=1./G.weight(s,w);
            edgeWeightsw=1./edgeWeightsw;
          }
          G.setWeight(s,w,edgeWeightsw);
          for(count j=0;j<vecOfNeighbors.size();j++){
            vecOfWeights[j]=G.weight(c,vecOfNeighbors[j]);
          }
          ERDLevel coarsed(c,vecOfNeighbors,vecOfWeights);
          G.removeNode(c);
        }
      }
      /*Star*/
      else{
        for(count i=0;i<vecOfChosenNodes.size();i++){
          c=vecOfChosenNodes[i];
          computeStarCliqueWeights(c);
          vecOfNeighbors=G.neighbors(c);
          for(count j=0;j<vecOfNeighbors.size();j++){
            vecOfWeights[j]=G.weight(c,vecOfNeighbors[j]);
          }
	  //TODO: Here problem when creating coarsed for the second time
          ERDLevel coarsed(c,vecOfNeighbors,vecOfWeights);
          G.removeNode(c);
        }
      }
    }

    void CurrentFlowGroupCloseness::computeStarCliqueWeights(node c){
      node v,w,x,y;
      double S,S2,weight;
      std::vector<node> vecOfNeighbors;

      S=0.;
      vecOfNeighbors=G.neighbors(c);
      for(count i=0;i<vecOfNeighbors.size();i++){
        v=vecOfNeighbors[i];
        S2=0.;
        for(count j=0;j<vecOfNeighbors.size();j++){
          w=vecOfNeighbors[j];
          if(w!=v){
            S2*=G.weight(v,w);
          }
        }
        S+=S2;
      }
      for(count i=0;i<vecOfNeighbors.size();i++){
        v=vecOfNeighbors[i];
        for(count j=0;j<vecOfNeighbors.size();j++){
          w=vecOfNeighbors[j];
          if(v!=w){
            weight=0.;
            for(count k=0;k<vecOfNeighbors.size();k++){
              x=vecOfNeighbors[k];
              if((x!=v)&&(x!=w)){
                for(count l=0;l<vecOfNeighbors.size();l++){
                  y=vecOfNeighbors[l];
                  if((y!=v)&&(y!=w)&&(y!=x)){
                    weight*=G.weight(x,y);
                  }//if y
                }//for y
              }//if x
            }//for x
	    //TODO: Here weight can be zero! Segfault when dividing with zero!!
	    // maria's example
            if(weight > 0.)
	      weight = S/weight;
	    else weight = 1.; // not sure if you want it one here.
	    // maria's example

            if(G.weight(v,w)!=0){
              weight=1./weight;
              weight+=1./G.weight(v,w);
              weight=1./weight;
            }
            G.setWeight(x,y,weight);
          }// if w
        }// for w
      }// for v
    }
    arma::Mat<double> CurrentFlowGroupCloseness::computePinvOfLaplacian(){
      node v,w;
      count w_i;
      double factor, weight;
      std::vector<node> vecOfNodes, vecOfNeighbours;
      std::vector<count> reverse;
      vecOfNodes=G.nodes();
      arma::Mat<double> L(G.numberOfNodes(),G.numberOfNodes());
      L.zeros();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<G.numberOfNodes();i++){
        v=vecOfNodes[i];
        vecOfNeighbours=G.neighbors(v);
        for(count j=0;j<vecOfNeighbours.size();j++){
          w=vecOfNeighbours[j];
          w_i=reverse[w];
          weight=G.weight(v,w);
          L(i,w_i)=-weight;
          L(i,i)+=weight;
        }
      }
      arma::Mat<double> J(L.n_rows,L.n_rows);
      factor=1./(double)(L.n_rows);
      J.fill(factor);
      L= L-J;
      L=arma::inv(L);
      L= L+J;
      return L;
    }
} /* namespace NetworKit*/


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "structures.h"
#include "transformations.h"
#include "ray_intersection_observation_equation_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<Eigen::Affine3d> bundle_of_rays;
Eigen::Vector3d intersection(0,0,0);

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	for(size_t i = 0; i < 10; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

		bundle_of_rays.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("bundle_of_rays_intersection");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < bundle_of_rays.size(); i++){
		Eigen::Vector3d z_begin(0, 0,-100);
		Eigen::Vector3d z_end(0, 0, 100);

		Eigen::Vector3d z_begin_t = bundle_of_rays[i] * z_begin;
		Eigen::Vector3d z_end_t = bundle_of_rays[i] * z_end;

		glVertex3f(z_begin_t.x(), z_begin_t.y(), z_begin_t.z());
		glVertex3f(z_end_t.x(), z_end_t.y(), z_end_t.z());
	}
	glEnd();

	glPointSize(10);
	glColor3f(1,0,0);
	glBegin(GL_POINTS);
	glVertex3f(intersection.x(), intersection.y(), intersection.z());
	glEnd();
	glPointSize(1);

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				Eigen::Vector3d vx(bundle_of_rays[i](0,0), bundle_of_rays[i](1,0), bundle_of_rays[i](2,0));
				Eigen::Vector3d vy(bundle_of_rays[i](0,1), bundle_of_rays[i](1,1), bundle_of_rays[i](2,1));

				Eigen::Matrix<double, 2, 1> delta;
				ray_intersection_observation_equation(delta,
						intersection.x(), intersection.y(), intersection.z(),
						bundle_of_rays[i](0,3), bundle_of_rays[i](1,3), bundle_of_rays[i](2,3),
						vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z());
				Eigen::Matrix<double, 2, 3> delta_jacobian;
				ray_intersection_observation_equation_jacobian(delta_jacobian,
						intersection.x(), intersection.y(), intersection.z(),
						bundle_of_rays[i](0,3), bundle_of_rays[i](1,3), bundle_of_rays[i](2,3),
						vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z());

				int ir = tripletListB.size();
				tripletListA.emplace_back(ir, 0, -delta_jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -delta_jacobian(0,1));
				tripletListA.emplace_back(ir, 2, -delta_jacobian(0,2));
				tripletListA.emplace_back(ir + 1, 0, -delta_jacobian(1,0));
				tripletListA.emplace_back(ir + 1, 1, -delta_jacobian(1,1));
				tripletListA.emplace_back(ir + 1, 2, -delta_jacobian(1,2));

				tripletListP.emplace_back(ir    , ir    ,  1);
				tripletListP.emplace_back(ir + 1, ir + 1,  1);

				tripletListB.emplace_back(ir    , 0,  delta(0,0));
				tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(3, 3);
			Eigen::SparseMatrix<double> AtPB(3, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 3){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				intersection.x() += h_x[0];
				intersection.y() += h_x[1];
				intersection.z() += h_x[2];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'n':{
			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				TaitBryanPose pose;

				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;

				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

				bundle_of_rays[i] = bundle_of_rays[i] * affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: modify rays" << std::endl;
	std::cout << "t: optimize" << std::endl;
}


#ifndef SIMPLE_SHORTEST_BASIS_HPP
#define SIMPLE_SHORTEST_BASIS_HPP 1
/**
 * @file simple_shortest_basis.hpp
 *
 * @brief calculate equidistribution property of the random number generator.
 *
 * calculate shortest basis of lattice. We can get the dimension of
 * equidistribution from the smallest norm of the vectors in lattice
 * basis.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto and
 * Hiroshima University. All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <tr1/memory>
#include <stdexcept>
#include <NTL/GF2.h>

namespace MTToolBox {
    /**
     * @class linear_generator_vector
     * @brief a pseudo random number generator as a vector
     *
     * This class is a pseudo random number
     * generator as a vector (and polynomial).  As a polynomial, Only
     * multiple by \b x is supported.
     *
     * @tparam G F2-linear generator
     * @tparam T the output type of \b G.
     */
    template<typename G, typename T> class linear_generator_vector {
    public:
        /**
         * constructor
         *
         * @param rand_ random number generator whose state transition
         * function is F2-linear.
         */
        linear_generator_vector<G, T>(const G& rand_) {
            using namespace std::tr1;
            shared_ptr<G> r(new G(rand_));
            rand = r;
            rand->seeding(1);
            count = 0;
            zero = false;
            next = 0;
        }

        /**
         * constructor of a vector in standard basis
         *
         * @param rand_ random number generator used as a template
         * @param bit_pos position of 1 for standard basis.
         */
        linear_generator_vector<G, T>(const G& rand_, int bit_pos) {
            using namespace std::tr1;
            shared_ptr<G> r(new G(rand_));
            rand = r;
            rand->make_zero_status();
            count = 0;
            zero = false;
            next = static_cast<T>(1) << (sizeof(T) * 8 - bit_pos - 1);
        }

        void add(const linear_generator_vector<G, T>& src);
        void next_state(int bit_len);
        void debug_print();

        /**
         * a pseudo random number generator whose state transition
         * function is F2-linear.
         */
        std::tr1::shared_ptr<G> rand;

        /**
         * The counter which shows how many times next_state() is called.
         * This is concerned with norm of vector and degree of polynomial.
         */
        int count;

        /**
         * This shows the vector is zero or not.
         */
        bool zero;

        /**
         * This is important.
         * v-bit MSBs of generator's recent output.
         * Or, the coefficent of highest degree term of polynomial.
         */
        T next;
    };

    /**
     * @class shotest_basis
     * @brief calculate shortest basis of lattice
     *
     * This class calculates shortest basis of lattice,
     * or equidistribution properties of random number generators.
     * This class is most complex part of this system.
     *
     * @tparam G a linear generator vector
     * @tparam T output type of the generator
     */
    template<typename G, typename T> class shortest_basis {
        typedef linear_generator_vector<G, T> linear_vec;
    public:
        /**
         * constructor
         *
         * @param rand a pseudo random number generator whose state
         * transition function is F2-linear.
         * @param bit_len_ from \b bit_len to 1 of dimensions of
         * equidistribution with v-bit accuracy are calculated.
         */
        shortest_basis(const G& rand, int bit_len_) {
            bit_len = bit_len_;
            size = bit_len + 1;
            basis = new linear_vec * [size];
            mexp = rand.get_mexp();
            for (int i = 0; i < bit_len; i++) {
                basis[i] = new linear_vec(rand, i);
            }
            basis[bit_len] = new linear_vec(rand);
            basis[bit_len]->next_state(bit_len);
        }
        /**
         * The destructor.
         */
        ~shortest_basis() {
            for (int i = 0; i < size; i++) {
                delete basis[i];
            }
            delete[] basis;
        }

        int get_all_equidist(int veq[]);
        int get_equidist(int *sum_equidist);
    private:
        int get_equidist_main(int bit_len);
        void adjust(int new_len);
        /** basis of lattice plus one vector */
        linear_vec **basis;
        /** bit lenght count from MSB */
        int bit_len;
        /** Mersenne Exponent, or max value of the dimension of
         * equidistribution. */
        int mexp;
        /** */
        int size;
    };

    /**
     * Adjust bit_len and recalculate the coefficient of the highest
     * degree term.
     *
     * @param new_len a bit length to be changed to.
     */
    template<typename G, typename T>
    void shortest_basis<G, T>::adjust(int new_len) {
        using namespace std;

        T mask = (~static_cast<T>(0)) << (sizeof(T) * 8 - new_len);
#if 0
        if (basis[basis.size() - 1]->zero) {
            basis.erase(basis.begin() + basis.size() - 1);
        } else {
            cerr << "no zero state" << endl;
            throw new logic_error("no zero state");
        }
#endif
        for (int i = 0; i < size; i++) {
            basis[i]->next = basis[i]->next & mask;
            if (basis[i]->next == 0) {
                basis[i]->next_state(new_len);
            }
        }
    }

    /**
     * print some information for debug.
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::debug_print() {
        using namespace std;

        cout << "debug ====" << endl;
        cout << "count = " << dec << count << endl;
        cout << "zero = " << zero << endl;
        cout << "next = " << hex << next << endl;
        cout << "debug ====" << endl;
        //rand->debug_print();
    }

    /**
     * calculate the dimensions of equidistribution with v-bit
     * accuracy, where v is form 1 to \b bit_len
     *
     * @param[out] veq array of dimensions of equidistribution at v
     * @return sum of the differences between the theoretical
     * upper bounds and the dimensions.
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_all_equidist(int veq[]) {
        using namespace std;

        int sum = 0;

        veq[bit_len - 1] = get_equidist_main(bit_len);
#ifdef DEBUG
        for (int i = 0; i < size; i++) {
            basis[i]->debug_print();
        }
#endif
        sum += mexp / bit_len - veq[bit_len - 1];
        bit_len--;
        for (; bit_len >= 1; bit_len--) {
            adjust(bit_len);
            veq[bit_len - 1] = get_equidist_main(bit_len);
            sum += mexp / bit_len - veq[bit_len - 1];
        }
        return sum;
    }

    /**
     * calculate the dimension of equidistribution with \b bit_len
     * accuracy, and additionally sum of the differences between the
     * theoretical upper bounds and veqs from veq is 1 to \b bit_len -1
     *
     * @param sum_equidist sum of the differences
     * @return the dimension of equidistribution at \b bit_len
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_equidist(int *sum_equidist) {
        using namespace std;

        int veq = get_equidist_main(bit_len);
        int sum = 0;
        bit_len--;
        for (; bit_len >= 1; bit_len--) {
            adjust(bit_len);
            sum += mexp / bit_len - get_equidist_main(bit_len);
        }
        *sum_equidist = sum;
        return veq;
    }

    /**
     * addition of vectors
     * @param src a vector which is added to this.
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::add(
        const linear_generator_vector<G, T>& src) {
        using namespace std;

        rand->add(*src.rand);
        next ^= src.next;
    }

    /**
     * transfer to the next state or n-th next state so that the
     * coeffcient of the maximum degree term should be non-zero. If internal
     * state is all zero, then set zero flag.
     *
     * @param bit_len bit length from MSB
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::next_state(int bit_len) {
        using namespace std;

        if (zero) {
            return;
        }
        int zero_count = 0;
        next = rand->generate(bit_len);
        count++;
        while (next == 0) {
            zero_count++;
            if (zero_count > rand->get_mexp() * 2) {
                zero = true;
                if (rand->is_zero()) {
                    zero = true;
                }
                break;
            }
            next = rand->generate(bit_len);
            count++;
        }
    }

    /**
     * Calculate dimension of equidistirbution with v bit accuracy for
     * one v.
     *
     * In this function, \b pivot_index is a important variable.  \b
     * pivot_index shows the position of the first bit which is one in
     * \b next of the vector of the last element of \b basis. And all
     * vectors in \b basis but last are sorted by pivot_index, so the
     * pivot_index of a vector of \b basis[0] is zero.
     *
     * @param bit_len bit length from MSB, so bit_Len is v.
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_equidist_main(int bit_len) {
        using namespace std;
        using namespace NTL;

        int pivot_index;
        int old_pivot = 0;

        pivot_index = calc_1pos(basis[bit_len]->next);
        while (!basis[bit_len]->zero) {
#ifdef DEBUG
            if (pivot_index != calc_1pos(basis[pivot_index]->next)) {
                cerr << "pivot error 1" << endl;
                cerr << "pivot_index:" << dec << pivot_index << endl;
                cerr << "calc_1pos:" << dec
                     << calc_1pos(basis[pivot_index]->next) << endl;
                cerr << "next:" << hex << basis[pivot_index]->next << endl;
                throw new std::logic_error("pivot error 1");
            }
#endif
            if (basis[bit_len]->count > basis[pivot_index]->count) {
                swap(basis[bit_len], basis[pivot_index]);
            }
            basis[bit_len]->add(*basis[pivot_index]);
            if (basis[bit_len]->next == 0) {
                basis[bit_len]->next_state(bit_len);
                pivot_index = calc_1pos(basis[bit_len]->next);
            } else {
                old_pivot = pivot_index;
                pivot_index = calc_1pos(basis[bit_len]->next);
                if (old_pivot <= pivot_index) {
                    cerr << "pivot error 2" << endl;
                    throw new std::logic_error("pivot error 2");
                }
            }
        }
        int min_count = basis[0]->count;
        for (int i = 1; i < bit_len; i++) {
            if (min_count > basis[i]->count) {
                min_count = basis[i]->count;
            }
        }
        if (min_count > mexp / bit_len) {
            cout << "over theoretical bound " << bit_len << endl;
            for(int i = 0; i < size; i++) {
                basis[i]->debug_print();
            }
            throw new std::logic_error("over theoretical bound");
        }
        return min_count;
    }
}
//  LocalWords:  equidistribution param endl NTL
#endif


#ifndef ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3
#define ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3

#include <limits>
#include <string>
#include <unordered_map>
#include <chrono>
#include <iostream>
#include <sstream>
#include <vector>

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

#include <anyode/anyode.hpp>

#if !defined(PYODEINT_NO_BOOST_CHECK)
  #if BOOST_VERSION / 100000 == 1
    #if BOOST_VERSION / 100 % 1000 == 60
      #error "Boost v 1.60 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
    #if BOOST_VERSION / 100 % 1000 == 61
      #error "Boost v 1.61 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
    #if BOOST_VERSION / 100 % 1000 == 62
      #error "Boost v 1.62 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
  #endif
#endif

namespace {
    class StreamFmt
    {
        std::stringstream m_s;
    public:
        StreamFmt() {}
        ~StreamFmt() {}

        template <typename T>
        StreamFmt& operator << (const T& v) {
            this->m_s << v;
            return *this;
        }

        std::string str() const {
            return this->m_s.str();
        }
        operator std::string() const {
            return this->m_s.str();
        }

    };
}

namespace odeint_anyode{
    using namespace std::placeholders;

    using boost::numeric::odeint::integrate_adaptive;
    using boost::numeric::odeint::make_dense_output;
    using boost::numeric::odeint::rosenbrock4;
    using boost::numeric::odeint::runge_kutta_dopri5;
    using boost::numeric::odeint::bulirsch_stoer_dense_out;

    // value_type is hardcoded to double at the moment
    using value_type = double;
    using vector_type = boost::numeric::ublas::vector<value_type>;
    using matrix_type = boost::numeric::ublas::matrix<value_type>;

    // using OdeSys_t = AnyODE::OdeSysBase;

    enum class StepType : int { bulirsch_stoer, rosenbrock4, dopri5 };

    StepType styp_from_name(std::string name){
        if (name == "bulirsch_stoer")
            return StepType::bulirsch_stoer;
        else if (name == "rosenbrock4")
            return StepType::rosenbrock4;
        else if (name == "dopri5")
            return StepType::dopri5;
        else
            throw std::runtime_error(StreamFmt() << "Unknown stepper type name: " << name);
    }

    bool requires_jacobian(StepType styp){
        if (styp == StepType::rosenbrock4)
            return true;
        else
            return false;
    }

    vector_type vec_from_ptr(const value_type * const arr, std::size_t len){
        vector_type vec(len);
        for (std::size_t i=0; i<len; ++i)
            vec[i] = arr[i];
        return vec;
    }

    // Integr will be specialzed for: rosenbrock, dopri5 and bulrisch-stoer
    // adaptive and predefined cannot be put here since make_dense_output
    // is a function and bulirsch_stoer_dense_out is a class
    template<class OdeSys>
    struct Integr {
        OdeSys * m_odesys;
        double m_time_cpu = -1.0, m_time_wall = -1.0;
        value_type m_dx0, m_dx_max, m_atol, m_rtol;
        StepType m_styp;
        long int m_mxsteps;
        int m_autorestart;
        long int m_nsteps;
        bool m_return_on_error;

        void rhs(const vector_type &yarr, vector_type &dydx, value_type xval);
        void jac(const vector_type & yarr, matrix_type &Jmat,
                 const value_type & xval, vector_type &dfdx);
        Integr(OdeSys * odesys, value_type dx0, value_type dx_max, value_type atol, value_type rtol, StepType styp,
               long int mxsteps, int autorestart=0, bool return_on_error=false) :
            m_odesys(odesys), m_dx0(dx0), m_dx_max(dx_max), m_atol(atol), m_rtol(rtol), m_styp(styp),
            m_mxsteps(mxsteps), m_autorestart(autorestart), m_return_on_error(return_on_error) {}

        std::pair<std::vector<value_type>, std::vector<value_type> >
        adaptive(const value_type x0,
                 const value_type xend,
                 const value_type * const ANYODE_RESTRICT y0){
            std::time_t cputime0 = std::clock();
            auto t_start = std::chrono::high_resolution_clock::now();
            std::pair<std::vector<value_type>, std::vector<value_type> > result;
            try{
                if ( m_styp == StepType::bulirsch_stoer ) {
                    this->adaptive_bulirsch_stoer(x0, xend, y0);
                } else if ( m_styp == StepType::dopri5 ) {
                    this->adaptive_dopri5(x0, xend, y0);
                } else if ( m_styp == StepType::rosenbrock4 ) {
                    this->adaptive_rosenbrock4(x0, xend, y0);
                } else {
                    goto impossible_adaptive;
                }
            } catch (const std::exception& e) {
                if (m_autorestart > 0){
                    std::cerr << e.what() << std::endl;
                    if (this->m_xout.size() > 0){
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart (" << m_autorestart
                                  << ") x=" << this->m_xout.back() << "\n";
                        m_autorestart--;
                        auto c_nsteps = this->m_nsteps;
                        auto c_xout = this->m_xout;
                        auto c_yout = this->m_yout;
                        adaptive(c_xout.back(), xend, &c_yout[c_yout.size() - m_odesys->get_ny()]);
                        c_xout.insert(c_xout.end(), m_xout.begin(), m_xout.end());
                        c_yout.insert(c_yout.end(), m_yout.begin(), m_yout.end());
                        m_xout = c_xout;
                        m_yout = c_yout;
                        m_nsteps += c_nsteps;
                    } else {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart failed." << "\n";
                        if (!m_return_on_error)
                            throw;
                    }
                } else {
                    if (!m_return_on_error)
                        throw;
                }
            }
            this->m_time_cpu = (std::clock() - cputime0) / (double)CLOCKS_PER_SEC;
            this->m_time_wall = std::chrono::duration<double>(
                std::chrono::high_resolution_clock::now() - t_start).count();
            return std::make_pair(this->m_xout, this->m_yout);
        impossible_adaptive:
            throw std::runtime_error("Impossible: unknown StepType!");
        }

        int predefined(const int nx,
                       const value_type * const ANYODE_RESTRICT xout,
                       const value_type * const ANYODE_RESTRICT y0,
                       value_type * const ANYODE_RESTRICT yout){
            int nreached;
            std::time_t cputime0 = std::clock();
            auto t_start = std::chrono::high_resolution_clock::now();
            std::copy(y0, y0 + (this->m_odesys->get_ny()), yout);
            try {
                if ( m_styp == StepType::bulirsch_stoer ) {
                    this->predefined_bulirsch_stoer(nx, xout, y0, yout, &nreached);
                } else if ( m_styp == StepType::dopri5 ) {
                    this->predefined_dopri5(nx, xout, y0, yout, &nreached);
                } else if ( m_styp == StepType::rosenbrock4 ) {
                    this->predefined_rosenbrock4(nx, xout, y0, yout, &nreached);
                } else {
                    goto impossible_predefined;
                }
            } catch (const std::exception& e) {
                if (m_autorestart > 0){
                    std::cerr << e.what() << std::endl;
                    if (this->m_xout.size() > 0) {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart (" << m_autorestart
                                  << ") x=" << this->m_xout.back() << "\n";
                        m_autorestart--;
                        auto c_nsteps = this->m_nsteps;
                        nreached += predefined(nx - nreached, xout + nreached,
                                               yout + nreached*m_odesys->get_ny(),
                                               yout + nreached*m_odesys->get_ny());
                        this->m_nsteps += c_nsteps;
                    } else {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart failed." << "\n";
                        if (!m_return_on_error)
                            throw;
                    }
                } else {
                    if (!m_return_on_error)
                        throw;
                }
            }
            this->m_time_cpu = (std::clock() - cputime0) / (double)CLOCKS_PER_SEC;
            this->m_time_wall = std::chrono::duration<double>(
                std::chrono::high_resolution_clock::now() - t_start).count();
            return nreached;
        impossible_predefined:
            throw std::runtime_error("Impossible: unknown StepType!");
        }
    private:
        std::vector<value_type> m_xout, m_yout;

        void reset() {
            this->m_nsteps = 0;
            this->m_xout.clear();
            this->m_yout.clear();
        }

        void obs_adaptive(const vector_type &yarr, value_type xval){
            this->m_xout.push_back(xval);
            for(int i=0 ; i < this->m_odesys->get_ny() ; ++i)
                this->m_yout.push_back(yarr[i]);
            if (this->m_nsteps == this->m_mxsteps)
                throw std::runtime_error(StreamFmt() << "Maximum number of steps reached: " << this->m_nsteps);
            m_nsteps++;
        }

        void obs_predefined(const vector_type & /* yarr */, value_type /* xval */){
            if (this->m_nsteps == this->m_mxsteps)
                throw std::runtime_error(StreamFmt() << "Maximum number of steps reached: " << this->m_nsteps);
            m_nsteps++;
        }

        void adaptive_bulirsch_stoer(const value_type x0,
                                     const value_type xend,
                                     const value_type * const ANYODE_RESTRICT y0
                                     ){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto stepper = bulirsch_stoer_dense_out< vector_type, value_type >(
                this->m_atol, this->m_rtol, 1.0, 1.0, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, f, y_, x0, xend, this->m_dx0,
                               std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_bulirsch_stoer(const int nx,
                                       const value_type * const ANYODE_RESTRICT xout,
                                       const value_type * const ANYODE_RESTRICT y0,
                                       value_type * const ANYODE_RESTRICT yout,
                                       int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            try{
                auto stepper = bulirsch_stoer_dense_out< vector_type, value_type >(
                    this->m_atol, this->m_rtol, 1.0, 1.0, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, f, y_, xout[ix-1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

        void adaptive_dopri5(const value_type x0,
                             const value_type xend,
                             const value_type * const ANYODE_RESTRICT y0){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };

            auto stepper = make_dense_output<runge_kutta_dopri5<vector_type, value_type> >(
                this->m_atol, this->m_rtol, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, f, y_, x0, xend, this->m_dx0,
                               std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_dopri5(const int nx,
                               const value_type * const ANYODE_RESTRICT xout,
                               const value_type * const ANYODE_RESTRICT y0,
                              value_type * const ANYODE_RESTRICT yout,
                              int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            try {
                auto stepper = make_dense_output<runge_kutta_dopri5<vector_type, value_type> >(
                    this->m_atol, this->m_rtol, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, f, y_, xout[ix - 1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

        void adaptive_rosenbrock4(const value_type x0,
                                  const value_type xend,
                                  const value_type * const ANYODE_RESTRICT y0){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto j = [&](const vector_type & yarr, matrix_type &Jmat,
                                     const value_type & xval, vector_type &dfdx) {
                this->m_odesys->dense_jac_rmaj(xval, &(yarr.data()[0]), nullptr, &(Jmat.data()[0]), ny, &(dfdx.data()[0]));
            };
            auto stepper = make_dense_output<rosenbrock4<value_type> >(this->m_atol, this->m_rtol, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, std::make_pair(f, j), y_, x0, xend, this->m_dx0,
                                              std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_rosenbrock4(const int nx,
                                    const value_type * const ANYODE_RESTRICT xout,
                                    const value_type * const ANYODE_RESTRICT y0,
                                    value_type * const ANYODE_RESTRICT yout,
                                    int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto j = [&](const vector_type & yarr, matrix_type &Jmat,
                                     const value_type & xval, vector_type &dfdx) {
                this->m_odesys->dense_jac_rmaj(xval, &(yarr.data()[0]), nullptr, &(Jmat.data()[0]), ny, &(dfdx.data()[0]));
            };
            try {
                auto stepper = make_dense_output<rosenbrock4<value_type> >(this->m_atol, this->m_rtol, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, std::make_pair(f, j), y_, xout[ix - 1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

    };

    template <class OdeSys>
    void set_integration_info(OdeSys * odesys, const Integr<OdeSys>& integrator){
        odesys->current_info.nfo_int["n_steps"] = integrator.m_nsteps;
        odesys->current_info.nfo_int["nfev"] = odesys->nfev;
        odesys->current_info.nfo_int["njev"] = odesys->njev;
        odesys->current_info.nfo_dbl["time_wall"] = integrator.m_time_wall;
        odesys->current_info.nfo_dbl["time_cpu"] = integrator.m_time_cpu;
    }

    template <class OdeSys>
    std::pair<std::vector<double>, std::vector<double> >
    simple_adaptive(OdeSys * const odesys,
                    const double atol,
                    const double rtol,
                    const StepType styp,
                    const double * const y0,
                    const double x0,
                    const double xend,
                    long int mxsteps=0,
                    double dx0=0.0,
                    double dx_max=0.0,
                    int autorestart=0,
                    bool return_on_error=false
                    )
                    //,
                    // const double dx_min=0.0,

                    // long int mxsteps=0)
    {
        if (dx0 == 0.0)
            dx0 = odesys->get_dx0(x0, y0);
        if (dx0 == 0.0){
            if (x0 == 0)
                dx0 = std::numeric_limits<double>::epsilon() * 100;
            else
                dx0 = std::numeric_limits<double>::epsilon() * 100 * x0;
        }
        if (dx_max == 0.0)
            dx_max = odesys->get_dx_max(x0, y0);
        if (mxsteps == 0)
            mxsteps = 500;
        auto integr = Integr<OdeSys>(odesys, dx0, dx_max, atol, rtol, styp, mxsteps, autorestart, return_on_error);
        auto result = integr.adaptive(x0, xend, y0);
        odesys->current_info.clear();
        set_integration_info<OdeSys>(odesys, integr);
        return result;
    }

    template <class OdeSys>
    int simple_predefined(OdeSys * const odesys,
                          const double atol,
                          const double rtol,
                          const StepType styp,
                          const double * const y0,
                          const int nout,
                          const double * const xout,
                          double * const yout,
                          long int mxsteps=0,
                          double dx0=0.0,
                          double dx_max=0.0,
                          int autorestart=0,
                          bool return_on_error=false
                          )
    // const double dx_min=0.0,
    {
        if (dx0 == 0.0)
            dx0 = odesys->get_dx0(xout[0], y0);
        if (dx0 == 0.0){
            if (xout[0] == 0)
                dx0 = std::numeric_limits<double>::epsilon() * 100;
            else
                dx0 = std::numeric_limits<double>::epsilon() * 100 * xout[0];
        }
        if (dx_max == 0.0)
            dx_max = INFINITY;
        if (mxsteps == 0)
            mxsteps = 500;
        auto integr = Integr<OdeSys>(odesys, dx0, dx_max, atol, rtol, styp, mxsteps, autorestart, return_on_error);
        int nreached = integr.predefined(nout, xout, y0, yout);
        odesys->current_info.nfo_int.clear();
        odesys->current_info.nfo_dbl.clear();
        set_integration_info(odesys, integr);
        return nreached;
    }

}

#endif /* ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3 */


#pragma once

#include <vector>
#include <algorithm>
#include <random>
#include <Eigen/Dense>

namespace cilantro {
    // CRTP base class
    template <class ModelEstimatorT, class ModelT, typename ResidualScalarT, typename IndexT = size_t>
    class RandomSampleConsensusBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef ModelT Model;
        typedef ResidualScalarT ResidualScalar;
        typedef std::vector<ResidualScalarT> ResidualVector;
        typedef IndexT Index;
        typedef std::vector<IndexT> IndexVector;

        RandomSampleConsensusBase(size_t sample_size,
                                  size_t inlier_count_thresh,
                                  size_t max_iter,
                                  ResidualScalar inlier_dist_thresh,
                                  bool re_estimate = true)
                : sample_size_(sample_size),
                  inlier_count_thresh_(inlier_count_thresh),
                  max_iter_(max_iter),
                  inlier_dist_thresh_(inlier_dist_thresh),
                  re_estimate_(re_estimate),
                  iteration_count_(0)
        {}

        inline size_t getSampleSize() const { return sample_size_; }

        inline ModelEstimatorT& setSampleSize(size_t sample_size) {
            sample_size_ = sample_size;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline size_t getTargetInlierCount() const { return inlier_count_thresh_; }

        inline ModelEstimatorT& setTargetInlierCount(size_t inlier_count_thres) {
            inlier_count_thresh_ = inlier_count_thres;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline size_t getMaxNumberOfIterations() const { return max_iter_; }

        inline ModelEstimatorT& setMaxNumberOfIterations(size_t max_iter) {
            max_iter_ = max_iter;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline ResidualScalar getMaxInlierResidual() const { return inlier_dist_thresh_; }

        inline ModelEstimatorT& setMaxInlierResidual(ResidualScalar inlier_dist_thresh) {
            inlier_dist_thresh_ = inlier_dist_thresh;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline bool getReEstimationStep() const { return re_estimate_; }

        inline ModelEstimatorT& setReEstimationStep(bool re_estimate) {
            re_estimate_ = re_estimate;
            return *static_cast<ModelEstimatorT*>(this);
        }

        ModelEstimatorT& estimate() {
            ModelEstimatorT& estimator = *static_cast<ModelEstimatorT*>(this);
            const size_t num_points = estimator.getDataPointsCount();
            if (num_points < sample_size_) sample_size_ = num_points;
            if (inlier_count_thresh_ > num_points) inlier_count_thresh_ = num_points;

            // Initialize index permutation and random engine
            IndexVector perm(num_points);
            for (size_t i = 0; i < num_points; i++) perm[i] = static_cast<IndexT>(i);
            std::mt19937 rng(std::random_device{}());

            // Random sample results
            Model curr_params;
            ResidualVector curr_residuals;
            IndexVector curr_inliers;

            iteration_count_ = 0;
            while (iteration_count_ < max_iter_) {
                // Pick a random sample
                IndexVector sample_ind(sample_size_);
                size_t prev_size = num_points;
                for (size_t i = 0; i < sample_size_; i++) {
                    std::uniform_int_distribution<size_t> dist(0, prev_size - 1);
                    size_t rand_ind = dist(rng);
                    sample_ind[i] = perm[rand_ind];
                    prev_size--;
                    std::swap(perm[rand_ind], perm[prev_size]);
                }

                // Fit model to sample and get its inliers
                estimator.estimateModel(sample_ind, curr_params);
                estimator.computeResiduals(curr_params, curr_residuals);
                curr_inliers.resize(num_points);
                size_t k = 0;
                for (size_t i = 0; i < num_points; i++) {
                    if (curr_residuals[i] <= inlier_dist_thresh_) curr_inliers[k++] = static_cast<IndexT>(i);
                }
                curr_inliers.resize(k);

                iteration_count_++;
                if (curr_inliers.size() < sample_size_) continue;

                // Update best found
                if (curr_inliers.size() > model_inliers_.size()) {
                    model_params_ = curr_params;
                    model_residuals_ = std::move(curr_residuals);
                    model_inliers_ = std::move(curr_inliers);
                }

                // Check if target inlier count was reached
                if (model_inliers_.size() >= inlier_count_thresh_) break;
            }

            // Re-estimate
            if (re_estimate_) {
                estimator.estimateModel(model_inliers_, model_params_);
                estimator.computeResiduals(model_params_, model_residuals_);
                model_inliers_.resize(num_points);
                size_t k = 0;
                for (size_t i = 0; i < num_points; i++){
                    if (model_residuals_[i] <= inlier_dist_thresh_) model_inliers_[k++] = static_cast<IndexT>(i);
                }
                model_inliers_.resize(k);
            }

            return estimator;
        }

        inline ModelEstimatorT& estimate(ResidualScalar max_residual,
                                         size_t target_inlier_count,
                                         size_t max_iter)
        {
            inlier_count_thresh_ = target_inlier_count;
            max_iter_ = max_iter;
            inlier_dist_thresh_ = max_residual;
            return estimate();
        }

        inline const ModelEstimatorT& getEstimationResults(Model &model_params,
                                                           ResidualVector &model_residuals,
                                                           IndexVector &model_inliers) const
        {
            model_params = model_params_;
            model_residuals = model_residuals_;
            model_inliers = model_inliers_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const Model& getModel() const { return model_params_; }

        inline const ModelEstimatorT& getModel(Model &model_params) const {
            model_params = model_params_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const ResidualVector& getModelResiduals() const { return model_residuals_; }

        inline const ModelEstimatorT& getModelResiduals(ResidualVector &model_residuals) const {
            model_residuals = model_residuals_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const IndexVector& getModelInliers() const { return model_inliers_; }

        inline const ModelEstimatorT& getModelInliers(IndexVector &model_inliers) const {
            model_inliers = model_inliers_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline bool targetInlierCountAchieved() const { return model_inliers_.size() >= inlier_count_thresh_; }

        inline size_t getNumberOfPerformedIterations() const { return iteration_count_; }

        inline size_t getNumberOfInliers() const { return model_inliers_.size(); }

    protected:
        // Parameters
        size_t sample_size_;
        size_t inlier_count_thresh_;
        size_t max_iter_;
        ResidualScalar inlier_dist_thresh_;
        bool re_estimate_;

        // Object state and results
        size_t iteration_count_;
        Model model_params_;
        ResidualVector model_residuals_;
        IndexVector model_inliers_;
    };
}


/*
 * WaveEquation.cc
 *
 *  Created on: 05.05.2017
 *      Author: thies
 */

/*
 * based on step23.cc from the deal.II tutorials
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <forward/WaveEquation.h>

#include <stddef.h>
#include <iostream>
#include <map>
#include <string>

#include <deal.II/base/data_out_base.h>
#include <deal.II/numerics/data_out.h>
#include <fstream>

namespace wavepi {
namespace forward {

using namespace dealii;
using namespace wavepi::base;

template<int dim>
WaveEquation<dim>::WaveEquation(std::shared_ptr<SpaceTimeMesh<dim>> mesh)
      : AbstractEquation<dim>(mesh), initial_values_u(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            initial_values_v(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            boundary_values_u(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            boundary_values_v(std::make_shared<Functions::ZeroFunction<dim>>(1)) {
}

template<int dim>
WaveEquation<dim>::WaveEquation(const WaveEquation<dim> &wave)
      : AbstractEquation<dim>(wave.get_mesh()), initial_values_u(wave.get_initial_values_u()),
            initial_values_v(wave.get_initial_values_v()), boundary_values_u(wave.get_boundary_values_u()),
            boundary_values_v(wave.get_boundary_values_v()) {

   this->set_param_c(wave.get_param_c());
   this->set_param_nu(wave.get_param_nu());
   this->set_param_rho(wave.get_param_rho());
   this->set_param_q(wave.get_param_q());
   this->set_rho_time_dependent(wave.is_rho_time_dependent());

   this->set_theta(wave.get_theta());
   this->set_solver_tolerance(wave.get_solver_tolerance());
   this->set_solver_max_iter(wave.get_solver_max_iter());
}

template<int dim>
void WaveEquation<dim>::apply_boundary_conditions_u(double time) {
   boundary_values_u->set_time(time);
   boundary_values_u->set_time(time);

   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *boundary_values_u, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_u, system_rhs);
}

template<int dim>
void WaveEquation<dim>::apply_boundary_conditions_v(double time) {
   boundary_values_v->set_time(time);

   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *boundary_values_v, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_v, system_rhs);
}

template<int dim>
void WaveEquation<dim>::initial_values(double time) {
   initial_values_u->set_time(time);
   initial_values_v->set_time(time);

   /* projecting might make more sense, but VectorTools::project
    leads to a mutex error (deadlock) on my laptop (Core i5 6267U) */
   //   VectorTools::project(*dof_handler, constraints, QGauss<dim>(3), *initial_values_u, old_solution_u);
   //   VectorTools::project(*dof_handler, constraints, QGauss<dim>(3), *initial_values_v, old_solution_v);
   VectorTools::interpolate(*dof_handler, *initial_values_u, solution_u);
   constraints->distribute(solution_u);

   VectorTools::interpolate(*dof_handler, *initial_values_v, solution_v);
   constraints->distribute(solution_v);
}

template<int dim>
void WaveEquation<dim>::assemble_matrices(size_t time_idx) {
   LogStream::Prefix p("assemble_matrices");

   this->fill_matrices(mesh, time_idx, matrix_A, matrix_B, matrix_C);
}

template<int dim>
DiscretizedFunction<dim> WaveEquation<dim>::run(std::shared_ptr<RightHandSide<dim>> right_hand_side,
      typename AbstractEquation<dim>::Direction direction) {
   {
      LogStream::Prefix p("WaveEq");
      LogStream::Prefix pp("BoundChecking");

      // bound checking for ρ and c (if possible)
      // (should not take long compared to the rest and can be very tricky to find out otherwise
      //    -> do it even in release mode)
      if (this->param_c_disc) {
         double c_min, c_max;
         this->param_c_disc->min_max_value(&c_min, &c_max);

         std::stringstream bound_str;
         bound_str << c_min << " ≤ c ≤ " << c_max;

         AssertThrow(c_min > 0, ExcMessage("C is not positive, " + bound_str.str()));
         AssertThrow(1.0 / (c_max * c_max) >= 1e-4, ExcMessage("C is not coercive, " + bound_str.str()));

         deallog << bound_str.str() << std::endl;
      }

      if (this->param_rho_disc) {
         double rho_min, rho_max;
         this->param_rho_disc->min_max_value(&rho_min, &rho_max);

         std::stringstream bound_str;
         bound_str << rho_min << " ≤ ρ ≤ " << rho_max;

         AssertThrow(rho_min > 0, ExcMessage("A and D are not positive, " + bound_str.str()));
         AssertThrow(1.0 / rho_max >= 1e-4, ExcMessage("A and D are not coercive, " + bound_str.str()));

         deallog << bound_str.str() << std::endl;
      }
   }

   return AbstractEquation<dim>::run(right_hand_side, direction);
}

template class WaveEquation<1> ;
template class WaveEquation<2> ;
template class WaveEquation<3> ;

} /* namespace forward */
} /* namespace wavepi */


#ifndef GIFS_CONVERSION_H
#define GIFS_CONVERSION_H

#include <algorithm>
#include <armadillo>

/* Direct conversion! */
class Conversion
{
public:
    //
    explicit Conversion(double mass, double length, double time);
    Conversion(const Conversion& rhs) = default;
    Conversion(Conversion&& rhs) = default;
    Conversion& operator=(const Conversion& rhs) = default;
    Conversion& operator=(Conversion&& rhs) = default;

    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_crd_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->crd_md2au(value); });
    };
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_veloc_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->veloc_md2au(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_veloc_au2md(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->veloc_au2md(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_gradient_au2md(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->grd_au2md(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_gradient_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->grd_md2au(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_masses_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->mass_md2au(value); });
    }
    //
    inline double energy_au2md(double value) const noexcept {return _energy_au2md*value;}
    inline double energy_md2au(double value) const noexcept {return _energy_md2au*value;}
    //
    inline double crd_au2md(double value) const noexcept {return _crd_au2md*value;}
    inline double crd_md2au(double value) const noexcept {return _crd_md2au*value;}
    //
    inline double veloc_au2md(double value) const noexcept {return _veloc_au2md*value;}
    inline double veloc_md2au(double value) const noexcept {return _veloc_md2au*value;}
    //
    inline double grd_au2md(double value) const noexcept {return _grd_au2md*value;}
    inline double grd_md2au(double value) const noexcept {return _grd_md2au*value;}
    //
    inline double mass_au2md(double value) const noexcept {return _mass_au2md*value;}
    inline double mass_md2au(double value) const noexcept {return _mass_md2au*value;}

private:
    // energies
    double _energy_au2md;
    double _energy_md2au;
    // coordinates
    double _crd_au2md;
    double _crd_md2au;
    // velocities
    double _veloc_au2md;
    double _veloc_md2au;
    // gradient
    double _grd_au2md;
    double _grd_md2au;
    // mass
    double _mass_au2md;
    double _mass_md2au;
    //
};

#endif


/*
 * File:   image_noise_sim.cc
 * Author: ts337
 *
 * Created on 22 March 2011, 18:49
 */

#include "../image_noise_sim.h"
#include <boost/random/poisson_distribution.hpp>
#include<iostream>

namespace coela {
namespace image_noise_simulation {

uniform_random_pixel_variate::uniform_random_pixel_variate(
    const PixelRange& image_outline_,
    unuran::StreamWrapper& rns)
    : urv(0.0, image_outline_.n_pix(), rns), range(image_outline_)
{}

PixelIndex uniform_random_pixel_variate::operator()()
{
    int data_vec_index = int(urv()) ;
    return range.get_PixelIndex_for_data_vector_element(data_vec_index);
}

intensity_map_random_pixel_variate::intensity_map_random_pixel_variate(
    const PixelArray2d<double>& intensity_map,
    unuran::StreamWrapper& rns,
    const size_t /*expected_number_of_uses*/)
    :gen(NULL), range(intensity_map.range())
{
    UNUR_DISTR *distr;    /* distribution object                   */
    UNUR_PAR   *par;      /* parameter object                      */

    distr = unur_distr_discr_new();

    const vector<double>& prob_vec(intensity_map.get_raw_data());

    unur_distr_discr_set_pv(distr, &prob_vec[0] , prob_vec.size());
//    par = unur_dgt_new(distr);
//    unur_dgt_set_guidefactor(par, std::min(3.0/sqrt(prob_vec.size()), 1.0) );

    par = unur_dau_new(distr);
    unur_set_urng(par, rns.get_stream_pointer());
    gen = unur_init(par);
    unur_distr_free(distr);

    if (gen==NULL) { throw std::runtime_error("intensity_map_random_pixel_variate::intensity_map_random_pixel_variate - could not construct generator."); }

}

PixelIndex intensity_map_random_pixel_variate::operator()()
{
    int data_vec_index = unur_sample_discr(gen);
    return range.get_PixelIndex_for_data_vector_element(data_vec_index);
}

PixelArray2d<int> generate_photon_arrival_map(
    const PixelArray2d<double>& source_intensity_map,
    const double mean_total_number_of_source_photons,
    const double mean_bg_photons_per_pixel,
    unuran::StreamWrapper& rns)
{

    unuran::PoissonRandomVariate source_total_prv(
        mean_total_number_of_source_photons, rns);

    int n_source_photons = source_total_prv();
//    return photon_arrival_map_fixed_n_low_flux_optimized(intensity_map, n_photons, rns);

    PixelArray2d<int> source_photon_arrival_map(
        source_intensity_map.range().x_dim(), source_intensity_map.range().y_dim(), 0);

    intensity_map_random_pixel_variate pixel_picker(source_intensity_map, rns);

    for (int phot_index=0; phot_index!=n_source_photons; ++phot_index) {
        source_photon_arrival_map(pixel_picker())+=1;
    }

    if (mean_bg_photons_per_pixel!=0.0) {
        double mean_bg_sum_flux =
            source_photon_arrival_map.range().n_pix() * mean_bg_photons_per_pixel;

        unuran::PoissonRandomVariate bg_total_prv(mean_bg_sum_flux, rns);
        int n_bg_photons = bg_total_prv();
        uniform_random_pixel_variate bg_pixel_picker(source_photon_arrival_map.range(),
                rns);
        for (int bg_phot_gen=0; bg_phot_gen!=n_bg_photons; ++bg_phot_gen) {
            source_photon_arrival_map(bg_pixel_picker()) +=1;
        }
    }
    return source_photon_arrival_map;

}

PixelArray2d<int> EMCCD_simulated_image(
    const PixelArray2d<int> & photon_arrival_map,
    EmccdModel& model_variate,
    unuran::StreamWrapper& rns)
{
    PixelArray2d<int> sim_image(photon_arrival_map.range().x_dim(),
                                photon_arrival_map.range().y_dim(), 0);

    //Simulate photon events
    for (PixelIterator i(photon_arrival_map.range()); i!=i.end; ++i) {
        if (photon_arrival_map(i)) {
            sim_image(i) +=
                model_variate.stochastically_multiply_photons(photon_arrival_map(i));
        }
    }

    //simulate CIC events
    if (model_variate.params.serial_CIC_rate!=0) {
        int n_CIC_events;

        double expected_num_CIC_events = model_variate.params.serial_CIC_rate *
                                         sim_image.range().n_pix() ;

        unuran::PoissonRandomVariate n_CIC_events_variate(
            expected_num_CIC_events,
            rns);
        n_CIC_events = n_CIC_events_variate();
        uniform_random_pixel_variate pixel_picker(sim_image.range(), rns);

        for (int i=0; i!=n_CIC_events; ++i) {
//            int pixel_number = int( pixel_picker() ) + 1;
//            PixelIndex pix = sim_image.range().get_nth_PixelIndex(pixel_number);
            PixelIndex pix = pixel_picker();
            sim_image(pix) += model_variate.CICIR_variate();
        }
    }
    //Simulate readout
    for (PixelIterator i(sim_image.range()); i!=i.end; ++i) {
        sim_image(i)+=model_variate.readout_noise_variate();
    }

    return sim_image;
}

}//end namespace coela::image_noise_simulation
}//end namespace coela


#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <iostream>
#include <memory>

int cell = 16;
int bin_num = 8;

struct Intrinsics{
    double fx;
    double fy;
    double cx;
    double cy;
};

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in);

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset);

class NID{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    void set_member_size();
    void ComputeHref();
    void ComputeH();

    int cell_ = 16;
    int bin_num_ ;
    Eigen::VectorXd intensity0_;
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > pw0_;
    Eigen::Matrix4d tf_;
    cv::Mat image1_;
    double fx_;
    double fy_;
    double cx_;
    double cy_;
    std::vector<double> pro_ref_, pro_current_;
    std::vector<std::vector<double> > pro_joint_;
    double sigma_ = 1e-30;
    double H_ref_ = 0.0, H_current_ = 0.0, H_joint_ = 0.0;
    Eigen::VectorXd intensity_current_;
    double nid_, MI_;

    inline double get_interpolated_pixel_value ( double x, double y )
    {
        int ix = (int)x;
        int iy = (int)y;

        double dx = x - ix;
        double dy = y - iy;
        double dxdy = dx*dy;

        double xx = x - floor ( x );
        double yy = y - floor ( y );

        return double (
        dxdy * image1_.ptr<uchar>(iy+1)[ix+1]
        + (dy - dxdy) * image1_.ptr<uchar>(iy+1)[ix]
        + (dx - dxdy) * image1_.ptr<uchar>(iy)[ix+1]
        + (1 - dx - dy + dxdy) * image1_.ptr<uchar>(iy)[ix]
        );
    }
};

int main(int argc, char** argv){
    if(argc!=2){
        std::cout<<"usage './program path_to_config.yaml', image1's timestamp should be smaller than image2"<<std::endl;
        exit(0);
    }

    std::string config_add = argv[1];
    cv::FileStorage fc;
    fc.open(config_add, cv::FileStorage::READ);
    std::string type0 = fc["image0_type"] , type1 = fc["image1_type"], id0 = fc["image0_id"], id1 = fc["image1_id"], use_gt = fc["use_groundtruth"], dataset = fc["dataset"], im_add = fc["im_address"];
    double depth_factor = 1.0/(int)fc["depth_factor"], fx = fc["fx"], fy = fc["fy"], cx = fc["cx"], cy = fc["cy"];

    cv::Mat im0,im1;
    std::string rgb0_add, rgb1_add;
    if(dataset == "eth_cvg"){
      rgb0_add = im_add + type0 + "/" + id0 + ".png";
      rgb1_add = im_add + type1 + "/" + id1 + ".png";
    }

    cv::Mat im_rgb0 = cv::imread(rgb0_add,CV_LOAD_IMAGE_UNCHANGED);//use cv::IMREAD_GRAYSCALE, the result will be different
    im0 = im_rgb0;
    cvtColor(im0,im0,CV_RGB2GRAY);

    cv::Mat im_rgb1 = cv::imread(rgb1_add,CV_LOAD_IMAGE_UNCHANGED);
    im1 = im_rgb1;
    cvtColor(im1,im1,CV_RGB2GRAY);

    std::cout<<"image size "<<im0.size()<<","<<im1.size()<<std::endl;

    //read corresponding depth
    cv::Mat depth0;
    if(dataset == "eth_cvg"){
      std::string depth_add = im_add + "depth/" + id0 + ".png";
      depth0 = cv::imread(depth_add, CV_LOAD_IMAGE_UNCHANGED);
      depth0.convertTo(depth0,CV_64F,depth_factor);
    }

    std::string pose_gt_add;
    if(dataset == "eth_cvg"){
      pose_gt_add = im_add + "groundtruth.txt";
    }
    std::vector<cv::Mat> gt = ReadGroundtruth(pose_gt_add, dataset);
    int pose_id0 = stoi(id0);
    int pose_id1 = stoi(id1);

    cv::Mat T_wc0_cv,T_wc1_cv, T_cw0_cv, T_cw1_cv;

    //pose id output from slam needs to corresponds to the id input of this program
    cv::FileStorage fp0, fp1;
    if(use_gt == "0"){
      if(fp0.open(std::to_string(pose_id0) + ".xml", cv::FileStorage::READ) && fp1.open(std::to_string(pose_id1) + ".xml", cv::FileStorage::READ)){
        std::cout<<"use the pose from etimation"<<std::endl;
        fp0["pose"]>>T_cw0_cv;
        fp1["pose"]>>T_cw1_cv;
        T_cw0_cv.convertTo(T_cw0_cv, CV_64F);
        T_cw1_cv.convertTo(T_cw1_cv, CV_64F);
        T_wc0_cv = T_cw0_cv.inv();
        T_wc1_cv = T_cw1_cv.inv();
      }
      else{
        std::cout<<"no correspoding pose, exit "<<std::endl;
        exit(0);
      }
    }
    else if(use_gt == "1"){
        std::cout<<"use groundtruth pose "<<std::endl;
        T_wc0_cv = gt[pose_id0];
        T_wc1_cv = gt[pose_id1];
    }
    else{
      std::cout<<"you must have pose provided by groundtruth or SLAM"<<std::endl;
    }

    //std::cout<<"pose id is "<<pose_id<<std::endl;
    Intrinsics in;
    in.fx = fx;
    in.fy = fy;
    in.cx = cx;
    in.cy = cy;

    std::vector<std::shared_ptr<NID> > nid_vec;
    Eigen::Matrix4d T_wc0, T_wc1;
    T_wc0<<T_wc0_cv.ptr<double>(0)[0], T_wc0_cv.ptr<double>(0)[1], T_wc0_cv.ptr<double>(0)[2], T_wc0_cv.ptr<double>(0)[3],
          T_wc0_cv.ptr<double>(1)[0], T_wc0_cv.ptr<double>(1)[1], T_wc0_cv.ptr<double>(1)[2], T_wc0_cv.ptr<double>(1)[3],
          T_wc0_cv.ptr<double>(2)[0], T_wc0_cv.ptr<double>(2)[1], T_wc0_cv.ptr<double>(2)[2], T_wc0_cv.ptr<double>(2)[3],
          0                              , 0                              , 0                              , 1;
    T_wc1<<T_wc1_cv.ptr<double>(0)[0], T_wc1_cv.ptr<double>(0)[1], T_wc1_cv.ptr<double>(0)[2], T_wc1_cv.ptr<double>(0)[3],
          T_wc1_cv.ptr<double>(1)[0], T_wc1_cv.ptr<double>(1)[1], T_wc1_cv.ptr<double>(1)[2], T_wc1_cv.ptr<double>(1)[3],
          T_wc1_cv.ptr<double>(2)[0], T_wc1_cv.ptr<double>(2)[1], T_wc1_cv.ptr<double>(2)[2], T_wc1_cv.ptr<double>(2)[3],
          0                              , 0                              , 0                              , 1;

    std::ofstream of("nid_test.csv", std::ofstream::out | std::ofstream::app);

    const double px_ori = T_wc0(0,3);
    const Eigen::Matrix4d SE3_ori= T_wc0;

    //calculate entropy of each cell
    double total_nid = 0.0;
    for(int i = 0; i<cell; i++){
        for(int j = 0; j<cell; j++){
            std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > points_3d;
            Eigen::VectorXd intensity;
            Get3dPointAndIntensity(cell, i, j, im0, depth0, points_3d, intensity, T_wc0, in);
            std::shared_ptr<NID> nid = std::make_shared<NID>();
            nid->cell_ = cell;
            nid->bin_num_ = bin_num;
            nid->image1_ = im1;
            nid->fx_ = in.fx;
            nid->fy_ = in.fy;
            nid->cx_ = in.cx;
            nid->cy_ = in.cy;
            nid->pw0_ = points_3d;
            nid->intensity0_ = intensity;
            nid->tf_ = T_wc1;
            nid->set_member_size();
            nid->ComputeHref();
            nid->ComputeH();
            total_nid += nid->nid_ * nid->nid_;
            //exit(0);
        }
    }

    std::cout<<"final nid is "<<sqrt(total_nid)<<std::endl;

    return 0;

}

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in){
    int row_block = image.rows / cell_size;
    int col_block = image.cols / cell_size;
    int row_start = row_block * cell_row_id;
    int col_start = col_block * cell_col_id;
    int row_end = row_block * (cell_row_id + 1);
    int col_end = col_block * (cell_col_id + 1);
    int counter = 0;

    std::vector<double> intensity_v;
    for(int i = row_start; i < row_end; i++)
        for(int j = col_start; j < col_end; j++){
            double z_p = depth.ptr<double>(i)[j];
            counter++;
            if(z_p <0.01 || z_p > 100)
                continue;

            double x_p = z_p * (j - in.cx) / in.fx;
            double y_p = z_p * (i - in.cy) / in.fy;

            Eigen::Vector3d p_world = (T_wc*Eigen::Vector4d(x_p,y_p,z_p,1)).head(3);

            points_3d.push_back(p_world);
            intensity_v.push_back(image.ptr<uchar>(i)[j]);

        }

    intensity = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(intensity_v.data(), intensity_v.size());

    return;
};

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset){
  std::vector<cv::Mat> all_gt;

  std::ifstream groundtruth_file(add.c_str());
  if(!groundtruth_file){
      printf("cannot find the file that contains groundtruth \n");
      return all_gt;
  }

  int counter = 0;
  std::string one_row_gt;

  while(getline(groundtruth_file,one_row_gt)){
      std::istringstream temp_one_row_gt(one_row_gt);
      std::string string_gt;
      Eigen::Matrix3d rm;
      double px,py,pz;
      int sequence = 0;
      if (dataset == "eth_cvg"){
      double qx,qy,qz,qw;
      while(getline(temp_one_row_gt, string_gt, ' ')){
        switch(sequence){
          case 0:{
              int msg_timestamp = atoi(string_gt.c_str());
              sequence++;
          }
              break;
          case 1:{
              //the case 1,2,3 is position x,y,z
              px = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 2:{
              py = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 3:{
              pz = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 4:{
              //the case 4,5,6,7 is quternion x,y,z,w
              qx = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 5:{
              qy = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 6:{
              qz = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 7:{
              qw = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          default:
              break;
        }
      }
      Eigen::Quaterniond qt(qw,qx,qy,qz);
      rm = qt.toRotationMatrix();
    }

    cv::Mat one_gt(4,4,CV_64FC1);
    one_gt.ptr<double>(0)[0] = rm(0,0);
    one_gt.ptr<double>(0)[1] = rm(0,1);
    one_gt.ptr<double>(0)[2] = rm(0,2);
    one_gt.ptr<double>(1)[0] = rm(1,0);
    one_gt.ptr<double>(1)[1] = rm(1,1);
    one_gt.ptr<double>(1)[2] = rm(1,2);
    one_gt.ptr<double>(2)[0] = rm(2,0);
    one_gt.ptr<double>(2)[1] = rm(2,1);
    one_gt.ptr<double>(2)[2] = rm(2,2);
    one_gt.ptr<double>(3)[0] = 0.0;
    one_gt.ptr<double>(3)[1] = 0.0;
    one_gt.ptr<double>(3)[2] = 0.0;

    one_gt.ptr<double>(0)[3] = px;
    one_gt.ptr<double>(1)[3] = py;
    one_gt.ptr<double>(2)[3] = pz;
    one_gt.ptr<double>(3)[3] = 1.0;

    all_gt.push_back(one_gt);

  }

  //std::cout<<"size of all gt "<<all_gt.size()<<std::endl;

  return all_gt;
};

void NID::set_member_size(){
    pro_current_ = std::vector<double>(bin_num_,0.0);
    pro_ref_ = std::vector<double>(bin_num_,0.0);
    pro_joint_ = std::vector<std::vector<double> >(bin_num_);
    for(int i = 0; i < bin_num_; i++){
        pro_joint_[i] = std::vector<double>(bin_num_, 0.0);
    }
    intensity_current_ = Eigen::VectorXd::Zero(intensity0_.rows());
}

void NID::ComputeHref(){

  Eigen::Matrix4d T_cw1 = tf_.inverse();
  int ob = 0;
  for(int i = 0 ; i<intensity0_.rows(); i++){

    //decide if there is a valid mapping first, or we won't count this pixel
    Eigen::Vector4d pw;
    pw << pw0_[i](0), pw0_[i](1), pw0_[i](2), 1;
    Eigen::Vector4d p_c = T_cw1 * pw;

    //2d pixel position in current frame
    double u = fx_ * p_c(0,0) / p_c(2,0) + cx_;
    double v = fy_ * p_c(1,0) / p_c(2,0) + cy_;

    //bilinear interporlation of pixel. DSO getInterpolatedElement33() function, bilinear interpolation
    if(u >= 0 && u+3 <= image1_.cols && v >= 0 && v+3 <= image1_.rows){
      intensity_current_(i,0) = get_interpolated_pixel_value(u,v);
    }
    else{
      ob++;
      continue;
    }

    //current frame mutual information probability
    if(intensity0_(i,0) >= 255)
      intensity0_(i,0) = 254.999;
    if(intensity0_(i,0) < 0)
      intensity0_(i, 0) = 0.0;

    double bin_pos_ref =  intensity0_(i,0) * bin_num_/255.0;

    int bins_index_ref = floor(bin_pos_ref);

    if(bins_index_ref < 0 || bins_index_ref>bin_num_ -1)
        std::cout<<"the intensity is \n"<<intensity0_<<std::endl;

    pro_ref_[bins_index_ref] += 1.0;

  }

  for(int i = 0; i < bin_num_ ; i++)
    pro_ref_[i] /=  (intensity0_.rows()-ob);

  for(int i = 0; i < bin_num_ ; i++){
    if(pro_ref_[i] < sigma_)
      continue;
    H_ref_ -= pro_ref_[i] * log2(pro_ref_[i]);
  }

};

void NID::ComputeH(){
  int ob = 0;

  Eigen::Matrix4d T_cw1 = tf_.inverse();
  for(int i = 0 ; i<intensity0_.rows(); i++){

    Eigen::Vector4d pw;
    pw << pw0_[i](0), pw0_[i](1), pw0_[i](2), 1;
    Eigen::Vector4d p_c = T_cw1 * pw;

    if(intensity0_(i,0) >= 255)
      intensity0_(i,0) = 254.999;
    if(intensity0_(i,0) < 0)
      intensity0_(i, 0) = 0.0;

    double bin_pos_ref =  intensity0_(i,0)* bin_num_/255.0;
    int bins_index_ref = floor(bin_pos_ref);

    //2d pixel position in current frame
    double u = fx_ * p_c(0,0) / p_c(2,0) + cx_;
    double v = fy_ * p_c(1,0) / p_c(2,0) + cy_;

    //bilinear interporlation of pixel. DSO getInterpolatedElement33() function, bilinear interpolation
    if(u >= 0 && u+3 <= image1_.cols && v >= 0 && v+3 <= image1_.rows){
      intensity_current_(i,0) = get_interpolated_pixel_value(u,v);
    }
    else{
      ob++;
      continue;
    }

    if(intensity_current_(i,0) >= 255)
      intensity_current_(i,0) = 254.999;
    if(intensity_current_(i,0) < 0)
      intensity_current_(i, 0) = 0.0;

    double bin_pos_current = intensity_current_(i,0)* bin_num_/255.0;
    double pos_cubic_current = bin_pos_current * bin_pos_current * bin_pos_current;
    double pos_qua_current  = bin_pos_current * bin_pos_current;
    double bins_index_current = floor(bin_pos_current);

    pro_current_[bins_index_current] += 1.0;

    pro_joint_[bins_index_ref][bins_index_current] += 1.0;

  }

  //if too many points are out of image boundary, we don't use this edge
  if(intensity0_.rows() - ob < 300){
    //std::cout<<"no enough points in the image boundary"<<std::endl;
    return;
  }

  //pro_last_.size() = bin_num
  for(int i = 0; i < bin_num_ ; i++){
    pro_current_[i] /= (intensity0_.rows() - ob);
  }

  for(int i = 0; i < bin_num_; i++)
    for(int j = 0; j<bin_num_; j++){
      pro_joint_[i][j] /= (intensity0_.rows() - ob);
    }

  for(int i = 0; i < bin_num_ ; i++){
    if(pro_current_[i] < sigma_)
      continue;
    H_current_ -= pro_current_[i] * log2(pro_current_[i]);
  }

  for(int i = 0; i < bin_num_; i++)
    for(int j = 0; j<bin_num_; j++){
      if(pro_joint_[i][j] < sigma_)
        continue;
      H_joint_ -= pro_joint_[i][j] * log2(pro_joint_[i][j]);
    }

    nid_ = (2*H_joint_ - H_ref_ - H_current_)/H_joint_;
    MI_ = H_ref_ + H_current_ - H_joint_;

    //if all points from one image is mapped into only one bin so that one of the pro_current_[i] == 1, then H_currrent and H_joint will be 0;
    //For example, if we have 3 bins, pro_ref is 0 1 0, pro_current is 0 1 0 (or 0 0 1, 1 0 0), then H_ref and current will be 0. Notice we can totally infer one image's state from another. For this perfect matching, we should count the cost as 0
    if(H_ref_ == 0.0 && H_current_ == 0.0 && H_joint_ == 0.0){
      MI_ = 0.0;
      nid_ = 0.0;
    }

    std::cout<<"Href, current, joint from standard method is "<<H_ref_<<","<<H_current_<<","<<H_joint_<<", MI "<<MI_<<", NID "<<nid_<<std::endl;

}