/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Filename    : misc.cpp                                                                              *
 * Project     : Planewalker - Schnorr-Euchner sphere decoder simulation for space-time lattice codes  *
 * Authors     : Pasi Pyrrö, Oliver Gnilke                                                             *
 * Version     : 1.0                                                                                *
 * Copyright   : Aalto University ~ School of Science ~ Department of Mathematics and Systems Analysis *
 * Date        : 17.8.2017                                                                             *
 * Language    : C++ (2011 or newer standard)                                                          *
 * Description : contain miscellaneous utilities for the sphere decoder program, mainly I/O stuff      *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define ARMA_NO_DEBUG /* disable Armadillo bound checks for addiotional speed */

#include <iostream>
#include <mutex>
#include <ctime>
#include <string>
#include <map>
#include <fstream>
#include <algorithm>
#include <armadillo>

#include "misc.hpp"
#ifdef PLOTTING
#include "gnuplot-iostream.hpp" /* Stand alone header file for Gnuplot API (requires Boost library though) */
#endif

using namespace std;
using namespace arma;

/* objects for parallel computing synchronazation */
std::mutex log_mutex;
std::mutex output_mutex;

string red = "\e[1;31m";
string green = "\e[1;32m";
string yellow = "\e[1;33m";
string def = "\e[0m";

/* Generates a standard datetime string of current local time */
string time_str(){
    struct tm *timeinfo;
    time_t t = time(nullptr);
    timeinfo = localtime(&t);
    char timestamp[50];
    strftime(timestamp, 50, "%d-%m-%Y %T", timeinfo);
    return string(timestamp);
}

/* function used for logging, should be thread safe, outputs to terminal and into a txt file
 *
 * Defined values for lvl argument:
 * "Raw"   : msg is printed as is without prefix or color
 * "Info"  : default value, has prefix but no color, used for basic logging
 * "Alert" : Something unexpected happened, uses yellow color
 * "Error" : An error occurred and the program needs to terminate, uses red color
 */
void log_msg(const string msg, const string lvl) {
    string prefix = "";
    string color = "";
    if (lvl.compare("Raw") != 0) { /* raw input contains no prefix */
        prefix += time_str();
        prefix += " | [" + lvl + "]\t";
    }
    if (lvl.compare("Alert") == 0){
        color = yellow;
    }
    if (lvl.compare("Error") == 0){
        color = red;
    }
    lock_guard<mutex> lock(log_mutex); /* make sure other threads don't write to log file simultaneosly */
    ofstream logfile(filenames["log"], ios_base::app);
    if (msg.compare("-start-") == 0)   /* just writes newline to log file to indicate new program run */
        logfile << endl;
    else {
        logfile << prefix << msg << endl; /* log txt file */
        cout << color << prefix << msg << def << endl;    /* stdout */
    }
    logfile.close();
}

/* Makes the user input somewhat more readable */
void clean_input(string &input){
    /* remove comments from string */
    string::size_type start = input.find("//", 0);
    if (start != string::npos)
        input = input.substr(0, start);
    /* remove white spaces from string */
    input.erase(remove_if(input.begin(), input.end(), ::isspace), input.end());
}

/* removes all characters c from input string */
void remove_from_string(string &input, const char c){
    input.erase(remove(input.begin(), input.end(), c), input.end());
}

/* Generates the output filename from current time and the used bases file name */
void create_output_filename(){
    string bf = filenames["bases"];
    size_t a = bf.find("/")+1;
    size_t b = bf.find(".");
    string name = bf.substr(a, b-a);
    filenames["output"] = string("output/") + time_str() + string(" ") + name + string(" output.csv");
}

/* Writes a vector of strings (lines) in a csv file */
void output_csv(const parallel_vector<string> &lines){
    ofstream csv(filenames["output"]);
    for (const auto &line : lines){
        csv << line << endl;
    }
    csv.close();
}

/* Used to compare the SNR values of each output line */
bool snr_ordering(string &a, string &b){
    /* Example:
     * a = "16,15.976382,10000,9.760000"
     * b = "18,17.976382,10000,10.160000"
     * --> i = 16, j = 18
     * --> a comes before b
     */
    int i = strtol(a.substr(0, a.find(",")).c_str(), NULL, 10);
    int j = strtol(b.substr(0, b.find(",")).c_str(), NULL, 10);
    return (i < j);
}

/* Uses gnuplot stream to plot the columns of the output csv file */
void plot_csv(int xcol, int ycol, const string &xlabel, const string &ylabel, bool logscale){
    #ifdef PLOTTING /* This function does nothing if PLOTTING is not defined when compiled */
    Gnuplot gp;
    gp << "set terminal x11\n"; /* Open plots in a new GUI window by default, can also be saved in png file for example */
    if (logscale)
        gp << "set logscale y 10\n";  /* Set y axis to logscale of base 10 (SNR is already in log10 scale) */
    gp << "set datafile separator \",\"\n";
    gp << "set xlabel \"" << xlabel << "\"\n";
    gp << "set ylabel \"" << ylabel << "\"\n";
    gp << "set grid\n";
    /* plot using the data from output csv file */
    gp << "plot '" << filenames["output"] << "' using " << xcol << ":" << ycol << " with linespoints ls 3\n";
    #endif
}

/* Prints simulation results in a file and optionally plots the results */
void output_data(parallel_vector<string> &output){
    if (output.size() > 1){
        sort(output.begin()+1, output.end(), snr_ordering); /* sort vector by SNR (ascending order) */
        log_msg("Printing simulation output to '" + filenames["output"] + "'...");
        output_csv(output);
        #ifdef PLOTTING /* Draw plots with Gnuplot if plotting is enabled */
        if (params["plot_results"] > 0){
            log_msg("Drawing plots...");
            plot_csv(2, 7, "SNR (dB)", "BLER (%)", true);
            plot_csv(2, 3, "SNR (dB)", "Average Complexity (# visited points)", false);
        }
        #endif
    }
}

/* outputs real matrix into a file in mathematica format */
void output_real_matrix(const string &filepath, const mat &A, bool append){
    ofstream mfile;
    /* selects write mode: either overwrite the old file or append to it */
    if (!append)
        mfile = ofstream(filepath);
    else
        mfile = ofstream(filepath, ios_base::app);
    mfile << "{";
    for (auto i = 0u; i < A.n_rows; i++){
        mfile << "{";
        for (auto j = 0u; j < A.n_cols; j++){
            mfile << float2str(A(i, j), 16);
            if (j < A.n_cols - 1)
                mfile << ", ";
        }
        mfile << "}";
        if (i < A.n_rows - 1)
            mfile << "," << endl;
    }
    mfile << "}";
    if (append) mfile << endl << endl;
    mfile.close();
}

/* outputs complex matrix into a file in mathematica format */
void output_complex_matrix(const string &filepath, const cx_mat &A, bool append){
    ofstream mfile;
    string real, imag;
    /* selects write mode: either overwrite the old file or append to it */
    if (!append)
        mfile = ofstream(filepath);
    else
        mfile = ofstream(filepath, ios_base::app);
    mfile << "{";
    for (auto i = 0u; i < A.n_rows; i++){
        mfile << "{";
        for (auto j = 0u; j < A.n_cols; j++){
            real = float2str(A(i, j).real(), 16);
            imag = float2str(A(i, j).imag(), 16);
            if (imag[0] == '-')
                mfile << real << " - " << imag.substr(1) << " *I";
            else
                mfile << real << " + " << imag << " *I";
            if (j < A.n_cols - 1)
                mfile << ", ";
        }
        mfile << "}";
        if (i < A.n_rows - 1)
            mfile << "," << endl;
    }
    mfile << "}";
    if (append) mfile << endl << endl;
    mfile.close();
}


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Overload of uBLAS prod function with MKL/GSL implementations

#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <votca/ctp/logger.h>
#include <votca/tools/constants.h>
#include <votca/xtp/gwbse.h>
#include <votca/xtp/numerical_integrations.h>
#include <votca/xtp/qmpackagefactory.h>
#include <votca/xtp/ppm.h>
#include <votca/xtp/sigma.h>
#include <votca/xtp/bse.h>
#include <votca/xtp/sigma.h>
#include <votca/xtp/orbitals.h>

using boost::format;
using namespace boost::filesystem;
using std::flush;
namespace votca {
namespace xtp {

void GWBSE::Initialize(tools::Property& options) {

#if (GWBSE_DOUBLE)
  CTP_LOG(ctp::logDEBUG, *_pLog) << " Compiled with full double support"
                                 << flush;
#else
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << " Compiled with float/double mixture (standard)" << flush;
#endif

  std::string key = Identify();

  // getting level ranges
  double qpminfactor=0;
  double qpmaxfactor=0;
  double rpamaxfactor=0;
  double bseminfactor=0;
  double bsemaxfactor=0;
std::string ranges = options.ifExistsReturnElseReturnDefault<std::string>(key + ".ranges",
                                                             "default");

  // now check validity, and get rpa, qp, and bse level ranges accordingly

  if (ranges == "factor") {
    // get factors
    rpamaxfactor = options.get(key + ".rpamax").as<double>();
    qpminfactor = options.get(key + ".qpmin").as<double>();
    qpmaxfactor = options.get(key + ".qpmax").as<double>();
    bseminfactor = options.get(key + ".bsemin").as<double>();
    bsemaxfactor = options.get(key + ".bsemax").as<double>();
  } else if (ranges == "explicit") {
    // get explicit numbers
    _rpamax = options.get(key + ".rpamax").as<int>();
    _qpmin = options.get(key + ".qpmin").as<int>();
    _qpmax = options.get(key + ".qpmax").as<int>();
    _bse_vmin = options.get(key + ".bsemin").as<int>();
    _bse_cmax = options.get(key + ".bsemax").as<int>();
  } else if (ranges == "" || ranges == "default") {
    ranges = "default";
  } else if (ranges == "full") {
    ranges = "full";
  } else {
    std::cerr << "\nSpecified range option " << ranges << " invalid. ";
    throw std::runtime_error(
        "\nValid options are: default,factor,explicit,full");
  }

  /* Preparation of calculation parameters:
   *  - number of electrons -> index of HOMO
   *  - number of levels
   *  - highest level considered in RPA
   *  - lowest and highest level considered in GWA
   *  - lowest and highest level considered in BSE
   *  - number of excitations calculates in BSE
   */

  _homo = _orbitals.getNumberOfElectrons() - 1;  // indexed from 0

 _reset_3c=options.ifExistsReturnElseReturnDefault<int>(
      key + ".rebuild_threecenter_freq", 5);

  _rpamin = 0;  // lowest index occ min(gwa%mmin, screening%nsum_low) ! always 1
  if (ranges == "default" || ranges == "full") {
    _rpamax = _orbitals.getNumberOfLevels() - 1;  // total number of levels
  } else if (ranges == "factor") {
    _rpamax = rpamaxfactor * _orbitals.getNumberOfLevels() -
              1;  // total number of levels
  }
  if (_rpamax > _orbitals.getNumberOfLevels() - 1) {
    _rpamax = _orbitals.getNumberOfLevels() - 1;
  }
  // convert _qpmin and _qpmax if needed
  if (ranges == "default") {
    _qpmin = 0;              // indexed from 0
    _qpmax = 2 * _homo + 1;  // indexed from 0
  } else if (ranges == "factor") {
    if (_orbitals.getNumberOfElectrons() -
            int(qpminfactor * _orbitals.getNumberOfElectrons()) - 1 <
        0) {
      _qpmin = 0;
    } else {
      _qpmin = _orbitals.getNumberOfElectrons() -
               int(qpminfactor * _orbitals.getNumberOfElectrons()) - 1;
    }
    _qpmax = _orbitals.getNumberOfElectrons() +
             int(qpmaxfactor * _orbitals.getNumberOfElectrons()) - 1;
  } else if (ranges == "explicit") {
    _qpmin -= 1;
    _qpmax -= 1;
  } else if (ranges == "full") {
    _qpmin = 0;
    _qpmax = _orbitals.getNumberOfLevels() - 1;
  }
  if (_qpmax > int(_orbitals.getNumberOfLevels() - 1)) {
    _qpmax = _orbitals.getNumberOfLevels() - 1;
  }
  if (_qpmax> _rpamax){
    _qpmax=_rpamax;
  }

  // set BSE band range indices
  // anything else would be stupid!
  _bse_vmax = _homo;
  _bse_cmin = _homo + 1;

  if (ranges == "default") {
    _bse_vmin = 0;              // indexed from 0
    _bse_cmax = 2 * _homo + 1;  // indexed from 0
  } else if (ranges == "factor") {
    _bse_vmin = _orbitals.getNumberOfElectrons() -
                int(bseminfactor * _orbitals.getNumberOfElectrons()) - 1;
    if (_orbitals.getNumberOfElectrons() -
            int(bseminfactor * _orbitals.getNumberOfElectrons()) - 1 <
        0) {
      _bse_vmin = 0;
    }
    _bse_cmax = _orbitals.getNumberOfElectrons() +
                int(bsemaxfactor * _orbitals.getNumberOfElectrons()) - 1;
  } else if (ranges == "explicit") {
    _bse_vmin -= 1;
    _bse_cmax -= 1;
  } else if (ranges == "full") {
    _bse_vmin = 0;
    _bse_cmax = _orbitals.getNumberOfLevels() - 1;
  }
  if (_bse_cmax > int(_orbitals.getNumberOfLevels() - 1)) {
    _bse_cmax = _orbitals.getNumberOfLevels() - 1;
  }

  bool ignore_corelevels = options.ifExistsReturnElseReturnDefault<bool>(
      key + ".ignore_corelevels", false);

   int ignored_corelevels = 0;
  if (ignore_corelevels) {
    if(!_orbitals.hasECP()){
      BasisSet basis;
      basis.LoadPseudopotentialSet("corelevels");//
      int coreElectrons=0;
      for(const auto& atom:_orbitals.QMAtoms()){
        coreElectrons+=basis.getElement(atom->getType()).getNcore();
      }
       ignored_corelevels = coreElectrons/2;
    }

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Ignoring "
                                   << ignored_corelevels << " core levels "
                                   << flush;
  }
  // autoignore core levels in QP
  if (ignore_corelevels && (_qpmin < ignored_corelevels)) {
    _qpmin = ignored_corelevels;
  }
  // autoignore core levels in BSE
  if (ignore_corelevels && (_bse_vmin < ignored_corelevels)) {
    _bse_vmin = ignored_corelevels;
  }

  int bse_vtotal = _bse_vmax - _bse_vmin + 1;
  int bse_ctotal = _bse_cmax - _bse_cmin + 1;
  int bse_size = bse_vtotal * bse_ctotal;

  // some QP - BSE consistency checks are required
  if (_bse_vmin < _qpmin) _qpmin = _bse_vmin;
  if (_bse_cmax > _qpmax) _qpmax = _bse_cmax;

  _qptotal = _qpmax - _qpmin + 1;

  // information for hybrid DFT

  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Set RPA level range ["
                                 << _rpamin + 1 << ":" << _rpamax + 1 << "]"
                                 << flush;
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Set QP  level range ["
                                 << _qpmin + 1 << ":" << _qpmax + 1 << "]"
                                 << flush;
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Set BSE level range occ[" << _bse_vmin + 1 << ":"
      << _bse_vmax + 1 << "]  virt[" << _bse_cmin + 1 << ":" << _bse_cmax + 1
      << "]" << flush;
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " BSE Hamiltonian has size " << bse_size << "x"
      << bse_size << flush;

  _bse_maxeigenvectors =
      options.ifExistsReturnElseReturnDefault<int>(key + ".exctotal", 25);
   if (_bse_maxeigenvectors > int(bse_size) || _bse_maxeigenvectors < 0) _bse_maxeigenvectors = bse_size;
  _fragA = options.ifExistsReturnElseReturnDefault<int>(key + ".fragment", -1);

  std::string BSEtype =
      options.ifExistsReturnElseReturnDefault<std::string>(key + ".BSEtype", "TDA");

  if (BSEtype == "full") {
    _do_full_BSE = true;
    CTP_LOG(ctp::logDEBUG, *_pLog) << " BSE type: full" << flush;
  } else {
    _do_full_BSE = false;
    CTP_LOG(ctp::logDEBUG, *_pLog) << " BSE type: TDA" << flush;
  }

  _openmp_threads =
      options.ifExistsReturnElseReturnDefault<int>(key + ".openmp", 0);

  if (options.exists(key + ".vxc")) {
    _doVxc =
        options.ifExistsReturnElseThrowRuntimeError<bool>(key + ".vxc.dovxc");
    if (_doVxc) {
      _functional = options.ifExistsReturnElseThrowRuntimeError<std::string>(
          key + ".vxc.functional");
      _grid = options.ifExistsReturnElseReturnDefault<std::string>(
          key + ".vxc.grid", "medium");
    }
  }

  _auxbasis_name =
      options.ifExistsReturnElseThrowRuntimeError<std::string>(key + ".gwbasis");
  _dftbasis_name =
      options.ifExistsReturnElseThrowRuntimeError<std::string>(key + ".dftbasis");

  _shift = options.ifExistsReturnElseThrowRuntimeError<double>(key + ".shift");
  _g_sc_limit = options.ifExistsReturnElseReturnDefault<double>(
      key + ".g_sc_limit",
      0.00001);  // convergence criteria for qp iteration [Hartree]]
  _g_sc_max_iterations = options.ifExistsReturnElseReturnDefault<int>(
      key + ".g_sc_max_iterations",
      40);  // convergence criteria for qp iteration [Hartree]]

  _gw_sc_max_iterations = options.ifExistsReturnElseReturnDefault<int>(
      key + ".gw_sc_max_iterations",
      20);  // convergence criteria for qp iteration [Hartree]]

  _gw_sc_limit = options.ifExistsReturnElseReturnDefault<double>(
      key + ".gw_sc_limit", 0.00001);  // convergence criteria for shift it
  _iterate_gw = false;
  std::string _shift_type =
      options.ifExistsReturnElseThrowRuntimeError<std::string>(key + ".shift_type");
  if (_shift_type != "fixed") {
    _iterate_gw = true;
  }
  CTP_LOG(ctp::logDEBUG, *_pLog) << " Shift: " << _shift_type << flush;
  CTP_LOG(ctp::logDEBUG, *_pLog) << " g_sc_limit [Hartree]: " << _g_sc_limit
                                 << flush;
  if (_iterate_gw) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " gw_sc_limit [Hartree]: " << _gw_sc_limit
                                   << flush;
  }
  _min_print_weight = options.ifExistsReturnElseReturnDefault<double>(
      key + ".bse_print_weight",
      0.2);  // print exciton WF composition weight larger that thin minimum

  // setting some defaults
  _do_qp_diag = false;
  _do_bse_singlets = false;
  _do_bse_triplets = false;
  // possible tasks
  // diagQP, singlets, triplets, all, ibse
  std::string _tasks_string =
      options.ifExistsReturnElseThrowRuntimeError<std::string>(key + ".tasks");
  if (_tasks_string.find("all") != std::string::npos) {
    _do_qp_diag = true;
    _do_bse_singlets = true;
    _do_bse_triplets = true;
  }
  if (_tasks_string.find("qpdiag") != std::string::npos) _do_qp_diag = true;
  if (_tasks_string.find("singlets") != std::string::npos)
    _do_bse_singlets = true;
  if (_tasks_string.find("triplets") != std::string::npos)
    _do_bse_triplets = true;
  _store_eh_interaction = false;
  _do_bse_diag = true;
  // special construction for ibse mode
  if (_tasks_string.find("iqm") != std::string::npos) {
    _do_qp_diag = false;   // no qp diagonalization
    _do_bse_diag = false;  // no diagonalization of BSE Hamiltonian
    _store_eh_interaction = true;
  }

  // possible storage
  // qpPert, qpdiag_energies, qp_diag_coefficients, bse_singlet_energies,
  // bse_triplet_energies, bse_singlet_coefficients, bse_triplet_coefficients
  _store_qp_pert = true;

  _store_qp_diag = false;
  _store_bse_triplets = false;
  _store_bse_singlets = false;
  std::string _store_string =
      options.ifExistsReturnElseThrowRuntimeError<std::string>(key + ".store");
  if ((_store_string.find("all") != std::string::npos) ||
      (_store_string.find("") != std::string::npos)) {
    // store according to tasks choice
    if (_do_qp_diag) _store_qp_diag = true;
    if (_do_bse_singlets && _do_bse_diag) _store_bse_singlets = true;
    if (_do_bse_triplets && _do_bse_diag) _store_bse_triplets = true;
  }
  if (_store_string.find("qpdiag") != std::string::npos) _store_qp_diag = true;
  if (_store_string.find("singlets") != std::string::npos)
    _store_bse_singlets = true;
  if (_store_string.find("triplets") != std::string::npos)
    _store_bse_triplets = true;
  if (_store_string.find("ehint") != std::string::npos)
    _store_eh_interaction = true;

  CTP_LOG(ctp::logDEBUG, *_pLog) << " Tasks: " << flush;
  if (_do_qp_diag) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " qpdiag " << flush;
  }
  if (_do_bse_singlets) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " singlets " << flush;
  }
  if (_do_bse_triplets) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " triplets " << flush;
  }
  CTP_LOG(ctp::logDEBUG, *_pLog) << " Store: " << flush;
  if (_store_qp_diag) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " qpdiag " << flush;
  }
  if (_store_bse_singlets) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " singlets " << flush;
  }
  if (_store_bse_triplets) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " triplets " << flush;
  }
  if (_store_eh_interaction) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << " ehint " << flush;
  }

  return;
}

void GWBSE::addoutput(tools::Property& summary) {

  const double hrt2ev = tools::conv::hrt2ev;
  tools::Property& gwbse_summary = summary.add("GWBSE", "");
  gwbse_summary.setAttribute("units", "eV");
  gwbse_summary.setAttribute("DeltaHLGap",
                               (format("%1$+1.6f ") % (_shift * hrt2ev)).str());

  gwbse_summary.setAttribute(
      "DFTEnergy", (format("%1$+1.6f ") % _orbitals.getQMEnergy()).str());
  int printlimit = _bse_maxeigenvectors;  // I use this to determine how much is printed,
                                 // I do not want another option to pipe through

  tools::Property &dft_summary = gwbse_summary.add("dft", "");
  dft_summary.setAttribute("HOMO", _homo);
  dft_summary.setAttribute("LUMO", _homo + 1);

  for (int state = _qpmin; state < _qpmax+1; state++) {

     tools::Property& level_summary = dft_summary.add("level", "");
    level_summary.setAttribute("number", state);
    level_summary.add("dft_energy",
                        (format("%1$+1.6f ") %
                         ((_orbitals.MOEnergies())(_qpmin + state) * hrt2ev))
                            .str());
    level_summary.add(
        "gw_energy",
        (format("%1$+1.6f ") % (_orbitals.QPpertEnergies()(_qpmin + state) * hrt2ev)).str());

    if (_do_qp_diag) {
      level_summary.add(
          "qp_energy",
          (format("%1$+1.6f ") % (_orbitals.QPdiagEnergies()(_qpmin + state) * hrt2ev))
              .str());
    }
  }

  if (_do_bse_singlets) {
     tools::Property &singlet_summary = gwbse_summary.add("singlets", "");
    for (int state = 0; state < _bse_maxeigenvectors; ++state) {
       tools::Property &level_summary = singlet_summary.add("level", "");
      level_summary.setAttribute("number", state + 1);
      level_summary.add("omega", (format("%1$+1.6f ") %
                                    (_orbitals.BSESingletEnergies()(state) * hrt2ev))
                                       .str());
      if (_orbitals.hasTransitionDipoles()) {

        const tools::vec &dipoles = (_orbitals.TransitionDipoles())[state];
        double f = 2 * dipoles * dipoles * _orbitals.BSESingletEnergies()(state) / 3.0;

        level_summary.add("f", (format("%1$+1.6f ") % f).str());
         tools::Property& dipol_summary = level_summary.add(
            "Trdipole", (format("%1$+1.4f %2$+1.4f %3$+1.4f") % dipoles.getX() %
                         dipoles.getY() % dipoles.getZ())
                            .str());
        dipol_summary.setAttribute("unit", "e*bohr");
        dipol_summary.setAttribute("gauge", "length");
      }
    }
  }
  if (_do_bse_triplets) {
     tools::Property &triplet_summary = gwbse_summary.add("triplets", "");
    for (int state = 0; state < printlimit; ++state) {
       tools::Property &level_summary = triplet_summary.add("level", "");
      level_summary.setAttribute("number", state + 1);
      level_summary.add("omega", (format("%1$+1.6f ") %
                                    (_orbitals.BSETripletEnergies()(state) * hrt2ev))
                                       .str());
    }
  }
  return;
}

/*
 *    Many-body Green's fuctions theory implementation
 *
 *  data required from orbitals file
 *  - atomic coordinates
 *  - DFT molecular orbitals (energies and coeffcients)
 *  - DFT exchange-correlation potential matrix in atomic orbitals
 *  - number of electrons, number of levels
 *

 */

Eigen::MatrixXd GWBSE::CalculateVXC(const AOBasis& dftbasis){

  Eigen::MatrixXd vxc_ao;
  if (_orbitals.hasAOVxc()) {
    if (_doVxc) {
      if(_orbitals.getQMpackage()=="xtp"){
        CTP_LOG(ctp::logDEBUG, *_pLog)
              << ctp::TimeStamp()
              << " Taking VXC from xtp DFT run."
              << flush;
      }else{
      CTP_LOG(ctp::logDEBUG, *_pLog)
              << ctp::TimeStamp()
              << " There is already a Vxc matrix loaded from DFT, did you maybe "
              "run a DFT code with outputVxc?\n I will take the external "
              "implementation"
              << flush;
      }
      _doVxc = false;
    }
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
            << " Loaded external Vxc matrix" << flush;
    vxc_ao = _orbitals.AOVxc();
  } else if (_doVxc) {

    NumericalIntegration numint;
    numint.setXCfunctional(_functional);
    double ScaHFX_temp = numint.getExactExchange(_functional);
    if (ScaHFX_temp != _orbitals.getScaHFX()) {
      throw std::runtime_error(
              (boost::format("GWBSE exact exchange a=%s differs from qmpackage "
              "exact exchange a=%s, probably your functionals are "
              "inconsistent") %
              ScaHFX_temp %  _orbitals.getScaHFX())
              .str());
    }
    numint.GridSetup(_grid, _orbitals.QMAtoms(),dftbasis);
    CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp()
            << " Setup grid for integration with gridsize: " << _grid << " with "
            << numint.getGridSize() << " points, divided into "
            << numint.getBoxesSize() << " boxes" << flush;
    CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp() << " Integrating Vxc in VOTCA with functional "
            << _functional << flush;
    Eigen::MatrixXd DMAT = _orbitals.DensityMatrixGroundState();

    vxc_ao = numint.IntegrateVXC(DMAT);
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
            << " Calculated Vxc in VOTCA" << flush;

  } else {
    throw std::runtime_error(
            "So your DFT data contains no Vxc, if you want to proceed use the "
            "dovxc option.");
  }

  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
          << " Set hybrid exchange factor: " <<  _orbitals.getScaHFX()
          << flush;

  // now get expectation values but only for those in _qpmin:_qpmax range
  Eigen::MatrixXd _mos = _orbitals.MOCoefficients().block(0,_qpmin,_orbitals.MOCoefficients().rows(),_qptotal);

  Eigen::MatrixXd vxc =_mos.transpose()*vxc_ao*_mos;
  CTP_LOG(ctp::logDEBUG, *_pLog)
          << ctp::TimeStamp()
          << " Calculated exchange-correlation expectation values " << flush;
  return vxc;
}

void GWBSE::PrintGWA_Energies(const Eigen::MatrixXd& vxc, const Sigma& sigma,const Eigen::VectorXd& _dft_energies){
  const Eigen::VectorXd& gwa_energies=sigma.getGWAEnergies();

  CTP_LOG(ctp::logINFO, *_pLog)
          << (format(
          "  ====== Perturbative quasiparticle energies (Hartree) ====== "))
          .str()
          << flush;
  CTP_LOG(ctp::logINFO, *_pLog)
          << (format("   DeltaHLGap = %1$+1.6f Hartree") % _shift).str() << flush;

  for (int i = 0; i < _qptotal; i++) {
    if ((i + _qpmin) == _homo) {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  HOMO  = %1$4d DFT = %2$+1.4f VXC = %3$+1.4f S-X = "
              "%4$+1.4f S-C = %5$+1.4f GWA = %6$+1.4f") %
              (i + _qpmin + 1) % _dft_energies(i + _qpmin) % vxc(i, i) %
              sigma.x(i) % sigma.c(i) % gwa_energies(i + _qpmin))
              .str()
              << flush;
    } else if ((i + _qpmin) == _homo + 1) {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  LUMO  = %1$4d DFT = %2$+1.4f VXC = %3$+1.4f S-X = "
              "%4$+1.4f S-C = %5$+1.4f GWA = %6$+1.4f") %
              (i + _qpmin + 1) % _dft_energies(i + _qpmin) % vxc(i, i) %
              sigma.x(i) % sigma.c(i) % gwa_energies(i + _qpmin))
              .str()
              << flush;

    } else {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  Level = %1$4d DFT = %2$+1.4f VXC = %3$+1.4f S-X = "
              "%4$+1.4f S-C = %5$+1.4f GWA = %6$+1.4f") %
              (i + _qpmin + 1) % _dft_energies(i + _qpmin) % vxc(i, i) %
              sigma.x(i) % sigma.c(i) % gwa_energies(i + _qpmin))
              .str()
              << flush;
    }
  }
  return;
}

void GWBSE::PrintQP_Energies(const Eigen::VectorXd& gwa_energies, const Eigen::VectorXd& qp_diag_energies){
  CTP_LOG(ctp::logDEBUG, *_pLog)
          << ctp::TimeStamp() << " Full quasiparticle Hamiltonian  " << flush;
  CTP_LOG(ctp::logINFO, *_pLog)
          << (format("  ====== Diagonalized quasiparticle energies (Hartree) "
          "====== "))
          .str()
          << flush;
  for (int _i = 0; _i < _qptotal; _i++) {
    if ((_i + _qpmin) == _homo) {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  HOMO  = %1$4d PQP = %2$+1.4f DQP = %3$+1.4f ") %
              (_i + _qpmin + 1) % gwa_energies(_i + _qpmin) %
              qp_diag_energies(_i))
              .str()
              << flush;
    } else if ((_i + _qpmin) == _homo + 1) {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  LUMO  = %1$4d PQP = %2$+1.4f DQP = %3$+1.4f ") %
              (_i + _qpmin + 1) % gwa_energies(_i + _qpmin) %
              qp_diag_energies(_i))
              .str()
              << flush;

    } else {
      CTP_LOG(ctp::logINFO, *_pLog)
              << (format("  Level = %1$4d PQP = %2$+1.4f DQP = %3$+1.4f ") %
              (_i + _qpmin + 1) % gwa_energies(_i + _qpmin) %
              qp_diag_energies(_i))
              .str()
              << flush;
    }
  }
  return;
}

bool GWBSE::Evaluate() {

// set the parallelization
#ifdef _OPENMP
  if (_openmp_threads > 0) {
    omp_set_num_threads(_openmp_threads);
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Using "
                                   << omp_get_max_threads() << " threads"
                                   << flush;
  }
#endif

  if(tools::globals::VOTCA_MKL){
     CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Using MKL overload for Eigen "<< flush;
  }else{
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Using native Eigen implementation, no BLAS overload "<< flush;
  }

  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Molecule Coordinates [A] " << flush;
  for (QMAtom* atom:_orbitals.QMAtoms()) {
    std::string output=(boost::format("  %1$s"
                                         "   %2$+1.4f %3$+1.4f %4$+1.4f")
                         %atom->getType() %(atom->getPos().getX()*tools::conv::bohr2ang)
                         %(atom->getPos().getY()*tools::conv::bohr2ang)
                         %(atom->getPos().getZ()*tools::conv::bohr2ang) ).str();

        CTP_LOG(ctp::logDEBUG, *_pLog) <<output<< flush;
  }

  /* check which QC program was used for the DFT run
   * -> implicit info about MO coefficient storage order
   */
  std::string dft_package = _orbitals.getQMpackage();
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " DFT data was created by " << dft_package
                                 << flush;

   // store information in _orbitals for later use
  _orbitals.setRPAindices(_rpamin, _rpamax);
  _orbitals.setGWAindices(_qpmin, _qpmax);
  _orbitals.setBSEindices(_bse_vmin, _bse_vmax, _bse_cmin, _bse_cmax,
                           _bse_maxeigenvectors);

    if (_do_full_BSE)
    _orbitals.setBSEtype("full");
  else {
    _orbitals.setBSEtype("TDA");
  }

  // load DFT basis set (element-wise information) from xml file
  BasisSet dftbs;

  if (_dftbasis_name != _orbitals.getDFTbasis()) {
    throw std::runtime_error(
        "Name of the Basisset from .orb file: " + _orbitals.getDFTbasis() +
        " and from GWBSE optionfile " + _dftbasis_name + " do not agree.");
  }

  dftbs.LoadBasisSet(_dftbasis_name);
  _orbitals.setDFTbasis(_dftbasis_name);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Loaded DFT Basis Set "
                                 << _dftbasis_name << flush;

  // fill DFT AO basis by going through all atoms
  AOBasis dftbasis;
  dftbasis.AOBasisFill(dftbs, _orbitals.QMAtoms(), _fragA);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Filled DFT Basis of size "
                                 << dftbasis.AOBasisSize() << flush;
  if (dftbasis.getAOBasisFragB() > 0  && dftbasis.getAOBasisFragA()>0) {
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " FragmentA size "
                                   << dftbasis.getAOBasisFragA() << flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " FragmentB size "
                                   << dftbasis.getAOBasisFragB()<< flush;
  }

  // load auxiliary basis set (element-wise information) from xml file
  BasisSet auxbs;
  auxbs.LoadBasisSet(_auxbasis_name);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Loaded Auxbasis Set "
                                 << _auxbasis_name << flush;

  // fill auxiliary AO basis by going through all atoms
  AOBasis auxbasis;
  auxbasis.AOBasisFill(auxbs, _orbitals.QMAtoms());
  _orbitals.setAuxbasis(_auxbasis_name);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Filled Auxbasis of size "
                                 << auxbasis.AOBasisSize() << flush;

   // process the DFT data
  // a) form the expectation value of the XC functional in MOs
  Eigen::MatrixXd vxc=CalculateVXC(dftbasis);

  /*
   * for the representation of 2-point functions with the help of the
   * auxiliary basis, its AO overlap matrix is required.
   * cf. M. Rohlfing, PhD thesis, ch. 3
   */
  AOOverlap auxoverlap;
  // Fill overlap
  auxoverlap.Fill(auxbasis);

  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Filled Aux Overlap matrix of dimension: "
                                 << auxoverlap.Matrix().rows() << flush;

  /*
   *  for the calculation of Coulomb and exchange term in the self
   *  energy and electron-hole interaction, the Coulomb interaction
   *  is represented using the auxiliary basis set.
   *  Here, we need to prepare the Coulomb matrix expressed in
   *  the AOs of the auxbasis
   */

  // get Coulomb matrix as AOCoulomb
  AOCoulomb auxcoulomb;

  // Fill Coulomb matrix
  auxcoulomb.Fill(auxbasis);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Filled Aux Coulomb matrix of dimension: "
                                 << auxcoulomb.Matrix().rows() << flush;

  Eigen::MatrixXd Coulomb_sqrtInv=auxcoulomb.Pseudo_InvSqrt_GWBSE(auxoverlap,5e-7);
    auxoverlap.FreeMatrix();
    auxcoulomb.FreeMatrix();
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Calculated Matrix Sqrt of Aux Coulomb Matrix"
      << flush;
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Removed " << auxcoulomb.Removedfunctions()
      << " functions from Aux Coulomb matrix to avoid near linear dependencies" << flush;

  // container => M_mn
  // prepare 3-center integral object

  TCMatrix_gwbse Mmn;
  //rpamin here, because RPA needs till rpamin
  Mmn.Initialize(auxbasis.AOBasisSize(), _rpamin, _qpmax, _rpamin, _rpamax);
  Mmn.Fill(auxbasis, dftbasis, _orbitals.MOCoefficients());
  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp()
      << " Calculated Mmn_beta (3-center-repulsion x orbitals)  " << flush;

  // make _Mmn symmetric
  Mmn.MultiplyRightWithAuxMatrix(Coulomb_sqrtInv);

  CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp()
      << " Multiplied Mmn_beta with Coulomb Matrix " << flush;
  PPM ppm;
  RPA rpa;
  rpa.configure(_homo,_rpamin,_rpamax);
  Eigen::VectorXd screen_r=Eigen::VectorXd::Zero(1);
  screen_r(0)=ppm.getScreening_r();
  Eigen::VectorXd screen_i=Eigen::VectorXd::Zero(1);
  screen_i(0)=ppm.getScreening_i();
  rpa.setScreening(screen_r,screen_i);
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " Prepared RPA  " << flush;

  Sigma sigma=Sigma(_pLog);
  sigma.configure(_homo,_qpmin,_qpmax,_g_sc_max_iterations,_g_sc_limit);
  sigma.setDFTdata(_orbitals.getScaHFX(),&vxc,&_orbitals.MOEnergies());

  // initialize _qp_energies;
  // shift unoccupied levels by the shift
  Eigen::VectorXd gwa_energies = Eigen::VectorXd::Zero(_orbitals.getNumberOfLevels());
  for (int i = 0; i < gwa_energies.size(); ++i) {
    gwa_energies(i) = _orbitals.MOEnergies()(i);
    if (i > int(_homo)) {
      gwa_energies(i) += _shift;
    }
  }
  sigma.setGWAEnergies(gwa_energies);

   /* for automatic iteration of both G and W, we need to
   * - make a copy of _Mmn
   * - calculate eps
   * - construct ppm
   * - threecenters for sigma
   * - sigma_x
   * - sigma_c
   * - test for convergence
   *
   */

  if (!_iterate_gw) {
    _gw_sc_max_iterations = 1;
  }

  const Eigen::VectorXd &dft_energies = _orbitals.MOEnergies();
  for (int gw_iteration = 0; gw_iteration < _gw_sc_max_iterations;
       ++gw_iteration) {

    Eigen::VectorXd qp_old_rpa = gwa_energies;
    if (_iterate_gw) {
      CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " GW Iteraton "
                                     << gw_iteration + 1 << " of "
                                     << _gw_sc_max_iterations << flush;
    }

    if(gw_iteration%_reset_3c==0 && gw_iteration!=0){
      CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Rebuilding Mmn_beta (3-center-repulsion x orbitals)" << flush;
       Mmn.Fill(auxbasis, dftbasis, _orbitals.MOCoefficients());
       Mmn.MultiplyRightWithAuxMatrix(Coulomb_sqrtInv);
    }

    rpa.calculate_epsilon(gwa_energies,Mmn);
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                   << " Calculated epsilon via RPA  " << flush;
    ppm.PPM_construct_parameters(rpa);
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                   << " Constructed PPM parameters  " << flush;

    Mmn.MultiplyRightWithAuxMatrix(ppm.getPpm_phi());
    CTP_LOG(ctp::logDEBUG, *_pLog)
        << ctp::TimeStamp() << " Prepared threecenters for sigma  " << flush;

    sigma.CalcdiagElements(Mmn,ppm);
    CTP_LOG(ctp::logDEBUG, *_pLog)
        << ctp::TimeStamp() << " Calculated diagonal part of Sigma  " << flush;
    // iterative refinement of qp energies
    gwa_energies=sigma.getGWAEnergies();
    double _DFTgap = dft_energies(_homo + 1) - dft_energies(_homo);
    double _QPgap = gwa_energies(_homo + 1) - gwa_energies(_homo);
    _shift = _QPgap - _DFTgap;

    // qp energies outside the update range are simply shifted.
    for (int i = _qpmax + 1; i < dft_energies.size(); ++i) {
      gwa_energies(i) = dft_energies(i) + _shift;
    }

    if (_iterate_gw) {
      bool gw_converged = true;
      Eigen::VectorXd diff = qp_old_rpa - gwa_energies;
      int state = 0;
      double E_diff_max=diff.cwiseAbs().maxCoeff(&state);
      if(E_diff_max>_gw_sc_limit){
           gw_converged = false;
      }

      double alpha = 0.0;
      gwa_energies = alpha * qp_old_rpa + (1 - alpha) * gwa_energies;
      sigma.setGWAEnergies(gwa_energies);
      if (tools::globals::verbose) {
        CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp() << " GW_Iteration: " << gw_iteration + 1
            << " shift=" << _shift << " E_diff max=" << E_diff_max
            << " StateNo:" << state << flush;
      }

      if (gw_converged) {
        CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp() << " Converged after " << gw_iteration + 1
            << " GW iterations" << flush;
        break;
      } else if (gw_iteration == _gw_sc_max_iterations - 1) {
        // continue regardless for now, but drop WARNING
        CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp() << " WARNING! GWA spectrum not converged after "
            << _gw_sc_max_iterations << " iterations." << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp() << "          GWA level " << state
            << " energy changed by " << E_diff_max << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog)
            << ctp::TimeStamp()
            << "          Run continues. Inspect results carefully!" << flush;
        break;
      }
    }else
    {
      sigma.setGWAEnergies(gwa_energies);
     }
  }

  ppm.FreeMatrix();
  rpa.FreeMatrices();
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                   << " Cleaned up PPM and MmnRPA Matrices" << flush;

  // Output of quasiparticle energies after all is done:
  PrintGWA_Energies(vxc, sigma, dft_energies);

  // store perturbative QP energy data in orbitals object (DFT, S_x,S_c, V_xc,
  // E_qp)
  if (_store_qp_pert) {
    Eigen::MatrixXd &qp_energies_store = _orbitals.QPpertEnergies();
    qp_energies_store=Eigen::MatrixXd::Zero(_qptotal, 5);
    for (int i = 0; i < _qptotal; i++) {
      qp_energies_store(i, 0) = dft_energies(i + _qpmin);
      qp_energies_store(i, 1) = sigma.x(i);
      qp_energies_store(i, 2) = sigma.c(i);
      qp_energies_store(i, 3) = vxc(i, i);
      qp_energies_store(i, 4) = gwa_energies(i + _qpmin);
    }
  }

  if (_do_qp_diag || _do_bse_singlets || _do_bse_triplets) {
      CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Calculating offdiagonal part of Sigma  " << flush;
  sigma.CalcOffDiagElements(Mmn,ppm);
   CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Calculated offdiagonal part of Sigma  " << flush;
    // free no longer required three-center matrices in _Mmn
  // max required is _bse_cmax (could be smaller than _qpmax)
  Mmn.Prune(_bse_vmin, _bse_cmax);
    Eigen::MatrixXd Hqp=sigma.SetupFullQPHamiltonian();

    if (_do_qp_diag) {
      Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Hqp);
      const Eigen::VectorXd& qp_diag_energies=es.eigenvalues();

      if(es.info()==Eigen::ComputationInfo::Success){
    CTP_LOG(ctp::logDEBUG, *_pLog)
      << ctp::TimeStamp() << " Diagonalized QP Hamiltonian  "<< flush;
      }

      PrintQP_Energies(gwa_energies, qp_diag_energies);

      if (_store_qp_diag) {
        _orbitals.QPdiagCoefficients()=es.eigenvectors();
        _orbitals.QPdiagEnergies()=es.eigenvalues();
      }
    }  // _do_qp_diag

  // proceed only if BSE requested
  if (_do_bse_singlets || _do_bse_triplets) {

      BSE bse=BSE(_orbitals,_pLog,_min_print_weight);
      bse.setBSEindices(_homo,_bse_vmin,_bse_cmax,_bse_maxeigenvectors);
      bse.setGWData(&Mmn,&ppm,&Hqp);
       // calculate direct part of eh interaction, needed for singlets and triplets

        if (_do_bse_triplets && _do_bse_diag) {
            bse.Solve_triplets();
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                    << " Solved BSE for triplets " << flush;
            // analyze and report results
            bse.Analyze_triplets(dftbasis);
            if (!_store_bse_triplets) {
                bse.FreeTriplets();
            }
        } // do_triplets

        if (_do_bse_singlets && _do_bse_diag) {
            if (_do_full_BSE) {
                bse.Solve_singlets_BTDA();
                CTP_LOG(ctp::logDEBUG, *_pLog)
                        << ctp::TimeStamp() << " Solved full BSE for singlets " << flush;
            } else {
                bse.Solve_singlets();
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                        << " Solved BSE for singlets " << flush;
            }
            bse.Analyze_singlets(dftbasis);
            if (!_store_bse_singlets) {
                bse.FreeSinglets();
            }
        }
        if (_store_eh_interaction) {
            if (_do_bse_singlets) {
                bse.SetupHs();
            }
            if (_do_bse_triplets) {
                bse.SetupHt();
            }
        }
    }
  }
  CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()
                                 << " GWBSE calculation finished " << flush;
  return true;
}
}
};


// #include "nabo/nabo.h"
#include "helpers.hpp"
#include <pcl/io/ply_io.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <omp.h>
#include <iostream>

// using namespace Nabo;
using namespace Eigen;
using namespace std;
using namespace TransformationUtilities;
using namespace InputUtilities;
using namespace InterfaceUtilities;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

class Optimizer
{
    private:
        string config_file;
        vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_downsampled;
        vector<MatrixXd> inverse_kinematics;
        std::vector<double> transformation_initial;
        std::vector<double> flange_transformation_initial;
        vector<double> getTransVector(boost::property_tree::ptree &pt,string s);
    public:
        Optimizer(string filename);
        void getInputs();
        void discreteCombintorialOptimizerTranslation();
        void discreteCombintorialOptimizerRotation();
        void discreteCombintorialOptimizerSmallBruteForce();
        void discreteCombintorialOptimizerObject();
        void discreteCombintorialOptimizerCamera();
        std::vector<double> transformation;
        std::vector<double> flange_transformation;

};

Optimizer::Optimizer(string filename)
{
    config_file = filename;
    cloud.reset (new PointCloudT);
    transformation = std::vector<double>(6,0);
    flange_transformation = std::vector<double>(6,0);
    transformation_initial = std::vector<double>(6,0);
    flange_transformation_initial = std::vector<double>(6,0);
}

vector<double> Optimizer::getTransVector(boost::property_tree::ptree &pt,string s)
{
    string angle_metric = pt.get<std::string>(s+".angle","radian");
    string camera_approx_trans_metric = pt.get<std::string>(s+".metric","m");
    double cam_approx_scale = 1.0;
    if(camera_approx_trans_metric=="cm")
        cam_approx_scale = 100.0;
    else if(camera_approx_trans_metric=="mm")
        cam_approx_scale=1000.0;
    string approx_transformation = pt.get<std::string>(s+".value","0,0,0,0,0,0");
    cout<<approx_transformation<<endl;
    vector<string> coords_str;
    boost::split(coords_str, approx_transformation , boost::is_any_of(","));
    vector<double> coords;
    for(int i=0;i<coords_str.size();i++)
        if(i<3)
            coords.push_back(stof(coords_str[i])/cam_approx_scale);
        else
        {
            double value = stof(coords_str[i]);
            if(angle_metric=="degree")
                value=degreeToRadian(value);
            coords.push_back(value);
        }
    for(auto x:coords)
        cout<<x<<" ";
    cout<<endl;
    return coords;
}
void Optimizer::getInputs()
{
    ifstream file;
    std::cout<<"Config File: "<<config_file<<endl;
    file.open(config_file);
    using boost::property_tree::ptree;
    ptree pt;
    read_xml(file, pt);
    string camera_metric = pt.get<std::string>("data.camera.metric","m");
    cout<<"------------- "<<camera_metric<<endl;
    for (const auto &cloud : pt.get_child("data.camera.clouds"))
    {
        string filename = cloud.second.data();
	ifstream file(filename);
	string line;
	for(int c=0;c<14&&getline(file,line);c++);
        PointCloudT::Ptr pointcloud(new PointCloudT);
	while(getline(file,line))
	{
		vector<string> values_from_file;
		boost::split(values_from_file, line, boost::is_any_of(" "));
		pcl::PointXYZRGB pt;
		pt.x = stof(values_from_file[0]);
		pt.y = stof(values_from_file[1]);
		pt.z = stof(values_from_file[2]);
		pt.r = stof(values_from_file[3]);
		pt.g = stof(values_from_file[4]);
		pt.b = stof(values_from_file[5]);
		pointcloud->points.push_back(pt);
	}
        PointCloudT::Ptr temp_cloud(new PointCloudT);
        for(int i=0;i<pointcloud->points.size();i++)
        {
            auto pt = pointcloud->points[i];
            if(pt.x==0.0&&pt.y==0.0&&pt.z==0.0)
                continue;
            temp_cloud->points.push_back(pt);
        }
        clouds.push_back(temp_cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bw_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*temp_cloud,*cloud_bw_temp);
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud (cloud_bw_temp);
        constexpr double leaf = 0.0225f;
        // constexpr double leaf = 0.007f;
        sor.setLeafSize (leaf, leaf, leaf);
        sor.filter (*cloud_filtered);
        cloud_downsampled.push_back(cloud_filtered);
        // PointCloudT::Ptr pointcloud_output(new PointCloudT);
        // cloud_outputs.push_back(pointcloud_output);
        cout<<filename<<endl;
    }
    string ik_filename  = pt.get<std::string>("data.camera.transformations.inverse_kinematics");
    cout<<ik_filename<<endl;
    inverse_kinematics = readTransformations(ik_filename,true);
    cout<<"Transformations Read"<<endl;
    //Reading the scan cloud.
    string object_metric = pt.get<std::string>("data.scan.metric","m");
    cout<<"------------- "<<object_metric <<endl;
    for(const auto &cloud_location :pt.get_child("data.scan.clouds"))
    {
        string filename = cloud_location.second.data();
        readPointCloud(filename,cloud,object_metric );
    }
    cout<<"Scan read"<<endl;
    //Reading Default Values;
    auto transformation_initial_object= getTransVector(pt,"data.scan.transformations.approximate_transformation");
    for(int i=0;i<transformation_initial_object.size();i++)
    {
        transformation[i]=transformation_initial_object[i];
        transformation_initial[i] = transformation_initial_object[i];
    }
    auto transformation_initial_flange= getTransVector(pt,"data.camera.transformations.approximate_transformation");
    for(int i=0;i<transformation_initial_flange.size();i++)
    {
        flange_transformation[i]=transformation_initial_flange[i];
        flange_transformation_initial[i]=transformation_initial_flange[i];
    }
    cout<<"Here"<<endl;
}

void Optimizer::discreteCombintorialOptimizerTranslation()
{
    int K = 1;
    MatrixXf M = MatrixXf::Zero(3, cloud->points.size());
    for(int i=0;i<cloud->points.size();i++)
    {
        auto pt = cloud->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
    //Setting up the structures.
    vector<MatrixXf> Ns;
    vector<MatrixXi> indis;
    vector<MatrixXf> dists;
    for(int i=0;i<clouds.size();i++)
    {
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled[i]->points.size());
        Ns.push_back(N);
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, Ns[i].cols());
        dists2.resize(1, Ns[i].cols());
        indis.push_back(indices);
        dists.push_back(dists2);
        cout<<"Points Size: "<<cloud_downsampled[i]->points.size()<<endl;
    }
    double min_error = 1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    vector<double> trans(6),flange_trans(6);
    int iter_min = -25;
    int iter_max = 25;
    TIC();
    int iterations = 0;
    for(int xf=iter_min;xf<=iter_max;xf+=5)
        for(int yf=iter_min;yf<=iter_max;yf+=5)
            for(int zf=iter_min;zf<=iter_max;zf+=5)
                for(int xo=iter_min;xo<=iter_max;xo+=5)
                    for(int yo=iter_min;yo<=iter_max;yo+=5)
                        for(int zo=iter_min;zo<=iter_max;zo+=5)
                        {
                            double err = 0.0;
                            flange_transformation[0]+=xf/1000.0;
                            flange_transformation[1]+=yf/1000.0;
                            flange_transformation[2]+=zf/1000.0;
                            transformation[0]+=xo/1000.0;
                            transformation[1]+=yo/1000.0;
                            transformation[2]+=zo/1000.0;
                            TIC();
                            // #pragma omp parallel
                            // #pragma omp for
                            for(int j=0;j<clouds.size();j++)
                            {
                                float average = 0.0;
                                float maximum = -1e9;
                                int counter = 0;
                                Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation);
                                Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
                                Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
                                world_T_object = world_T_object.inverse();
                                Eigen::Affine3d trans;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        trans(a,b) = transformation(a,b);
                                Eigen::Affine3d transW;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        transW(a,b) = world_T_object(a,b);
                                for(int i=0;i<cloud_downsampled[j]->points.size();i++)
                                {
                                    auto point = cloud_downsampled[j]->points[i];
#if 0
                                    float src[3];
                                    float out[3];
                                    src[0] = point.x;
                                    src[1] = point.y;
                                    src[2] = point.z;
                                    apply_transformation_optimized(src,out,trans);
                                    src[0] = out[0];
                                    src[1] = out[1];
                                    src[2] = out[2];
                                    apply_transformation_optimized(src,out,transW);
                                    Ns[j](0,i) = out[0];
                                    Ns[j](1,i) = out[1];
                                    Ns[j](2,i) = out[2];
#else
                                    pts(0,0)=point.x;
                                    pts(0,1)=point.y;
                                    pts(0,2)=point.z;
                                    pts=apply_transformation(pts,transformation);
                                    pts=apply_transformation(pts,world_T_object);
                                    Ns[j](0,i) = pts(0,0);
                                    Ns[j](1,i) = pts(0,1);
                                    Ns[j](2,i) = pts(0,2);
#endif
                                    counter++;
                                }

                                // nns->knn(Ns[j], indis[j],dists[j], 1, 0.1, NNSearchF::SORT_RESULTS);

                                for(int i=0;i<counter;i++)
                                {
                                    double distance = sqrt(dists[j](0,i));
                                    if(maximum<distance)
                                        maximum=distance;
                                    average+=distance;
                                }
                                average=average/counter;
                                err = err + average;
                            }
                            if(err<min_error)
                            {
                                min_error = err;
                                flange_trans = flange_transformation;
                                trans = transformation;
                            }
                            if(iterations%100000==0)
                            {
                                cout<<"Iteration: "<<iterations<<endl;
                                cout<<xf<<" "<<yf<<" "<<zf<<" "<<xo<<" "<<yo<<" "<<zo<<endl;
                                cout<<"Minimum Error: "<<min_error<<endl;
                                cout<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                cout<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                ofstream ofile("temp.tmp",std::ios_base::app);
                                ofile<<"Iteration: "<<iterations<<endl;
                                ofile<<xf<<" "<<yf<<" "<<zf<<" "<<xo<<" "<<yo<<" "<<zo<<endl;
                                ofile<<"Minimum Error: "<<min_error<<endl;
                                ofile<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                ofile<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                sleep(10);
                                cout<<"Sleep Over"<<endl;
                            }
                            flange_transformation[0]-=xf/1000.0;
                            flange_transformation[1]-=yf/1000.0;
                            flange_transformation[2]-=zf/1000.0;
                            transformation[0]-=xo/1000.0;
                            transformation[1]-=yo/1000.0;
                            transformation[2]-=zo/1000.0;
                            iterations++;
                        }
    TOC();
    cout<<"Iterations: "<<iterations<<endl;
    cout<<"Final Minimum Error: "<<min_error<<endl;
    cout<<"Flange Transformation"<<endl;
    for(auto x:flange_trans)
        cout<<x<<" ";
    cout<<"Object Transformation"<<endl;
    for(auto x:trans)
        cout<<x<<" ";
    flange_transformation = flange_trans;
    transformation = trans;
}

void Optimizer::discreteCombintorialOptimizerRotation()
{
    int K = 1;
    MatrixXf M = MatrixXf::Zero(3, cloud->points.size());
    for(int i=0;i<cloud->points.size();i++)
    {
        auto pt = cloud->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
    //Setting up the structures.
    vector<MatrixXf> Ns;
    vector<MatrixXi> indis;
    vector<MatrixXf> dists;
    for(int i=0;i<clouds.size();i++)
    {
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled[i]->points.size());
        Ns.push_back(N);
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, Ns[i].cols());
        dists2.resize(1, Ns[i].cols());
        indis.push_back(indices);
        dists.push_back(dists2);
        cout<<"Points Size: "<<cloud_downsampled[i]->points.size()<<endl;
    }
    double min_error = 1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    vector<double> trans(6),flange_trans(6);
    int iter_min = -1;
    int iter_max = 1;
    TIC();
    int iterations = 0;
    for(float xf=iter_min;xf<=iter_max;xf+=1)
        for(float yf=iter_min;yf<=iter_max;yf+=1)
            for(float zf=iter_min;zf<=iter_max;zf+=1)
                for(float xo=iter_min;xo<=iter_max;xo+=1)
                    for(float yo=iter_min;yo<=iter_max;yo+=1)
                        for(float zo=iter_min;zo<=iter_max;zo+=1)
                        {
                            double err = 0.0;
                            flange_transformation[3]+=degreeToRadian(double(xf));
                            flange_transformation[4]+=degreeToRadian(double(yf));
                            flange_transformation[5]+=degreeToRadian(double(zf));
                            transformation[3]+=degreeToRadian(double(xo));
                            transformation[4]+=degreeToRadian(double(yo));
                            transformation[5]+=degreeToRadian(double(zo));
                            TIC();
                            // #pragma omp parallel
                            // #pragma omp for
                            for(int j=0;j<clouds.size();j++)
                            {
                                float average = 0.0;
                                float maximum = -1e9;
                                int counter = 0;
                                Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation);
                                Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
                                Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
                                world_T_object = world_T_object.inverse();
                                Eigen::Affine3d trans;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        trans(a,b) = transformation(a,b);
                                Eigen::Affine3d transW;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        transW(a,b) = world_T_object(a,b);
                                for(int i=0;i<cloud_downsampled[j]->points.size();i++)
                                {
                                    auto point = cloud_downsampled[j]->points[i];
#if 1
                                    float src[3];
                                    float out[3];
                                    src[0] = point.x;
                                    src[1] = point.y;
                                    src[2] = point.z;
                                    apply_transformation_optimized(src,out,trans);
                                    src[0] = out[0];
                                    src[1] = out[1];
                                    src[2] = out[2];
                                    apply_transformation_optimized(src,out,transW);
                                    Ns[j](0,i) = out[0];
                                    Ns[j](1,i) = out[1];
                                    Ns[j](2,i) = out[2];
#else
                                    pts(0,0)=point.x;
                                    pts(0,1)=point.y;
                                    pts(0,2)=point.z;
                                    pts=apply_transformation(pts,transformation);
                                    pts=apply_transformation(pts,world_T_object);
                                    Ns[j](0,i) = pts(0,0);
                                    Ns[j](1,i) = pts(0,1);
                                    Ns[j](2,i) = pts(0,2);
#endif
                                    counter++;
                                }

                                // nns->knn(Ns[j], indis[j],dists[j], 1, 0.1, NNSearchF::SORT_RESULTS);

                                for(int i=0;i<counter;i++)
                                {
                                    double distance = sqrt(dists[j](0,i));
                                    if(maximum<distance)
                                        maximum=distance;
                                    average+=distance;
                                }
                                average=average/counter;
                                err = err + average*0.5 + maximum*0.5;
                            }
                            if(err<min_error)
                            {
                                min_error = err;
                                flange_trans = flange_transformation;
                                trans = transformation;
                            }
                            if(iterations%100000==0)
                            {
                                cout<<"Iteration: "<<iterations<<endl;
                                cout<<xf<<" "<<yf<<" "<<zf<<" "<<xo<<" "<<yo<<" "<<zo<<endl;
                                cout<<"Minimum Error: "<<min_error<<endl;
                                cout<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                cout<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    cout<<x<<" ";
                                cout<<endl;
                            }
                            flange_transformation[3]-=degreeToRadian(xf/10.0);
                            flange_transformation[4]-=degreeToRadian(yf/10.0);
                            flange_transformation[5]-=degreeToRadian(zf/10.0);
                            transformation[3]-=degreeToRadian(xo/10.0);
                            transformation[4]-=degreeToRadian(yo/10.0);
                            transformation[5]-=degreeToRadian(zo/10.0);
                            iterations++;
                        }
    TOC();
    cout<<"Iterations: "<<iterations<<endl;
    cout<<"Final Minimum Error: "<<min_error<<endl;
    cout<<"Flange Transformation"<<endl;
    for(auto x:flange_trans)
        cout<<x<<" ";
    cout<<endl;
    cout<<"Object Transformation"<<endl;
    for(auto x:trans)
        cout<<x<<" ";
    cout<<endl;
    cout<<degreeToRadian(1)<<endl;
}

void Optimizer::discreteCombintorialOptimizerObject()
{
    int K = 1;
    MatrixXf M = MatrixXf::Zero(3, cloud->points.size());
    for(int i=0;i<cloud->points.size();i++)
    {
        auto pt = cloud->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
    //Setting up the structures.
    vector<MatrixXf> Ns;
    vector<MatrixXi> indis;
    vector<MatrixXf> dists;
    for(int i=0;i<clouds.size();i++)
    {
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled[i]->points.size());
        Ns.push_back(N);
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, Ns[i].cols());
        dists2.resize(1, Ns[i].cols());
        indis.push_back(indices);
        dists.push_back(dists2);
        cout<<"Points Size: "<<cloud_downsampled[i]->points.size()<<endl;
    }
    double min_error = 1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    vector<double> trans(6),flange_trans(6);
    int iter_min = -10;
    int iter_max = 10;
    TIC();
    int iterations = 0;
    for(int xc=iter_min;xc<iter_max;xc+=2)
    for(int yc=iter_min;yc<iter_max;yc+=2)
    for(int zc=iter_min;zc<iter_max;zc+=2)
    for(int xr=iter_min;xr<iter_max;xr+=2)
    for(int yr=iter_min;yr<iter_max;yr+=2)
    for(int zr=iter_min;zr<iter_max;zr+=2)
                        {
                            double err = 0.0;
                            transformation[0]+=xc/1000.0;
                            transformation[1]+=yc/1000.0;
                            transformation[2]+=zc/1000.0;
                            transformation[3]+=degreeToRadian(xr/5.0);
                            transformation[4]+=degreeToRadian(yr/5.0);
                            transformation[5]+=degreeToRadian(zr/5.0);
                            TIC();
                            // #pragma omp parallel
                            // #pragma omp for
                            for(int j=0;j<clouds.size();j++)
                            {
                                float average = 0.0;
                                float maximum = -1e9;
                                int counter = 0;
                                Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation);
                                Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
                                Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
                                world_T_object = world_T_object.inverse();
                                Eigen::Affine3d trans;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        trans(a,b) = transformation(a,b);
                                Eigen::Affine3d transW;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        transW(a,b) = world_T_object(a,b);
                                for(int i=0;i<cloud_downsampled[j]->points.size();i++)
                                {
                                    auto point = cloud_downsampled[j]->points[i];
#if 0
                                    float src[3];
                                    float out[3];
                                    src[0] = point.x;
                                    src[1] = point.y;
                                    src[2] = point.z;
                                    apply_transformation_optimized(src,out,trans);
                                    src[0] = out[0];
                                    src[1] = out[1];
                                    src[2] = out[2];
                                    apply_transformation_optimized(src,out,transW);
                                    Ns[j](0,i) = out[0];
                                    Ns[j](1,i) = out[1];
                                    Ns[j](2,i) = out[2];
#else
                                    pts(0,0)=point.x;
                                    pts(0,1)=point.y;
                                    pts(0,2)=point.z;
                                    pts=apply_transformation(pts,transformation);
                                    pts=apply_transformation(pts,world_T_object);
                                    Ns[j](0,i) = pts(0,0);
                                    Ns[j](1,i) = pts(0,1);
                                    Ns[j](2,i) = pts(0,2);
#endif
                                    counter++;
                                }

                                // nns->knn(Ns[j], indis[j],dists[j], 1, 0.1, NNSearchF::SORT_RESULTS);

                                for(int i=0;i<counter;i++)
                                {
                                    double distance = sqrt(dists[j](0,i));
                                    if(maximum<distance)
                                        maximum=distance;
                                    average+=distance;
                                }
                                average=average/counter;
                                err = err + average;
                            }
                            if(err<min_error)
                            {
                                min_error = err;
                                flange_trans = flange_transformation;
                                trans = transformation;
                            }
                            if(iterations%100000==0)
                            {
                                cout<<"Iteration: "<<iterations<<endl;
                                cout<<"Minimum Error: "<<min_error<<endl;
                                cout<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                cout<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                ofstream ofile("temp.tmp",std::ios_base::app);
                                ofile<<"Iteration: "<<iterations<<endl;
                                ofile<<"Minimum Error: "<<min_error<<endl;
                                ofile<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                ofile<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                sleep(10);
                                cout<<"Sleep Over"<<endl;
                            }
                            transformation[0]-=xc/1000.0;
                            transformation[1]-=yc/1000.0;
                            transformation[2]-=zc/1000.0;
                            transformation[3]-=degreeToRadian(xr/5.0);
                            transformation[4]-=degreeToRadian(yr/5.0);
                            transformation[5]-=degreeToRadian(zr/5.0);
                            iterations++;
                        }
    TOC();
    cout<<"Iterations: "<<iterations<<endl;
    cout<<"Final Minimum Error: "<<min_error<<endl;
    cout<<"Flange Transformation"<<endl;
    for(auto x:flange_trans)
        cout<<x<<" ";
    cout<<"Object Transformation"<<endl;
    for(auto x:trans)
        cout<<x<<" ";
    flange_transformation = flange_trans;
    transformation = trans;
}

void Optimizer::discreteCombintorialOptimizerCamera()
{
    int K = 1;
    MatrixXf M = MatrixXf::Zero(3, cloud->points.size());
    for(int i=0;i<cloud->points.size();i++)
    {
        auto pt = cloud->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
    //Setting up the structures.
    vector<MatrixXf> Ns;
    vector<MatrixXi> indis;
    vector<MatrixXf> dists;
    for(int i=0;i<clouds.size();i++)
    {
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled[i]->points.size());
        Ns.push_back(N);
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, Ns[i].cols());
        dists2.resize(1, Ns[i].cols());
        indis.push_back(indices);
        dists.push_back(dists2);
        cout<<"Points Size: "<<cloud_downsampled[i]->points.size()<<endl;
    }
    double min_error = 1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    vector<double> trans(6),flange_trans(6);
    int iter_min = -10;
    int iter_max = 10;
    TIC();
    int iterations = 0;
    for(int xc=iter_min;xc<iter_max;xc+=2)
    for(int yc=iter_min;yc<iter_max;yc+=2)
    for(int zc=iter_min;zc<iter_max;zc+=2)
    for(int xr=iter_min;xr<iter_max;xr+=2)
    for(int yr=iter_min;yr<iter_max;yr+=2)
    for(int zr=iter_min;zr<iter_max;zr+=2)
                        {
                            double err = 0.0;
                            flange_transformation[0]+=xc/1000.0;
                            flange_transformation[1]+=yc/1000.0;
                            flange_transformation[2]+=zc/1000.0;
                            flange_transformation[3]+=degreeToRadian(xr/5.0);
                            flange_transformation[4]+=degreeToRadian(yr/5.0);
                            flange_transformation[5]+=degreeToRadian(zr/5.0);
                            TIC();
                            // #pragma omp parallel
                            // #pragma omp for
                            for(int j=0;j<clouds.size();j++)
                            {
                                float average = 0.0;
                                float maximum = -1e9;
                                int counter = 0;
                                Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation);
                                Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
                                Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
                                world_T_object = world_T_object.inverse();
                                Eigen::Affine3d trans;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        trans(a,b) = transformation(a,b);
                                Eigen::Affine3d transW;
                                for(int a=0;a<3;a++)
                                    for(int b=0;b<4;b++)
                                        transW(a,b) = world_T_object(a,b);
                                for(int i=0;i<cloud_downsampled[j]->points.size();i++)
                                {
                                    auto point = cloud_downsampled[j]->points[i];
#if 0
                                    float src[3];
                                    float out[3];
                                    src[0] = point.x;
                                    src[1] = point.y;
                                    src[2] = point.z;
                                    apply_transformation_optimized(src,out,trans);
                                    src[0] = out[0];
                                    src[1] = out[1];
                                    src[2] = out[2];
                                    apply_transformation_optimized(src,out,transW);
                                    Ns[j](0,i) = out[0];
                                    Ns[j](1,i) = out[1];
                                    Ns[j](2,i) = out[2];
#else
                                    pts(0,0)=point.x;
                                    pts(0,1)=point.y;
                                    pts(0,2)=point.z;
                                    pts=apply_transformation(pts,transformation);
                                    pts=apply_transformation(pts,world_T_object);
                                    Ns[j](0,i) = pts(0,0);
                                    Ns[j](1,i) = pts(0,1);
                                    Ns[j](2,i) = pts(0,2);
#endif
                                    counter++;
                                }

                                // nns->knn(Ns[j], indis[j],dists[j], 1, 0.1, NNSearchF::SORT_RESULTS);

                                for(int i=0;i<counter;i++)
                                {
                                    double distance = sqrt(dists[j](0,i));
                                    if(maximum<distance)
                                        maximum=distance;
                                    average+=distance;
                                }
                                average=average/counter;
                                err = err + average;
                            }
                            if(err<min_error)
                            {
                                min_error = err;
                                flange_trans = flange_transformation;
                                trans = transformation;
                            }
                            if(iterations%100000==0)
                            {
                                cout<<"Iteration: "<<iterations<<endl;
                                cout<<"Minimum Error: "<<min_error<<endl;
                                cout<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                cout<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    cout<<x<<" ";
                                cout<<endl;
                                ofstream ofile("temp.tmp",std::ios_base::app);
                                ofile<<"Iteration: "<<iterations<<endl;
                                ofile<<"Minimum Error: "<<min_error<<endl;
                                ofile<<"Flange Transformation"<<endl;
                                for(auto x:flange_trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                ofile<<"Object Transformation"<<endl;
                                for(auto x:trans)
                                    ofile<<x<<" ";
                                ofile<<endl;
                                sleep(10);
                                cout<<"Sleep Over"<<endl;
                            }
                            flange_transformation[0]-=xc/1000.0;
                            flange_transformation[1]-=yc/1000.0;
                            flange_transformation[2]-=zc/1000.0;
                            flange_transformation[3]-=degreeToRadian(xr/5.0);
                            flange_transformation[4]-=degreeToRadian(yr/5.0);
                            flange_transformation[5]-=degreeToRadian(zr/5.0);
                            iterations++;
                        }
    TOC();
    cout<<"Iterations: "<<iterations<<endl;
    cout<<"Final Minimum Error: "<<min_error<<endl;
    cout<<"Flange Transformation"<<endl;
    for(auto x:flange_trans)
        cout<<x<<" ";
    cout<<"Object Transformation"<<endl;
    for(auto x:trans)
        cout<<x<<" ";
    flange_transformation = flange_trans;
    transformation = trans;
}

void Optimizer::discreteCombintorialOptimizerSmallBruteForce()
{
    int K = 1;
    MatrixXf M = MatrixXf::Zero(3, cloud->points.size());
    for(int i=0;i<cloud->points.size();i++)
    {
        auto pt = cloud->points[i];
        M(0,i) = pt.x;
        M(1,i) = pt.y;
        M(2,i) = pt.z;
    }
    // NNSearchF* nns = NNSearchF::createKDTreeLinearHeap(M);
    //Setting up the structures.
    vector<MatrixXf> Ns;
    vector<MatrixXi> indis;
    vector<MatrixXf> dists;
    for(int i=0;i<clouds.size();i++)
    {
        MatrixXf N = MatrixXf::Zero(3, cloud_downsampled[i]->points.size());
        Ns.push_back(N);
        MatrixXi indices;
        MatrixXf dists2;
        indices.resize(1, Ns[i].cols());
        dists2.resize(1, Ns[i].cols());
        indis.push_back(indices);
        dists.push_back(dists2);
        cout<<"Points Size: "<<cloud_downsampled[i]->points.size()<<endl;
    }
    double min_error = 1e9;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    vector<double> trans(6),flange_trans(6);
    int iter_min = -1;
    int iter_max = 1;
    TIC();
    int iterations = 0;
    for(int x=0;x<7;x++)
        for(int a=iter_min;a<=iter_max;a++)
            for(int b=iter_min;b<=iter_max;b++)
                for(int c=iter_min;c<=iter_max;c++)
                    for(int d=iter_min;d<=iter_max;d++)
                        for(int e=iter_min;e<=iter_max;e++)
                            for(int f=iter_min;f<=iter_max;f++)
                                for(int g=iter_min;g<=iter_max;g++)
                                    for(int h=iter_min;h<=iter_max;h++)
                                        for(int i=iter_min;i<=iter_max;i++)
                                            for(int j=iter_min;j<=iter_max;j++)
                                                for(int k=iter_min;k<=iter_max;k++)
                                                    for(int l=iter_min;l<=iter_max;l++)
                                                    {
                                                        double err = 0.0;
                                                        flange_transformation[0]+=a/1000.0;
                                                        flange_transformation[1]+=b/1000.0;
                                                        flange_transformation[2]+=c/1000.0;
                                                        flange_transformation[3]+=degreeToRadian(float(d/2.0));
                                                        flange_transformation[4]+=degreeToRadian(float(e/2.0));
                                                        flange_transformation[5]+=degreeToRadian(float(f/2.0));
                                                        transformation[0]+=g/1000.0;
                                                        transformation[1]+=h/1000.0;
                                                        transformation[2]+=i/1000.0;
                                                        transformation[3]+=degreeToRadian(float(j/2.0));
                                                        transformation[4]+=degreeToRadian(float(k/2.0));
                                                        transformation[5]+=degreeToRadian(float(l/2.0));
                                                        TIC();
                                                        // #pragma omp parallel
                                                        // #pragma omp for
                                                        for(int j=0;j<clouds.size();j++)
                                                        {
                                                            float average = 0.0;
                                                            float maximum = -1e9;
                                                            int counter = 0;
                                                            Eigen::MatrixXd world_T_object = vectorToTransformationMatrix(transformation);
                                                            Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
                                                            Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
                                                            world_T_object = world_T_object.inverse();
                                                            Eigen::Affine3d trans;
                                                            for(int a=0;a<3;a++)
                                                                for(int b=0;b<4;b++)
                                                                    trans(a,b) = transformation(a,b);
                                                            Eigen::Affine3d transW;
                                                            for(int a=0;a<3;a++)
                                                                for(int b=0;b<4;b++)
                                                                    transW(a,b) = world_T_object(a,b);
                                                            for(int i=0;i<cloud_downsampled[j]->points.size();i++)
                                                            {
                                                                auto point = cloud_downsampled[j]->points[i];
#if 1
                                                                float src[3];
                                                                float out[3];
                                                                src[0] = point.x;
                                                                src[1] = point.y;
                                                                src[2] = point.z;
                                                                apply_transformation_optimized(src,out,trans);
                                                                src[0] = out[0];
                                                                src[1] = out[1];
                                                                src[2] = out[2];
                                                                apply_transformation_optimized(src,out,transW);
                                                                Ns[j](0,i) = out[0];
                                                                Ns[j](1,i) = out[1];
                                                                Ns[j](2,i) = out[2];
#else
                                                                pts(0,0)=point.x;
                                                                pts(0,1)=point.y;
                                                                pts(0,2)=point.z;
                                                                pts=apply_transformation(pts,transformation);
                                                                pts=apply_transformation(pts,world_T_object);
                                                                Ns[j](0,i) = pts(0,0);
                                                                Ns[j](1,i) = pts(0,1);
                                                                Ns[j](2,i) = pts(0,2);
#endif
                                                                counter++;
                                                            }

                                                            // nns->knn(Ns[j], indis[j],dists[j], 1, 0.1, NNSearchF::SORT_RESULTS);

                                                            for(int i=0;i<counter;i++)
                                                            {
                                                                double distance = sqrt(dists[j](0,i));
                                                                if(maximum<distance)
                                                                    maximum=distance;
                                                                average+=distance;
                                                            }
                                                            average=average/counter;
                                                            err = err + average*0.5 + maximum*0.5;
                                                        }
                                                        if(err<min_error)
                                                        {
                                                            min_error = err;
                                                            flange_trans = flange_transformation;
                                                            trans = transformation;
                                                        }
                                                        if(iterations%100000==0)
                                                        {
                                                            cout<<"Iteration: "<<iterations<<endl;
                                                            cout<<"Minimum Error: "<<min_error<<endl;
                                                            cout<<"Flange Transformation"<<endl;
                                                            for(auto x:flange_trans)
                                                                cout<<x<<" ";
                                                            cout<<endl;
                                                            cout<<"Object Transformation"<<endl;
                                                            for(auto x:trans)
                                                                cout<<x<<" ";
                                                            cout<<endl;
                                                        }
                                                        flange_transformation[0]-=a/1000.0;
                                                        flange_transformation[1]-=b/1000.0;
                                                        flange_transformation[2]-=c/1000.0;
                                                        flange_transformation[3]-=degreeToRadian(float(d/2.0));
                                                        flange_transformation[4]-=degreeToRadian(float(e/2.0));
                                                        flange_transformation[5]-=degreeToRadian(float(f/2.0));
                                                        transformation[0]-=g/1000.0;
                                                        transformation[1]-=h/1000.0;
                                                        transformation[2]-=i/1000.0;
                                                        transformation[3]-=degreeToRadian(float(j/2.0));
                                                        transformation[4]-=degreeToRadian(float(k/2.0));
                                                        transformation[5]-=degreeToRadian(float(l/2.0));
                                                        iterations++;
                                                    }
    TOC();
    cout<<"Iterations: "<<iterations<<endl;
    cout<<"Final Minimum Error: "<<min_error<<endl;
    cout<<"Flange Transformation"<<endl;
    for(auto x:flange_trans)
        cout<<x<<" ";
    cout<<endl;
    cout<<"Object Transformation"<<endl;
    for(auto x:trans)
        cout<<x<<" ";
    cout<<endl;
    cout<<degreeToRadian(1)<<endl;
}

int main(int argc, char** argv)
{
    if(argc<2)
    {
        std::cout<<"Usage: optimizer_test <config file>"<<endl;
        exit(-1);
    }
    Optimizer opti = Optimizer(string(argv[1]));
    opti.getInputs();
    cout<<" Discrete optimization on Translation.."<<endl;
    // opti.discreteCombintorialOptimizerSmallBruteForce();
    opti.discreteCombintorialOptimizerTranslation();
    opti.discreteCombintorialOptimizerRotation();
    // opti.discreteCombintorialOptimizerObject();
    // opti.discreteCombintorialOptimizerCamera();
    return 0;
}