/*
 * graphSlam6D implementation
 *
 * Copyright (C) Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nuechter
 *
 * Released under the GPL version 3.
 *
 */


/**
 * @file 
 * @brief The implementation of globally consistent scan matching algorithm
 * @author Dorit Borrman. Institute of Computer Science, University of Osnabrueck, Germany.
 * @author Jan Elseberg. Institute of Computer Science, University of Osnabrueck, Germany.
 * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
 * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
 */

#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP 
#define _OPENMP
#endif
#endif

#include "slam6d/graphSlam6D.h"
#include "sparse/csparse.h"

#include <cfloat>
#include <fstream>
using std::ofstream;
using std::flush;
#include "slam6d/globals.icc"

using namespace NEWMAT;
/**
 * Constructor
 *
 * @param my_icp6Dminimizer Pointer to ICP minimization functor
 * @param mdm Maximum PtoP distance to which point pairs are collected for ICP
 * @param max_dist_match Maximum PtoP distance to which point pairs are collected for LUM
 * @param max_num_iterations Maximal number of iterations for ICP
 * @param quiet Suspesses all output to std out
 * @param meta Indicates if metascan matching has to be used
 * @param rnd Indicates if randomization has to be used
 * @param eP Extrapolate odometry?
 * @param anim Animate which frames?
 * @param epsilonICP Termination criterion for ICP
 * @param nns_method Specifies which NNS method to use
 * @param epsilonLUM Termination criterion for LUM
 */
graphSlam6D::graphSlam6D(icp6Dminimizer *my_icp6Dminimizer,
					double mdm, double max_dist_match, 
					int max_num_iterations, bool quiet, bool meta, int rnd,
					bool eP, int anim, double epsilonICP, int nns_method, double epsilonLUM)
{
  this->nns_method = nns_method;
  this->quiet = quiet;
  this->epsilonLUM = epsilonLUM;
  this->max_dist_match2_LUM = sqr(max_dist_match);

  ctime = 0;

  this->my_icp = new icp6D(my_icp6Dminimizer, mdm, max_num_iterations,
					  quiet, meta, rnd, eP, anim, epsilonICP, nns_method);
}

graphSlam6D::~graphSlam6D()
 {
   cout << "Time spent in the SLAM backend:" << ctime << endl;
 }
/**
 * This function is used to match a set of laser scans with any minimally
 * connected Graph, using the globally consistent LUM-algorithm in 3D.
 *
 * @param allScans Contains all laser scans
 * @param nrIt The number of iterations the LUM-algorithm will run
 * @param clpairs minimal number of points aximal distance for closing loops
 * @param loopsize minimal loop size
 */
void graphSlam6D::matchGraph6Dautomatic(vector <Scan *> allScans, int nrIt, int clpairs, int loopsize)
{
  // the IdentityMatrix to transform some Scans with
  double id[16];
  M4identity(id);

  Graph *gr = 0;
  int i = 0;

  do {
    cout << "Generate graph ... " << flush;
    i++;
    if (gr) delete gr;
    gr = new Graph(0, false);
    int j, maxj = (int)allScans.size();
#ifdef _OPENMP
    omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
    for (j = 0; j <  maxj; j++) {
#ifdef _OPENMP
      int thread_num = omp_get_thread_num();
#else
      int thread_num = 0;
#endif
      for (int k = 0; k < (int)allScans.size(); k++) {
        if (j == k) continue;
        Scan * FirstScan  = allScans[j];
        Scan * SecondScan = allScans[k];
        double centroid_d[3] = {0.0, 0.0, 0.0};
        double centroid_m[3] = {0.0, 0.0, 0.0};
        vPtPair temp;
        double sum_dummy;
        Scan::getPtPairs(&temp, FirstScan, SecondScan, thread_num,
            my_icp->get_rnd(), (int)max_dist_match2_LUM, sum_dummy,
            centroid_m, centroid_d);
        if ((int)temp.size() > clpairs) {
#ifdef _OPENMP
#pragma omp critical
#endif
          gr->addLink(j, k);  
        } 
      }
    }
    cout << "done" << endl;
  } while ((doGraphSlam6D(*gr, allScans, 1) > 0.001) && (i < nrIt));

  return;
}


Graph *graphSlam6D::computeGraph6Dautomatic(vector <Scan *> allScans, int clpairs) 
{
  // the IdentityMatrix to transform some Scans with
  double id[16];
  M4identity(id);

  int i = 0;

  cout << "Generate graph ... " << flush;
  i++;
  Graph *gr = new Graph(0, false);
  int j, maxj = (int)allScans.size();
#ifdef _OPENMP
  omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
  for (j = 0; j <  maxj; j++) {
#ifdef _OPENMP
    int thread_num = omp_get_thread_num();
#else
    int thread_num = 0;
#endif
    for (int k = 0; k < (int)allScans.size(); k++) {
      if (j == k) continue;
      Scan * FirstScan  = allScans[j];
      Scan * SecondScan = allScans[k];
      double centroid_d[3] = {0.0, 0.0, 0.0};
      double centroid_m[3] = {0.0, 0.0, 0.0};
      vPtPair temp;
      double sum_dummy;
      Scan::getPtPairs(&temp, FirstScan, SecondScan, thread_num,
          my_icp->get_rnd(), (int)max_dist_match2_LUM, sum_dummy,
          centroid_m, centroid_d);
      if ((int)temp.size() > clpairs) {
#ifdef _OPENMP
#pragma omp critical
#endif
        gr->addLink(j, k);  
      } 
    }
  }
  cout << "done" << endl;

  return gr;
}

/**
 * This function is used to solve the system of linear eq.
 *
 * @param G symmetric, positive definite Matrix, thus invertable
 * @param B column vector
 */
void graphSlam6D::writeMatrixPGM(const Matrix &G)
{
  int n = G.Ncols();
  static int matrixnum = 0;
  string mf = "matrix" + to_string(matrixnum,4) + ".pgm";
  ofstream matrixout(mf.c_str());
  matrixout << "P2" << endl
		  << "# CREATOR slam6D (c) Andreas Nuechter, 05/2007" << endl
		  << n << " " << n << endl
		  << 255 << endl;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
	  if (G.element(i, j) > 0.001) {
	    matrixout << 0 << " ";
	  } else {
	    matrixout << 255 << " ";
	  }
    }
    matrixout << endl;
  }
  // matrixout << G << endl; 
  matrixout.close();
  matrixout.clear();
  matrixnum++;

}



/**
 * This function is used to solve the system of linear eq.
 *
 * @param G Matrix, invertable
 * @param B column vector
 */
ColumnVector graphSlam6D::solve(const Matrix &G, const ColumnVector &B)
{
  
#ifdef WRITE_MATRIX_PGM
  writeMatrixPGM(G);
#endif

  // ----------------------------------
  // solve eqn via inverting the matrix
  // ----------------------------------
  return  (ColumnVector)(G.i() * B);
}


/**
 * This function is used to solve the system of linear eq.
 * The implementation from the numerical recepies are used
 *
 * @param G symmetric, positive definite Matrix, thus invertable
 * @param B column vector
 */
ColumnVector graphSlam6D::solveCholesky(const Matrix &G, const ColumnVector &B)
{
  
#ifdef WRITE_MATRIX_PGM
  writeMatrixPGM(G);
#endif

  // We copy the newmat matrices and use our own
  // Cholesky decomposition code here. The Cholesky
  // decomosition is based on the Numerical Recipes in C.
  // This speed ups computation time

  // copy values
  int n = G.Ncols();
  double **A   = new double*[n];
  double *C    = new double[n];
  double *diag = new double[n];
  double *x    = new double[n];
  ColumnVector X(n);
  for (int i = 0; i < n; i++) {
    A[i] = new double[n];
    for (int j = 0; j < n; j++) {
	 A[i][j] = G.element(i, j);
    }
    C[i] = B.element(i);
  }
  // --------------------------------------------------
  // make cholesky dekomposition with numerical recipes
  // --------------------------------------------------
  if (!choldc(n, A, diag)) {
    cout << "cannot perfom cholesky decomposition" << endl;     
  }
  // solve A x = C
  cholsl(n, A, diag, C, x);
  // copy values back
  for (int i = 0; i < n; i++) {
    X.element(i) = x[i];
  }
  // clean up
  for (int i = 0; i < n; i++) {
    delete [] A[i];
  }
  delete [] x;
  delete [] diag;
  delete [] C;  
  delete [] A;

  return X;
}


/**
 * This function is used to solve the system of linear eq.
 *
 * @param G symmetric, positive definite Matrix, thus invertable
 * @param B column vector
 */
ColumnVector graphSlam6D::solveSparseCholesky(const Matrix &G, const ColumnVector &B)
{

  long starttime = GetCurrentTimeInMilliSec();
    
#ifdef WRITE_MATRIX_PGM
  writeMatrixPGM(G);
#endif

  int n = G.Ncols();
  
  // ------------------------------
  // Sparse Cholsekey decomposition
  // ------------------------------
  ColumnVector X(n);
  cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
  double *x = new double[n];
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
	 if (fabs(G.element(i, j)) > 0.00001) {
	   cs_entry (T, i, j, G.element(i, j));
	 }
    }
    x[i] = B.element(i);
  }
  A = cs_triplet (T);
  cs_dropzeros (A) ;			// drop zero entries
  cs_cholsol (A, x, 1) ;
  // copy values back  
  for (int i = 0; i < n; i++) {
    X.element(i) = x[i];
  }

  cs_spfree(A);
  cs_spfree(T);
  delete [] x;

  ctime += GetCurrentTimeInMilliSec() - starttime;

  return X;
}

ColumnVector graphSlam6D::solveSparseCholesky(GraphMatrix *G, const ColumnVector &B)
{

  long starttime = GetCurrentTimeInMilliSec();

  int n = B.Nrows();
  ColumnVector X(n);
  
  // ------------------------------
  // Sparse Cholsekey decomposition
  // ------------------------------
  cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
  double *x = new double[n];
  for (int i = 0; i < n; i++) {
    x[i] = B.element(i);
  }
  G->convertToCS(T);
  A = cs_triplet (T);
  cs_dropzeros (A) ;			// drop zero entries
//  cs_print(T, 0);
  cs_cholsol (A, x, 1) ;
  // copy values back  
  for (int i = 0; i < n; i++) {
    X.element(i) = x[i];
  }

  cs_spfree(A);
  cs_spfree(T);
  delete [] x;

  ctime += GetCurrentTimeInMilliSec() - starttime;

  return X;
}


/**
 * This function is used to solve the system of linear eq.
 *
 * @param G invertable Matrix
 * @param B column vector
 */
ColumnVector graphSlam6D::solveSparseQR(const Matrix &G, const ColumnVector &B)
{
  
#ifdef WRITE_MATRIX_PGM
  writeMatrixPGM(G);
#endif

  int n = B.Ncols();

  ColumnVector X(n);
  cs *A, *T = cs_spalloc (0, 0, 1, 1, 1) ;
  double *x = new double[n];
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
	 if (fabs(G.element(i, j)) > 0.00001) {
	   cs_entry (T, i, j, G.element(i, j));
	 }
    }
    x[i] = B.element(i);
  }
  A = cs_triplet (T);
  cs_dropzeros (A) ;			// drop zero entries
  int order = 3;                   // for qr-ordering
  cs_qrsol ( A, x, order) ;
  // copy values back  
  for (int i = 0; i < n; i++) {
    X.element(i) = x[i];
  }

  cs_spfree(A);
  cs_spfree(T);
  delete [] x;

  return X;
}

void graphSlam6D::set_mdmll(double mdmll) {
  max_dist_match2_LUM = sqr(mdmll);
}


void GraphMatrix::add(const unsigned int i, const unsigned int j, Matrix &Cij) {
  uipair ui(i,j);
  it = matrix.find( ui );
  if (it != matrix.end()) {
    (*(it->second)) += Cij;
  } else {
    Matrix *C = new Matrix(6,6);
    *C = Cij;
    matrix.insert( uimpair( ui, C));
  }
}

void GraphMatrix::subtract(const unsigned int i, const  unsigned int j,Matrix &Cij) {
  uipair ui(i,j);
  it = matrix.find( ui );
  if (it != matrix.end()) {
    (*it->second) -= Cij;
  } else {
    Matrix *C = new Matrix(6,6);
    *C = Cij;
    *C *= -1.0;
    matrix.insert( uimpair( ui, C));
  }
}

void GraphMatrix::print() {
  for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
    uimpair uim = *it;
    uipair ui = uim.first;
    cout << ui.first << " " << ui.second << " :" << endl << *uim.second << endl;
  }
}

GraphMatrix::~GraphMatrix() {
  for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
    uimpair uim = *it;
    delete uim.second;
  }

}

void GraphMatrix::convertToCS(cs *T) {
  unsigned int a,b;
  int imin,imax,jmin,jmax;

  for ( it = matrix.begin() ; it != matrix.end(); it++ ) {
    Matrix *C = it->second;
    a = it->first.first;
    b = it->first.second;
//    cout << a << " " << b << " " << C << endl;
    imin = a*6;
    jmin = b*6;

    imax = a*6 + 6;
    jmax = b*6 + 6;
    a = b = 0;

    for (int i = imin; i < imax; i++, a++) {
      b = 0;
      for (int j = jmin; j < jmax; j++, b++) {
        if (fabs(C->element(a, b)) > 0.00001) {
          cs_entry (T, i, j, C->element(a, b));
        }
      }
    }
  }
//  print();
//  cs_print(T, 0);
}