#include <iostream>
#include <fstream>
#include <iterator>

#include <boost/program_options.hpp>

#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkLabelStatisticsImageFilter.h>
#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>
#include <itkMaskImageFilter.h>
#include <itkImageDuplicator.h>

int main(int argc,char **argv)
{
    std::string labelTableFile, labelImageFile, inImageFile, outTableFile, outImageFile;
    bool	writeOutTableFile(false), writeOutImageFile(false), startFromInImage(false);

    boost::program_options::options_description optionsDescription("Write a file with total and average Jacobians from a given label table, label image and Jacobian image.");
    optionsDescription.add_options()
	("help,h", "display help message")
	("labelTable,t", boost::program_options::value< std::string >(&labelTableFile), "labels in a single column for which sum and average of inImage is to be computed.")
	("inImage,i", boost::program_options::value< std::string >(&inImageFile), "Input intensity image whose mean regional values are to be computed.")
	("labelImage,l", boost::program_options::value< std::string >(&labelImageFile), "input label image file from which the regions matching the labels in the table will be extracted")
	("outTable,o",boost::program_options::value< std::string >(&outTableFile), "[Optional] If given create this table file which contains table with labels, mean values and number of voxels for the given label")
	("outImage,u",boost::program_options::value< std::string >(&outImageFile), "[Optional]. If given creates this image which has uniformized mean intensity in each ROIs corresponding to the given labels and labelImage. The mean intensity is computed from the given input intensity image. ")
	("startFromInImage,s",boost::program_options::bool_switch(&startFromInImage)->default_value(false), "If given the ouImage copies the intensity values of inImage for all ROIs not covered by the input labels; otherwise all regions not covered by labels will have zero value.")
	;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc,argv,optionsDescription),options);
    boost::program_options::notify(options);

//If help is asked!
    if(options.count("help")) {
	std::cout<<optionsDescription<<std::endl;
	return EXIT_SUCCESS;
    }
//Confirm all the options required are given.
    if(!options.count("labelTable") || !options.count("inImage") || !options.count("labelImage")
	)
    {
	std::cerr<<"invalid options! run with --help or -h to see the proper options."<<std::endl;
	return EXIT_FAILURE;
    }
    if(options.count("outTable"))
	writeOutTableFile = true;
    if(options.count("outImage"))
	writeOutImageFile = true;
    if(!(writeOutTableFile || writeOutImageFile))
    {
	std::cerr<<"neither of outTable and outImage given. Nothing to write => I do nothing. Provide at lease one of these filenames to write.";
	return EXIT_FAILURE;

    }
    typedef itk::Image<int, 3> LabelImageType;
    typedef itk::Image<float, 3> ImageType;
    // Container to put labels.
    typedef std::vector<LabelImageType::PixelType> LabelsVecType;
    typedef LabelsVecType::iterator LabelsVecItType;

// Open label table file for reading
    std::ifstream labelTable(labelTableFile.c_str(),std::ios::in);
    if (!labelTable.is_open()) {
	std::cerr<<"could not open file: "<<labelTableFile<<std::endl;
	return EXIT_FAILURE;
    }
    //Read all the numbers and put them into a vector.
    std::istream_iterator<double> start(labelTable), end;
    //std::vector<LabelImageType::PixelType> inLabels(start, end);
    LabelsVecType inLabels(start, end);
    std::cout << "Read " << inLabels.size() << " labels" << std::endl;
    labelTable.close();

    // for (LabelsVecItType it = inLabels.begin(); it != inLabels.end(); ++it) {
    // 	std::cout<<*it<<std::endl;
    // }

// Read input label image
    LabelImageType::Pointer labelImage;
    {
	typedef itk::ImageFileReader< LabelImageType > LabelImageReaderType;
	LabelImageReaderType::Pointer labelImageReader = LabelImageReaderType::New();
	labelImageReader->SetFileName(labelImageFile);
	labelImageReader->Update();
	labelImage = labelImageReader->GetOutput();
    }

// Read input intensity image.
    ImageType::Pointer inImage;
    {
	typedef itk::ImageFileReader< ImageType > InImageReaderType;
	InImageReaderType::Pointer inImageReader = InImageReaderType::New();
	inImageReader->SetFileName(inImageFile);
	inImageReader->Update();
	inImage = inImageReader->GetOutput();
    }

    //Label Statistics Filter to compute mean of each regions:
    typedef itk::LabelStatisticsImageFilter< ImageType, LabelImageType > LabelStatsFilterType;
    LabelStatsFilterType::Pointer labelStats = LabelStatsFilterType::New();

    labelStats->SetLabelInput( labelImage);
    labelStats->SetInput(inImage);
    labelStats->Update();

    std::cout << "Number of labels in the input label image: " << labelStats->GetNumberOfLabels() << std::endl;
    typedef LabelStatsFilterType::ValidLabelValuesContainerType ValidLabelValuesType;
    typedef LabelStatsFilterType::LabelPixelType                LabelPixelType;
    typedef ImageType::PixelType				OutPixelType;
    // for(ValidLabelValuesType::const_iterator vIt=labelStats->GetValidLabelValues().begin();
    // 	vIt != labelStats->GetValidLabelValues().end();
    // 	++vIt)

    // For writing into a table
    if(writeOutTableFile)
    {
	std::ofstream outputFile(outTableFile.c_str());
	outputFile<<"LabelId\tMeanIntensity\tNumberOfVoxels\n";
	for (LabelsVecItType it = inLabels.begin(); it != inLabels.end(); ++it)
	{
	    if ( labelStats->HasLabel(*it) )
	    {
		LabelPixelType labelValue = *it;
		OutPixelType meanValue = labelStats->GetMean(labelValue);
		unsigned int labelVoxelCount = labelStats->GetCount(labelValue);
		// std::cout <<"For Label Id: "<<labelValue<<std::endl;
		// std::cout << "mean: " << meanValue << std::endl;
		// std::cout << "count: " << labelVoxelCount << std::endl;
		// std::cout << std::endl;
		outputFile<<labelValue<<"\t"<<meanValue<<"\t"<<labelVoxelCount<<'\n';
	    }
	    else
	    {
		std::cout<<" Label Id "<<*it<<" not present in the label image"<<std::endl;
	    }
	}
	outputFile.close();
    }

    if(writeOutImageFile)
    { // For writing an image
        // Output image, initialize with zero.
	ImageType::Pointer outImage;
	if(startFromInImage)
	{
	    typedef itk::ImageDuplicator< ImageType > DuplicatorType;
	    DuplicatorType::Pointer duplicator = DuplicatorType::New();
	    duplicator->SetInputImage(inImage);
	    duplicator->Update();
	    outImage = duplicator->GetModifiableOutput();
	}
	else
	{
	    outImage = ImageType::New();
	    outImage->SetRegions(inImage->GetLargestPossibleRegion());
	    outImage->CopyInformation(inImage);
	    outImage->Allocate();
	    outImage->FillBuffer(0.);
	}

	typedef itk::MaskImageFilter<ImageType, LabelImageType, ImageType> MaskImageFilterType;
	MaskImageFilterType::Pointer maskImageFilter = MaskImageFilterType::New();
	maskImageFilter->SetMaskImage(labelImage);
	for (LabelsVecItType it = inLabels.begin(); it != inLabels.end(); ++it)
	{
	    if ( labelStats->HasLabel(*it) )
	    {
		LabelPixelType labelValue = *it;
		OutPixelType meanValue = labelStats->GetMean(labelValue);
		maskImageFilter->SetInput(outImage);
		//if (pixel_from_mask_image != masking_value)     pixel_output_image = pixel_input_image
		//else pixel_output_image = outside_value
		maskImageFilter->SetMaskingValue(labelValue);
		maskImageFilter->SetOutsideValue(meanValue);
		maskImageFilter->Update();
		outImage = maskImageFilter->GetOutput();
		outImage->DisconnectPipeline();
	    }
	    else
	    {
		std::cout<<" Label Id "<<*it<<" not present in the label image"<<std::endl;
	    }
	}
	// Write the out output.
	typedef itk::ImageFileWriter< ImageType > ValueImageWriterType;
	ValueImageWriterType::Pointer outImageWriter = ValueImageWriterType::New();
	outImageWriter->SetFileName(outImageFile);
	outImageWriter->SetInput(outImage);
	outImageWriter->Update();
    }

    return EXIT_SUCCESS;
}


/*
 *            Copyright 2009-2020 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.
 *
 */

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/bse.h"
#include "votca/xtp/bse_operator.h"
#include "votca/xtp/bsecoupling.h"

namespace votca {
namespace xtp {
using namespace std;
using boost::format;
using namespace tools;

void BSECoupling::Initialize(Property& options) {
  string spintype = options.get("spin").as<std::string>();
  if (spintype == "all") {
    doSinglets_ = true;
    doTriplets_ = true;
  } else if (spintype == "triplet") {
    doTriplets_ = true;
    doSinglets_ = false;
  } else if (spintype == "singlet") {
    doSinglets_ = true;
    doTriplets_ = false;
  } else {
    throw std::runtime_error(
        (boost::format(
             "Choice % for type not known. Available singlet,triplet,all") %
         spintype)
            .str());
  }
  output_perturbation_ = options.get("use_perturbation").as<bool>();

  levA_ = options.get("moleculeA.states").as<Index>();
  levB_ = options.get("moleculeB.states").as<Index>();
  occA_ = options.get("moleculeA.occLevels").as<Index>();
  occB_ = options.get("moleculeB.occLevels").as<Index>();
  unoccA_ = options.get("moleculeA.unoccLevels").as<Index>();
  unoccB_ = options.get("moleculeB.unoccLevels").as<Index>();
}

void BSECoupling::WriteToProperty(Property& summary, const QMState& stateA,
                                  const QMState& stateB) const {
  Property& coupling_summary = summary.add("coupling", "");
  double JAB_pert = 0;
  double JAB_diag = 0;
  if (stateA.Type() == QMStateType::Singlet) {
    JAB_pert =
        getSingletCouplingElement(stateA.StateIdx(), stateB.StateIdx(), 0);
    JAB_diag =
        getSingletCouplingElement(stateA.StateIdx(), stateB.StateIdx(), 1);
  } else if (stateA.Type() == QMStateType::Triplet) {
    JAB_pert =
        getTripletCouplingElement(stateA.StateIdx(), stateB.StateIdx(), 0);
    JAB_diag =
        getTripletCouplingElement(stateA.StateIdx(), stateB.StateIdx(), 1);
  }
  coupling_summary.setAttribute("stateA", stateA.ToString());
  coupling_summary.setAttribute("stateB", stateB.ToString());
  coupling_summary.setAttribute("j_pert", (format("%1$1.6e") % JAB_pert).str());
  coupling_summary.setAttribute("j_diag", (format("%1$1.6e") % JAB_diag).str());
}

void BSECoupling::Addoutput(Property& type_summary, const Orbitals&,
                            const Orbitals&) const {
  tools::Property& bsecoupling = type_summary.add(Identify(), "");
  string algorithm = "j_diag";
  if (output_perturbation_) {
    algorithm = "j_pert";
  }
  if (doSinglets_) {
    QMStateType singlet = QMStateType(QMStateType::Singlet);
    Property& singlet_summary = bsecoupling.add(singlet.ToLongString(), "");
    singlet_summary.setAttribute("algorithm", algorithm);
    for (Index stateA = 0; stateA < levA_; ++stateA) {
      QMState qmstateA = QMState(singlet, stateA, false);
      for (Index stateB = 0; stateB < levB_; ++stateB) {
        QMState qmstateB = QMState(singlet, stateB, false);
        WriteToProperty(singlet_summary, qmstateA, qmstateB);
      }
    }
  }

  if (doTriplets_) {
    QMStateType triplet = QMStateType(QMStateType::Triplet);
    Property& triplet_summary = bsecoupling.add(triplet.ToLongString(), "");
    triplet_summary.setAttribute("algorithm", algorithm);
    for (Index stateA = 0; stateA < levA_; ++stateA) {
      QMState qmstateA = QMState(triplet, stateA, false);
      for (Index stateB = 0; stateB < levB_; ++stateB) {
        QMState qmstateB = QMState(triplet, stateB, false);
        WriteToProperty(triplet_summary, qmstateA, qmstateB);
      }
    }
  }
}

double BSECoupling::getSingletCouplingElement(Index levelA, Index levelB,
                                              Index methodindex) const {
  return JAB_singlet[methodindex](levelA, levelB + levA_) *
         votca::tools::conv::hrt2ev;
}

double BSECoupling::getTripletCouplingElement(Index levelA, Index levelB,
                                              Index methodindex) const {
  return JAB_triplet[methodindex](levelA, levelB + levA_) *
         votca::tools::conv::hrt2ev;
}

Eigen::MatrixXd BSECoupling::SetupCTStates(Index bseA_vtotal, Index bseB_vtotal,
                                           Index bseAB_vtotal,
                                           Index bseAB_ctotal,
                                           const Eigen::MatrixXd& A_AB,
                                           const Eigen::MatrixXd& B_AB) const {

  Index noAB = occA_ * unoccB_;
  Index noBA = unoccA_ * occB_;
  Index bseAB_size = bseAB_vtotal * bseAB_ctotal;
  Eigen::MatrixXd CTstates = Eigen::MatrixXd::Zero(bseAB_size, noAB + noBA);

  auto A_occ = A_AB.middleCols(bseA_vtotal - occA_, occA_);
  auto A_unocc = A_AB.middleCols(bseA_vtotal, unoccA_);
  auto B_occ = B_AB.middleCols(bseB_vtotal - occB_, occB_);
  auto B_unocc = B_AB.middleCols(bseB_vtotal, unoccB_);

  const Eigen::MatrixXd A_occ_occ = A_occ.topRows(bseAB_vtotal);
  const Eigen::MatrixXd B_unocc_unocc = B_unocc.bottomRows(bseAB_ctotal);

  // notation AB is CT states with A+B-, BA is the counterpart
  // Setting up CT-states:
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   Setting up CT-states" << flush;

  // Number of A+B- states
#pragma omp parallel for
  for (Index a_occ = 0; a_occ < occA_; a_occ++) {
    for (Index b_unocc = 0; b_unocc < unoccB_; b_unocc++) {
      Index index = a_occ * unoccB_ + b_unocc;
      Eigen::MatrixXd Coeff =
          B_unocc_unocc.col(b_unocc) * A_occ_occ.col(a_occ).transpose();
      CTstates.col(index) =
          Eigen::Map<Eigen::VectorXd>(Coeff.data(), bseAB_size);
    }
  }
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "  " << noBA << " CT states A+B- created" << flush;

  const Eigen::MatrixXd A_unocc_unocc = A_unocc.bottomRows(bseAB_ctotal);
  const Eigen::MatrixXd B_occ_occ = B_occ.topRows(bseAB_vtotal);

#pragma omp parallel for
  for (Index b_occ = 0; b_occ < occB_; b_occ++) {
    for (Index a_unocc = 0; a_unocc < unoccA_; a_unocc++) {
      Index index = b_occ * unoccA_ + a_unocc + noAB;
      Eigen::MatrixXd Coeff =
          A_unocc_unocc.col(a_unocc) * B_occ_occ.col(b_occ).transpose();
      CTstates.col(index) =
          Eigen::Map<Eigen::VectorXd>(Coeff.data(), bseAB_size);
    }
  }
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "  " << noBA << " CT states A-B+ created" << flush;
  return CTstates;
}

Eigen::MatrixXd BSECoupling::ProjectFrenkelExcitons(
    const Eigen::MatrixXd& BSE_Coeffs, const Eigen::MatrixXd& X_AB,
    Index bseX_vtotal, Index bseX_ctotal, Index bseAB_vtotal,
    Index bseAB_ctotal) const {
  Index bseAB_size = bseAB_vtotal * bseAB_ctotal;
  auto X_occ = X_AB.leftCols(bseX_vtotal);
  auto X_unocc = X_AB.rightCols(bseX_ctotal);
  const Eigen::MatrixXd X_occ_occ = X_occ.topRows(bseAB_vtotal);
  const Eigen::MatrixXd X_unocc_unocc = X_unocc.bottomRows(bseAB_ctotal);
  Eigen::MatrixXd result = Eigen::MatrixXd::Zero(bseAB_size, BSE_Coeffs.cols());
  // no pragma here because often we will only have one Coeff
  for (Index i = 0; i < BSE_Coeffs.cols(); i++) {
    Eigen::VectorXd coeff = BSE_Coeffs.col(i);
    Eigen::Map<Eigen::MatrixXd> coeffmatrix =
        Eigen::Map<Eigen::MatrixXd>(coeff.data(), bseX_ctotal, bseX_vtotal);
    Eigen::MatrixXd proj = X_unocc_unocc * coeffmatrix * X_occ_occ.transpose();
    result.col(i) = Eigen::Map<Eigen::VectorXd>(proj.data(), proj.size());
  }
  return result;
}

int GetSign(double value) {
  if (value < 0) {
    return -1;
  } else if (value > 0) {
    return 1;
  }
  return 0;
}

void BSECoupling::CalculateCouplings(const Orbitals& orbitalsA,
                                     const Orbitals& orbitalsB,
                                     const Orbitals& orbitalsAB) {
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "  Calculating exciton couplings" << flush;
  // set the parallelization
  XTP_LOG(Log::error, *pLog_) << TimeStamp() << " Using "
                              << OPENMP::getMaxThreads() << " threads" << flush;

  CheckAtomCoordinates(orbitalsA, orbitalsB, orbitalsAB);

  // constructing the direct product orbA x orbB
  Index basisB = orbitalsB.getBasisSetSize();
  Index basisA = orbitalsA.getBasisSetSize();
  if ((basisA == 0) || (basisB == 0)) {
    throw std::runtime_error("Basis set size is not stored in monomers");
  }
  // get exciton information of molecule A
  Index bseA_cmax = orbitalsA.getBSEcmax();
  Index bseA_cmin = orbitalsA.getBSEcmin();
  Index bseA_vmax = orbitalsA.getBSEvmax();
  Index bseA_vmin = orbitalsA.getBSEvmin();
  Index bseA_vtotal = bseA_vmax - bseA_vmin + 1;
  Index bseA_ctotal = bseA_cmax - bseA_cmin + 1;
  Index bseA_total = bseA_vtotal + bseA_ctotal;
  Index bseA_size = bseA_vtotal * bseA_ctotal;
  Index bseA_singlet_exc = orbitalsA.BSESinglets().eigenvectors().cols();
  Index bseA_triplet_exc = orbitalsA.BSETriplets().eigenvectors().cols();

  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   molecule A has " << bseA_singlet_exc
      << " singlet excitons with dimension " << bseA_size << flush;
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   molecule A has " << bseA_triplet_exc
      << " triplet excitons with dimension " << bseA_size << flush;

  // get exciton information of molecule B
  Index bseB_cmax = orbitalsB.getBSEcmax();
  Index bseB_cmin = orbitalsB.getBSEcmin();
  Index bseB_vmax = orbitalsB.getBSEvmax();
  Index bseB_vmin = orbitalsB.getBSEvmin();
  Index bseB_vtotal = bseB_vmax - bseB_vmin + 1;
  Index bseB_ctotal = bseB_cmax - bseB_cmin + 1;
  Index bseB_total = bseB_vtotal + bseB_ctotal;
  Index bseB_size = bseB_vtotal * bseB_ctotal;
  Index bseB_singlet_exc = orbitalsB.BSESinglets().eigenvectors().cols();
  Index bseB_triplet_exc = orbitalsB.BSETriplets().eigenvectors().cols();

  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   molecule B has " << bseB_singlet_exc
      << " singlet excitons with dimension " << bseB_size << flush;
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   molecule B has " << bseB_triplet_exc
      << " triplet excitons with dimension " << bseB_size << flush;

  if (doSinglets_) {
    if (levA_ > bseA_singlet_exc) {
      XTP_LOG(Log::error, *pLog_) << TimeStamp()
                                  << "  Number of excitons you want is greater "
                                     "than stored for molecule "
                                     "A. Setting to max number available"
                                  << flush;
      levA_ = bseA_singlet_exc;
    }
    if (levB_ > bseB_singlet_exc) {
      XTP_LOG(Log::error, *pLog_) << TimeStamp()
                                  << "  Number of excitons you want is greater "
                                     "than stored for molecule "
                                     "B. Setting to max number available"
                                  << flush;
      levB_ = bseB_singlet_exc;
    }
  }
  if (doTriplets_) {
    if (levA_ > bseA_triplet_exc) {
      XTP_LOG(Log::error, *pLog_)
          << TimeStamp()
          << "  Number of Frenkel states you want is greater than stored for "
             "molecule A. Setting to max number available"
          << flush;
      levA_ = bseA_triplet_exc;
    }
    if (levB_ > bseB_triplet_exc) {
      XTP_LOG(Log::error, *pLog_)
          << TimeStamp()
          << "  Number of Frenkel states you want is greater than stored for "
             "molecule B. Setting to max number available"
          << flush;
      levB_ = bseB_triplet_exc;
    }
  }
  if (unoccA_ > bseA_ctotal || unoccA_ < 0) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp()
        << "  Number of unoccupied orbitals in molecule A for CT creation "
           "exceeds number of KS-orbitals in BSE"
        << flush;
    unoccA_ = bseA_ctotal;
  }
  if (unoccB_ > bseB_ctotal || unoccB_ < 0) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp()
        << "  Number of unoccupied orbitals in molecule B for CT creation "
           "exceeds number of KS-orbitals in BSE"
        << flush;
    unoccB_ = bseB_ctotal;
  }

  if (occA_ > bseA_vtotal || occA_ < 0) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp()
        << "  Number of occupied orbitals in molecule A for CT creation "
           "exceeds number of KS-orbitals in BSE"
        << flush;
    occA_ = bseA_vtotal;
  }
  if (occB_ > bseB_vtotal || occB_ < 0) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp()
        << "  Number of occupied orbitals in molecule B for CT creation "
           "exceeds number of KS-orbitals in BSE"
        << flush;
    occB_ = bseB_vtotal;
  }

  // get exciton information of pair AB
  Index bseAB_cmax = orbitalsAB.getBSEcmax();
  Index bseAB_cmin = orbitalsAB.getBSEcmin();
  Index bseAB_vmax = orbitalsAB.getBSEvmax();
  Index bseAB_vmin = orbitalsAB.getBSEvmin();
  Index bseAB_vtotal = bseAB_vmax - bseAB_vmin + 1;
  Index bseAB_ctotal = bseAB_cmax - bseAB_cmin + 1;
  Index bseAB_total = bseAB_vtotal + bseAB_ctotal;
  Index bseAB_size = bseAB_vtotal * bseAB_ctotal;

  // DFT levels of monomers can be reduced to those used in BSE
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   levels used for BSE of molA: " << bseA_vmin
      << " to " << bseA_cmax << " total: " << bseA_total << flush;
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   levels used for BSE of molB: " << bseB_vmin
      << " to " << bseB_cmax << " total: " << bseB_total << flush;
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "   levels used for BSE of dimer AB: " << bseAB_vmin
      << " to " << bseAB_cmax << " total: " << bseAB_total << flush;

  Eigen::MatrixXd MOsA =
      orbitalsA.MOs().eigenvectors().middleCols(bseA_vmin, bseA_total);
  Eigen::MatrixXd MOsB =
      orbitalsB.MOs().eigenvectors().middleCols(bseB_vmin, bseB_total);
  Eigen::MatrixXd MOsAB =
      orbitalsAB.MOs().eigenvectors().middleCols(bseAB_vmin, bseAB_total);

  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << " Calculating overlap matrix for basisset: "
      << orbitalsAB.getDFTbasisName() << flush;

  Eigen::MatrixXd overlap = CalculateOverlapMatrix(orbitalsAB) * MOsAB;
  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << " Projecting monomers onto dimer orbitals" << flush;

  Eigen::MatrixXd A_AB = overlap.topRows(basisA).transpose() * MOsA;
  Eigen::MatrixXd B_AB = overlap.bottomRows(basisB).transpose() * MOsB;
  Eigen::VectorXd mag_A = A_AB.colwise().squaredNorm();
  if (mag_A.any() < 0.95) {
    XTP_LOG(Log::error, *pLog_)
        << "\nWarning: "
        << "Projection of orbitals of monomer A on dimer is insufficient,mag="
        << mag_A.minCoeff() << flush;
  }
  Eigen::VectorXd mag_B = B_AB.colwise().squaredNorm();
  if (mag_B.any() < 0.95) {
    XTP_LOG(Log::error, *pLog_)
        << "\nWarning: "
        << "Projection of orbitals of monomer B on dimer is insufficient,mag="
        << mag_B.minCoeff() << flush;
  }

  AOBasis dftbasis = orbitalsAB.getDftBasis();
  AOBasis auxbasis = orbitalsAB.getAuxBasis();
  TCMatrix_gwbse Mmn;
  // rpamin here, because RPA needs till rpamin
  Mmn.Initialize(auxbasis.AOBasisSize(), orbitalsAB.getRPAmin(),
                 orbitalsAB.getGWAmax(), orbitalsAB.getRPAmin(),
                 orbitalsAB.getRPAmax());
  Mmn.Fill(auxbasis, dftbasis, orbitalsAB.MOs().eigenvectors());

  const Eigen::MatrixXd& qpcoeff = orbitalsAB.QPdiag().eigenvectors();
  Eigen::MatrixXd Hqp = qpcoeff *
                        orbitalsAB.QPdiag().eigenvalues().asDiagonal() *
                        qpcoeff.transpose();

  BSE::options opt;
  opt.cmax = orbitalsAB.getBSEcmax();
  opt.homo = orbitalsAB.getHomo();
  opt.qpmin = orbitalsAB.getGWAmin();
  opt.qpmax = orbitalsAB.getGWAmax();
  opt.rpamax = orbitalsAB.getRPAmax();
  opt.rpamin = orbitalsAB.getRPAmin();
  opt.useTDA = true;
  opt.vmin = orbitalsAB.getBSEvmin();
  opt.use_Hqp_offdiag = orbitalsAB.GetFlagUseHqpOffdiag();
  BSE bse(*pLog_, Mmn);
  bse.configure(opt, orbitalsAB.RPAInputEnergies(), Hqp);
  XTP_LOG(Log::error, *pLog_) << TimeStamp() << " Setup BSE operator" << flush;

  // now the different spin types
  if (doSinglets_) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << "   Evaluating singlets" << flush;
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << "   Setup Hamiltonian" << flush;
    Eigen::MatrixXd FE_AB = Eigen::MatrixXd::Zero(bseAB_size, levA_ + levB_);
    const Eigen::MatrixXd bseA =
        orbitalsA.BSESinglets().eigenvectors().leftCols(levA_);
    FE_AB.leftCols(levA_) = ProjectFrenkelExcitons(
        bseA, A_AB, bseA_vtotal, bseA_ctotal, bseAB_vtotal, bseAB_ctotal);
    const Eigen::MatrixXd bseB =
        orbitalsB.BSESinglets().eigenvectors().leftCols(levB_);
    FE_AB.rightCols(levB_) = ProjectFrenkelExcitons(
        bseB, B_AB, bseB_vtotal, bseB_ctotal, bseAB_vtotal, bseAB_ctotal);
    Eigen::MatrixXd CTStates = SetupCTStates(
        bseA_vtotal, bseB_vtotal, bseAB_vtotal, bseAB_ctotal, A_AB, B_AB);
    JAB_singlet =
        ProjectExcitons(FE_AB, CTStates, bse.getSingletOperator_TDA());
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << "   calculated singlet couplings " << flush;
  }
  if (doTriplets_) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << "   Evaluating triplets" << flush;
    Eigen::MatrixXd FE_AB = Eigen::MatrixXd::Zero(bseAB_size, levA_ + levB_);
    const Eigen::MatrixXd bseA =
        orbitalsA.BSETriplets().eigenvectors().leftCols(levA_);
    FE_AB.leftCols(levA_) = ProjectFrenkelExcitons(
        bseA, A_AB, bseA_vtotal, bseA_ctotal, bseAB_vtotal, bseAB_ctotal);
    const Eigen::MatrixXd bseB =
        orbitalsB.BSETriplets().eigenvectors().leftCols(levB_);
    FE_AB.rightCols(levB_) = ProjectFrenkelExcitons(
        bseB, B_AB, bseB_vtotal, bseB_ctotal, bseAB_vtotal, bseAB_ctotal);
    Eigen::MatrixXd CTStates = SetupCTStates(
        bseA_vtotal, bseB_vtotal, bseAB_vtotal, bseAB_ctotal, A_AB, B_AB);
    JAB_triplet =
        ProjectExcitons(FE_AB, CTStates, bse.getTripletOperator_TDA());
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << "   calculated triplet couplings " << flush;
  }
  XTP_LOG(Log::error, *pLog_)
      << TimeStamp() << "  Done with exciton couplings" << flush;
}

Eigen::MatrixXd BSECoupling::OrthogonalizeCTs(Eigen::MatrixXd& FE_AB,
                                              Eigen::MatrixXd& CTStates) const {
  Index ct = CTStates.cols();

  if (ct > 0) {
    XTP_LOG(Log::error, *pLog_)
        << TimeStamp() << " Orthogonalizing CT-states with respect to FE-states"
        << flush;
    Eigen::MatrixXd correction = FE_AB * (FE_AB.transpose() * CTStates);
    CTStates -= correction;

    // normalize
    Eigen::VectorXd norm = CTStates.colwise().norm();
    for (Index i = 0; i < CTStates.cols(); i++) {
      CTStates.col(i) /= norm(i);
    }
    Index minstateindex = 0;
    double minnorm = norm.minCoeff(&minstateindex);
    if (minnorm < 0.95) {
      XTP_LOG(Log::error, *pLog_)
          << TimeStamp() << " WARNING: CT-state " << minstateindex
          << " norm is only " << minnorm << flush;
    }
  }
  Index bse_exc = levA_ + levB_;

  Index bseAB_size = CTStates.rows();
  Eigen::MatrixXd projection(bseAB_size, bse_exc + ct);
  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << " merging projections into one vector  " << flush;
  projection.leftCols(bse_exc) = FE_AB;
  FE_AB.resize(0, 0);
  if (ct > 0) {
    projection.rightCols(ct) = CTStates;
  }
  CTStates.resize(0, 0);
  return projection;
}

template <class BSE_OPERATOR>
Eigen::MatrixXd BSECoupling::CalcJ_dimer(BSE_OPERATOR& H,
                                         Eigen::MatrixXd& projection) const {

  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << "   Setting up coupling matrix size "
      << projection.cols() << flush;
  // matrix  J_
  //  E_A         J_AB        J_A_ABCT        J_A_BACT
  //  J_BA        E_B         J_B_ABCT        J_B_BACT
  //  J_ABCT_A    J_ABCT_B    E_ABCT          J_ABCT_BACT
  //  J_BACT_A   J_BACT_B    J_BACT_ABCT     E_BACT

  // this only works for hermitian/symmetric H so only in TDA

  Eigen::MatrixXd J_dimer = projection.transpose() * (H * projection).eval();

  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << "   Setting up overlap matrix size "
      << projection.cols() << flush;
  Eigen::MatrixXd S_dimer = projection.transpose() * projection;

  projection.resize(0, 0);
  if (projection.cols()) {
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
    XTP_LOG(Log::debug, *pLog_) << " J_dimer_[Ryd]" << flush;

    XTP_LOG(Log::debug, *pLog_) << J_dimer << flush;
    XTP_LOG(Log::debug, *pLog_) << " S_dimer_" << flush;

    XTP_LOG(Log::debug, *pLog_) << S_dimer << flush;
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
  }

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(S_dimer);
  Eigen::MatrixXd Sm1 = es.operatorInverseSqrt();
  Eigen::MatrixXd J_ortho = Sm1 * J_dimer * Sm1;

  if (projection.cols()) {
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
    XTP_LOG(Log::debug, *pLog_) << " J_ortho_[Ryd]" << flush;
    XTP_LOG(Log::debug, *pLog_) << J_ortho << flush;
    XTP_LOG(Log::debug, *pLog_) << " S_-1/2" << flush;
    XTP_LOG(Log::debug, *pLog_) << Sm1 << flush;
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
  }
  XTP_LOG(Log::debug, *pLog_)
      << TimeStamp() << "   Smallest value of dimer overlapmatrix is "
      << es.eigenvalues()(0) << flush;
  return J_ortho;
}

template <class BSE_OPERATOR>
std::array<Eigen::MatrixXd, 2> BSECoupling::ProjectExcitons(
    Eigen::MatrixXd& FE_AB, Eigen::MatrixXd& CTStates, BSE_OPERATOR H) const {

  Eigen::MatrixXd projection = OrthogonalizeCTs(FE_AB, CTStates);
  Eigen::MatrixXd J_ortho = CalcJ_dimer(H, projection);

  std::array<Eigen::MatrixXd, 2> J;

  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << "   Running Perturbation algorithm" << flush;
  J[0] = Perturbation(J_ortho);
  XTP_LOG(Log::info, *pLog_)
      << TimeStamp() << "    Running Projection algorithm" << flush;
  J[1] = Fulldiag(J_ortho);

  XTP_LOG(Log::debug, *pLog_)
      << "---------------------------------------" << flush;
  XTP_LOG(Log::debug, *pLog_) << "Jeff_pert[Hrt]" << flush;
  XTP_LOG(Log::debug, *pLog_) << J[0] << flush;
  XTP_LOG(Log::debug, *pLog_) << "Jeff_diag[Hrt]" << flush;
  XTP_LOG(Log::debug, *pLog_) << J[1] << flush;
  XTP_LOG(Log::debug, *pLog_)
      << "---------------------------------------" << flush;

  return J;
}

Eigen::MatrixXd BSECoupling::Perturbation(
    const Eigen::MatrixXd& J_dimer) const {
  Index bse_exc = levA_ + levB_;
  Index ct = J_dimer.rows() - bse_exc;
  Eigen::MatrixXd J_result = J_dimer;
  if (ct > 0) {
    Eigen::MatrixXd transformation =
        Eigen::MatrixXd::Identity(J_dimer.rows(), J_dimer.cols());
    Eigen::MatrixXd Ct = J_dimer.bottomRightCorner(ct, ct);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Ct);
    transformation.bottomRightCorner(ct, ct) = es.eigenvectors();
    Ct.resize(0, 0);

    XTP_LOG(Log::debug, *pLog_) << "FE state hamiltonian" << flush;
    XTP_LOG(Log::debug, *pLog_)
        << J_dimer.topLeftCorner(bse_exc, bse_exc) << flush;
    if (ct > 0) {
      XTP_LOG(Log::debug, *pLog_) << "eigenvalues of CT states" << flush;
      XTP_LOG(Log::debug, *pLog_) << es.eigenvalues().transpose() << flush;
    }

    J_result = transformation.transpose() * J_dimer * transformation;
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
    XTP_LOG(Log::debug, *pLog_) << " J_ortho_[Hrt] CT-state diag" << flush;
    XTP_LOG(Log::debug, *pLog_) << J_result << flush;
    XTP_LOG(Log::debug, *pLog_)
        << "---------------------------------------" << flush;
  }

  Eigen::MatrixXd Jmatrix = Eigen::MatrixXd::Zero(bse_exc, bse_exc);
  for (Index stateA = 0; stateA < levA_; stateA++) {
    double Ea = J_result(stateA, stateA);
    for (Index stateB = 0; stateB < levB_; stateB++) {
      Index stateBd = stateB + levA_;
      XTP_LOG(Log::info, *pLog_)
          << TimeStamp() << "   Calculating coupling between exciton A"
          << stateA + 1 << " and exciton B" << stateB + 1 << flush;
      double J = J_result(stateA, stateBd);

      double Eb = J_result(stateBd, stateBd);
      for (Index k = bse_exc; k < (bse_exc + ct); k++) {
        double Eab = J_result(k, k);
        if (std::abs(Eab - Ea) < 0.001) {
          XTP_LOG(Log::error, *pLog_)
              << TimeStamp() << "Energydifference between state A "
              << stateA + 1 << "and CT state " << k + 1 << " is " << Eab - Ea
              << "[Hrt]" << flush;
        }
        if (std::abs(Eab - Eb) < 0.001) {
          XTP_LOG(Log::error, *pLog_)
              << TimeStamp() << "Energydifference between state B "
              << stateB + 1 << "and CT state " << k + 1 << " is " << Eab - Eb
              << "[Hrt]" << flush;
        }
        J += 0.5 * J_result(k, stateA) * J_result(k, stateBd) *
             (1 / (Ea - Eab) + 1 / (Eb - Eab));  // Have no clue why 0.5
      }
      Jmatrix(stateA, stateBd) = J;
      Jmatrix(stateBd, stateA) = J;
    }
  }
  return Jmatrix;
}

Eigen::MatrixXd BSECoupling::Fulldiag(const Eigen::MatrixXd& J_dimer) const {
  Index bse_exc = levA_ + levB_;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(J_dimer);
  XTP_LOG(Log::debug, *pLog_)
      << "---------------------------------------" << flush;
  XTP_LOG(Log::debug, *pLog_) << "Eigenvectors of J" << flush;
  XTP_LOG(Log::debug, *pLog_) << es.eigenvectors() << flush;
  XTP_LOG(Log::debug, *pLog_) << "J_eigenvalues[Hrt]" << flush;
  XTP_LOG(Log::debug, *pLog_) << es.eigenvalues() << flush;
  XTP_LOG(Log::debug, *pLog_)
      << "---------------------------------------" << flush;
  Eigen::MatrixXd Jmat = Eigen::MatrixXd::Zero(bse_exc, bse_exc);
  // Calculate projection on subspace for every pair of excitons separately
  for (Index stateA = 0; stateA < levA_; stateA++) {
    for (Index stateB = 0; stateB < levB_; stateB++) {

      Index stateBd = stateB + levA_;
      XTP_LOG(Log::info, *pLog_)
          << TimeStamp() << "   Calculating coupling between exciton A"
          << stateA + 1 << " and exciton B" << stateB + 1 << flush;

      std::array<int, 2> indexes;
      std::array<int, 2> signs;

      // Find the eigenstate state, which in an L2 is closed to A or B
      // respectively
      es.eigenvectors().row(stateA).cwiseAbs().maxCoeff(&indexes[0]);
      es.eigenvectors().row(stateBd).cwiseAbs().maxCoeff(&indexes[1]);
      if (indexes[0] == indexes[1]) {
        Eigen::RowVectorXd stateamplitudes =
            es.eigenvectors().row(stateBd).cwiseAbs();
        stateamplitudes[indexes[1]] = 0.0;
        stateamplitudes.maxCoeff(&indexes[1]);
      }

      signs[0] = GetSign(es.eigenvectors()(stateA, indexes[0]));
      signs[1] = GetSign(es.eigenvectors()(stateBd, indexes[1]));

      XTP_LOG(Log::info, *pLog_)
          << TimeStamp() << "   Order is: [Initial state n->nth eigenvalue]"
          << flush;
      XTP_LOG(Log::info, *pLog_) << "    A" << stateA + 1 << ":" << stateA + 1
                                 << "->" << indexes[0] + 1 << " ";
      XTP_LOG(Log::info, *pLog_) << "    B" << stateB + 1 << ":" << stateBd + 1
                                 << "->" << indexes[1] + 1 << " " << flush;

      // setting up transformation matrix Tmat and diagonal matrix Emat for the
      // eigenvalues;
      Eigen::Matrix2d Emat = Eigen::Matrix2d::Zero();
      Eigen::Matrix2d Tmat = Eigen::Matrix2d::Zero();
      // find the eigenvectors which are most similar to the initial states
      // row
      for (Index i = 0; i < 2; i++) {
        Index k = indexes[i];
        double sign = signs[i];
        Tmat(0, i) = sign * es.eigenvectors()(stateA, k);
        Tmat(1, i) = sign * es.eigenvectors()(stateBd, k);
        Emat(i, i) = es.eigenvalues()(k);
      }
      Tmat.colwise().normalize();

      if (Tmat.determinant() < 0) {
        XTP_LOG(Log::info, *pLog_)
            << " Reduced state matrix is not in a right handed basis, "
               "multiplying second eigenvector by -1 "
            << flush;
        Tmat.col(1) *= -1;
      }

      XTP_LOG(Log::debug, *pLog_)
          << "---------------------------------------" << flush;
      XTP_LOG(Log::debug, *pLog_) << " T_" << flush;
      XTP_LOG(Log::debug, *pLog_) << Tmat << flush;

      Eigen::Matrix2d S_small = Tmat * Tmat.transpose();

      XTP_LOG(Log::debug, *pLog_) << "S_small" << flush;
      XTP_LOG(Log::debug, *pLog_) << S_small << flush;
      // orthogonalize that matrix

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> ss(S_small);
      Eigen::Matrix2d sm1 = ss.operatorInverseSqrt();
      Emat = sm1 * Emat * sm1;

      XTP_LOG(Log::info, *pLog_)
          << TimeStamp() << "   Smallest value of dimer overlapmatrix is "
          << ss.eigenvalues()(0) << flush;

      XTP_LOG(Log::debug, *pLog_) << "S-1/2" << flush;
      XTP_LOG(Log::debug, *pLog_) << sm1 << flush;
      XTP_LOG(Log::debug, *pLog_) << "E_ortho" << flush;
      XTP_LOG(Log::debug, *pLog_) << Emat << flush;

      Tmat = Tmat * sm1;

      XTP_LOG(Log::debug, *pLog_) << "T_ortho" << flush;
      XTP_LOG(Log::debug, *pLog_) << Tmat << flush;
      XTP_LOG(Log::debug, *pLog_)
          << "---------------------------------------" << flush;

      Eigen::Matrix2d J_small = Tmat * Emat * Tmat.transpose();
      XTP_LOG(Log::debug, *pLog_) << "T_ortho*E_ortho*T_ortho^T" << flush;
      XTP_LOG(Log::debug, *pLog_) << J_small << flush;

      Jmat(stateA, stateBd) = J_small(0, 1);
      Jmat(stateBd, stateA) = J_small(1, 0);
    }
  }

  return Jmat;
}

}  // namespace xtp
}  // namespace votca


// Copyright (c) Sergiu Dotenco 2010
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

/**
 * @brief Antithetic variates.
 * @file antithetic.hpp
 */

#ifndef ANTITHETIC_HPP
#define ANTITHETIC_HPP

#include <boost/call_traits.hpp>
#include <boost/operators.hpp>
#include <boost/type_traits/remove_reference.hpp>

/**
 * @brief Antithetic variate used for variance reduction.
 *
 * @tparam UniformRandomNumberGenerator The engine that generates uniformly
 *         distributed random numbers.
 */
template<class UniformRandomNumberGenerator>
class Antithetic
    : boost::equality_comparable
      <
          Antithetic<UniformRandomNumberGenerator>,
          Antithetic<UniformRandomNumberGenerator>
      >
{
    bool toggle_;
    UniformRandomNumberGenerator generator_;

    typedef typename
        boost::remove_reference<UniformRandomNumberGenerator>::type Engine;

public:
    typedef Engine::result_type result_type;

private:
    result_type value_;

public:
    Antithetic(typename
               boost::call_traits<UniformRandomNumberGenerator>::param_type u)
        : toggle_(false)
        , generator_(u)
    {
    }

    result_type operator()()
    {
        if (!toggle_) {
            value_ = generator_();
            toggle_ = true;
        }
        else {
            value_ = max() - value_ + min();
            toggle_ = false;
        }

        return value_;
    }

    result_type min() const
    {
        return generator_.min();
    }

    result_type max() const
    {
        return generator_.max();
    }

    bool operator==(const Antithetic& other) const
    {
        return generator_ == generator_ && toggle_ == toggle_ &&
            value_ == value_;
    }
};

/**
 * Returns an antithetic variate for the specified uniform random number
 * generator @a u.
 *
 * @tparam UniformRandomNumberGenerator The engine that generates uniformly
 *         distributed random numbers.
 */
template<class UniformRandomNumberGenerator>
inline Antithetic<UniformRandomNumberGenerator>
    make_antithetic(UniformRandomNumberGenerator& u)
{
    return Antithetic<UniformRandomNumberGenerator>(u);
}

#endif // ANTITHETIC_HPP


/*
 *  statetree.cpp
 *
 *   Created on: 28-sep-2015
 *       Author: M. El-Kebir
 */

#include "statetree.h"

#include <lemon/connectivity.h>
#include <stdio.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <iomanip>

StateTree::StateTree()
  : _k(0)
  , _S()
  , _root(lemon::INVALID)
  , _label(_S)
  , _stateToNode(_k, lemon::INVALID)
  , _nodeToState(_S)
  , _D(_S)
  , _cnState(_S)
{
}

StateTree::StateTree(int k)
  : _k(k)
  , _S()
  , _root(lemon::INVALID)
  , _label(_S)
  , _stateToNode(_k, lemon::INVALID)
  , _nodeToState(_S)
  , _D(_S)
  , _cnState(_S)
{
  IntVector pi(_k, -1);
  for (int i = 1; i < _k; ++i)
  {
    pi[i] = i - 1;
  }

  init(pi);
}

void StateTree::writeSummary(const ReadMatrix& R,
                             const StateTreeVector& sol,
                             std::ostream& out)
{
  const int n = R.getNrCharacters();
  const int m = R.getNrSamples();

  out << "SNV_index\t"
      << "SNV_label\t"
      << "sample_index\t"
      << "sample_label\t"
      << "ref\t"
      << "alt\t"
      << "VAF\t"
      << "sanger_n_mut\t"
      << "sanger_CCF\t"
      << "decifer_CCF\t"
      << "decifer_DCF\t"
      << "decifer_cluster_CCF\t"
      << "decifer_cluster_DCF\t"
//      << "decifer_cluster_index\t"
//      << "decifer_state_tree_index\t"
      << "nr_nonzero_z"
      << std::endl;

  for (int i = 0; i < n; ++i)
  {
    for (int p = 0; p < m; ++p)
    {
      int ref = R.getRef(p, i);
      int var = R.getVar(p, i);
      double vaf = R.getVAF(p, i);
      double purity = 1;
      auto sanger = R.computeSangerCCF(ref, var,
                                       purity, R.getCopyNumberStates(p, i));
      int sanger_n_mut = sanger.first;
      double sangerCCF = sanger.second;

      IntSet zSet;
      IntPairSet mutTumorSet;
      IntPairSet nonMutTumorSet;
      const StateTree& T_i = sol[i];
      for (int j = 0; j != T_i.numVertices(); ++j)
      {
        if (T_i.isPresent(j))
        {
          int x_j = T_i.x(j);
          int y_j = T_i.y(j);
          int z_j = T_i.z(j);
          if (z_j > 0)
          {
            zSet.insert(z_j);
          }
          if (x_j != 1 || y_j != 1)
          {
            if (z_j > 0)
            {
              mutTumorSet.insert(IntPair(x_j, y_j));
            }
            else
            {
              nonMutTumorSet.insert(IntPair(x_j, y_j));
            }
          }
        }
      }

      double deciferCCF = T_i.maxLikelihoodCCF(p, vaf);
      double deciferDCF = T_i.maxLikelihoodDCF(p, vaf);
      double deciferClusterDCF = T_i.dcf(p);
      double deciferClusterCCF = T_i.cf(p);

      out << i << "\t" << R.indexToCharacter(i) << "\t"
          << p << "\t" << R.indexToSample(p) << "\t"
          << ref << "\t" << var << "\t"
          << R.getVAF(p, i) << "\t"
          << sanger_n_mut << "\t" << sangerCCF << "\t"
          << deciferCCF << "\t" << deciferDCF << "\t"
          << deciferClusterCCF << "\t" << deciferClusterDCF << "\t"
          << zSet.size()
          << std::endl;
    }
  }
}

Node StateTree::computeMaxLikelihoodStateProportions(DoubleNodeMap& s,
                                                     int p,
                                                     double vaf) const
{
  // 1. identify mutation vertex
  int star = -1;
  int x_star = -1;
  int y_star = -1;
  double mu_star = 0;
  Node v_star = lemon::INVALID;

  for (NodeIt v_j(_S); v_j != lemon::INVALID; ++v_j)
  {
    const int j = _nodeToState[v_j];
    if (isPresent(j))
    {
      int i = parent(j);
      if (i == -1 || !isPresent(i)) continue;
      if (z(i) == 0 && z(j) == 1)
      {
        star = j;
        x_star = x(i);
        y_star = y(i);
        v_star = v_j;
        mu_star = _cnState[_stateToNode[i]]._s[p] + _cnState[v_j]._s[p];
        break;
      }
    }
  }

  assert(star != -1);

  double C = mu_star * (x_star + y_star);
  double D = 0;
  for (NodeIt v_j(_S); v_j != lemon::INVALID; ++v_j)
  {
    const int j = _nodeToState[v_j];
    if (isPresent(j) && (x(j) != x_star || y(j) != y_star))
    {
      C += _cnState[v_j]._s[p] * (x(j) + y(j));
      D += _cnState[v_j]._s[p] * z(j);
    }
  }

  for (NodeIt v_j(_S); v_j != lemon::INVALID; ++v_j)
  {
    const int j = _nodeToState[v_j];
    if (x_star == x(j) && y_star == y(j))
    {
      if (j == star)
      {
        s[v_j] = std::min(mu_star, vaf * C - D);
      }
      else
      {
        s[v_j] = std::max(0., mu_star - vaf * C + D);
      }
    }
    else
    {
      s[v_j] = _cnState[v_j]._s[p];
    }
  }

  return v_star;
}

double StateTree::maxLikelihoodCCF(int p,
                                   double vaf) const
{
  DoubleNodeMap s(_S, 0.);
  computeMaxLikelihoodStateProportions(s, p, vaf);

  double ccf = 0;
  for (NodeIt v_j(_S); v_j != lemon::INVALID; ++v_j)
  {
    const int j = _nodeToState[v_j];
    if (isPresent(j) && z(j) > 0)
    {
      ccf += s[v_j];
    }
  }

  return ccf;
}

double StateTree::maxLikelihoodDCF(int p,
                                   double vaf) const
{
  DoubleNodeMap s(_S, 0.);
  Node v_star = computeMaxLikelihoodStateProportions(s, p, vaf);
  int star = _nodeToState[v_star];

  double dcf = 0;
  for (NodeIt v_j(_S); v_j != lemon::INVALID; ++v_j)
  {
    const int j = _nodeToState[v_j];
//    if (isPresent(j))
//      std::cout << "s_{" << x(j) << "," << y(j) << "," << z(j) << "} = " << s[v_j] << " " << _cnState[v_j]._s[p] << std::endl;
    if (isPresent(j) && isDescendant(j, star))
    {
      dcf += s[v_j];
    }
  }

  return dcf;
}

void StateTree::init(const IntVector& pi)
{
  // state 0 should be the root
  assert(pi[0] == -1);
  char buf[1024];

  _root = _S.addNode();
  _stateToNode[0] = _root;
  _nodeToState[_root] = 0;
  snprintf(buf, 1024, "%d", 0);
  _label[_root] = buf;

  // init nodes of S_c
  for (int i = 1; i < _k; ++i)
  {
//    if (0 <= pi[i] && pi[i] < _k)
//    if (pi[i] != -2)
    {
      Node v_i = _S.addNode();
      _stateToNode[i] = v_i;
      _nodeToState[v_i] = i;
      snprintf(buf, 1024, "%d", i);
      _label[v_i] = buf;
    }
  }

  // init edges of S_c
  for (int i = 1; i < _k; ++i)
  {
    int pi_i = pi[i];

    //assert(0 <= pi_i < _k);
    if (0 <= pi_i && pi_i < _k)
    {
      _S.addArc(_stateToNode[pi_i], _stateToNode[i]);
    }
  }

  initD(_root);

  assert(lemon::dag(_S));
}

StateTree::StateTree(const IntVector& pi)
  : _k(pi.size())
  , _S()
  , _root(lemon::INVALID)
  , _label(_S, "")
  , _stateToNode(_k, lemon::INVALID)
  , _nodeToState(_S)
  , _D(_S)
  , _cnState(_S)
{
  init(pi);
}

StateTree::StateTree(const StateTree& other)
  : _k(other._k)
  , _S()
  , _root(lemon::INVALID)
  , _label(_S)
  , _stateToNode(_k, lemon::INVALID)
  , _nodeToState(_S)
  , _D(_S)
  , _cnState(_S)
{
  lemon::digraphCopy(other._S, _S)
    .node(other._root, _root)
    .nodeMap(other._nodeToState, _nodeToState)
    .nodeMap(other._D, _D)
    .nodeMap(other._label, _label)
    .nodeMap(other._cnState, _cnState)
    .run();

  for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
  {
    _stateToNode[_nodeToState[v_i]] = v_i;
  }
}

StateTree& StateTree::operator=(const StateTree& other)
{
  if (this != &other)
  {
    _k = other._k;
    _stateToNode = NodeVector(_k, lemon::INVALID);

    lemon::digraphCopy(other._S, _S)
      .node(other._root, _root)
      .nodeMap(other._nodeToState, _nodeToState)
      .nodeMap(other._label, _label)
      .nodeMap(other._D, _D)
      .nodeMap(other._cnState, _cnState)
      .run();

    for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
    {
      _stateToNode[_nodeToState[v_i]] = v_i;
    }
  }
  return *this;
}

void StateTree::writeEdgeList(std::ostream& out) const
{
  bool first = true;
  for (ArcIt a_ij(_S); a_ij != lemon::INVALID; ++a_ij)
  {
    if (first)
    {
      first = false;
    }
    else
    {
      out << " ; ";
    }

    Node v_i = _S.source(a_ij);
    Node v_j = _S.target(a_ij);

    out << _label[v_i] << " -> " << _label[v_j];
  }
}

void StateTree::writeDOT(const int m,
                         std::ostream& out) const
{
  out << "digraph S {" << std::endl;
  out << "\tlabel=\"VAF =";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << vaf(p);
  }
  out << "\\nCF =";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << cf(p);
  }
  out << "\\nDCF =";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << dcf(p);
  }
  out << "\"" << std::endl;

  for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
  {
    int i = _nodeToState[v_i];
    if (!isPresent(i)) continue;

    out << "\t" << i << " [label=\"("
        << _cnState[v_i]._x << ","
        << _cnState[v_i]._y << ","
        << _cnState[v_i]._z << ")";
    for (double s : _cnState[v_i]._s)
    {
      out << std::setprecision(2) << "\\n" << s;
    }
    out << "\"]" << std::endl;
  }

  for (ArcIt a_ij(_S); a_ij != lemon::INVALID; ++a_ij)
  {
    int i = _nodeToState[_S.source(a_ij)];
    int j = _nodeToState[_S.target(a_ij)];

    out << "\t" << i << " -> " << j << std::endl;
  }

  out << "}" << std::endl;
}

void StateTree::writeDOT(std::ostream& out) const
{
  out << "digraph S {" << std::endl;

  for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
  {
    int i = _nodeToState[v_i];
    if (!isPresent(i)) continue;

    out << "\t" << i << " [label=\"" << _label[v_i];
    for (double s : _cnState[v_i]._s)
    {
      out << std::setprecision(2) << "\\n" << s;
    }
    out << "\"]" << std::endl;
  }

  for (ArcIt a_ij(_S); a_ij != lemon::INVALID; ++a_ij)
  {
    int i = _nodeToState[_S.source(a_ij)];
    int j = _nodeToState[_S.target(a_ij)];

    out << "\t" << i << " -> " << j << std::endl;
  }

  out << "}" << std::endl;
}

void StateTree::initD(Node v_i)
{
  IntSet& D_i = _D[v_i];
  D_i.clear();

  D_i.insert(_nodeToState[v_i]);

  for (OutArcIt a(_S, v_i); a != lemon::INVALID; ++a)
  {
    Node v_j = _S.target(a);

    initD(v_j);
    D_i.insert(_D[v_j].begin(), _D[v_j].end());
  }
}

void StateTree::sampleMixtureProportions(double f, double purity)
{
  const int mutState = mutationState();
  const IntSet& D_mutState = D(mutState);
  std::gamma_distribution<> gamma_dist(1, 1);

  NodeSet postMutNodes;
  NodeSet preMutNodes;
  double sum_pre = 0;
  double sum_post = 0;
  for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
  {
    const int i = _nodeToState[v_i];
    if (!isPresent(i)) continue;

    _cnState[v_i]._s.push_back(gamma_dist(g_rng));
    if (D_mutState.count(i) == 1)
    {
      postMutNodes.insert(v_i);
      sum_post += _cnState[v_i]._s.back();
    }
    else
    {
      preMutNodes.insert(v_i);
      sum_pre += _cnState[v_i]._s.back();
    }
  }

  // zero out nodes with small weights
  for (Node v_i : postMutNodes)
  {
    if ((_cnState[v_i]._s.back() / sum_post) < 0.05)
    {
      sum_post -= _cnState[v_i]._s.back();
      _cnState[v_i]._s.back() = 0;
    }
  }
  for (Node v_i : preMutNodes)
  {
    if ((_cnState[v_i]._s.back() / sum_pre) < 0.05)
    {
      sum_pre -= _cnState[v_i]._s.back();
      _cnState[v_i]._s.back() = 0;
    }
  }

  for (Node v_i : postMutNodes)
  {
    _cnState[v_i]._s.back() /= sum_post;
    _cnState[v_i]._s.back() *= f;
    if (!g_tol.nonZero(_cnState[v_i]._s.back()))
    {
      _cnState[v_i]._s.back() = 0;
    }
  }

  for (Node v_i : preMutNodes)
  {
    _cnState[v_i]._s.back() /= sum_pre;
    _cnState[v_i]._s.back() *= (1 - f - (1 - purity));
    if (!g_tol.nonZero(_cnState[v_i]._s.back()))
    {
      _cnState[v_i]._s.back() = 0;
    }
  }

  _cnState[_root]._s.back() += 1 - purity;
  if (!g_tol.nonZero(_cnState[_root]._s.back()))
  {
    _cnState[_root]._s.back() = 0;
  }
}

void StateTree::writeClusterDOT(double gamma,
                                int t,
                                std::ostream& out) const
{
  const int m = getNrSamples();

  out << "\tsubgraph cluster_" << t << " {" << std::endl;
  out << "\t\tlabel=\"gamma = " << gamma;

  out << "\\nVAF = ";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << vaf(p);
  }
  out << "\\nCF =";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << cf(p);
  }
  out << "\\nDCF =";
  for (int p = 0; p < m; ++p)
  {
    out << std::setprecision(2) << " " << dcf(p);
  }
  out << "\"" << std::endl;

  for (NodeIt v_i(_S); v_i != lemon::INVALID; ++v_i)
  {
    int i = _nodeToState[v_i];
    if (!isPresent(i)) continue;

    out << "\t\t" << i + t * 100 << " [label=\"("
        << _cnState[v_i]._x << ","
        << _cnState[v_i]._y << ","
        << _cnState[v_i]._z << ")";
    for (double s : _cnState[v_i]._s)
    {
      out << std::setprecision(2) << "\\n" << s;
    }
    out << "\"]" << std::endl;
  }

  for (ArcIt a_ij(_S); a_ij != lemon::INVALID; ++a_ij)
  {
    int i = _nodeToState[_S.source(a_ij)];
    int j = _nodeToState[_S.target(a_ij)];

    out << "\t\t" << i + t * 100 << " -> " << j + t * 100 << std::endl;
  }

  out << "\t}" << std::endl;
}

std::ostream& operator<<(std::ostream& out, const StateTree& S)
{
  // output pi
  out << -1;
  for (int i = 1; i < S.k(); ++i)
  {
    out << " " << S.parent(i);
  }
  out << std::endl;

  // output vertices
  for (int i = 0; i < S.k(); ++i)
  {
    Node v_i = S.node(i);
    out << S._cnState[v_i]._x
        << " " << S._cnState[v_i]._y
        << " " << S._cnState[v_i]._z;
    for (double s : S._cnState[v_i]._s)
    {
      out << " " << s;
    }
    out << std::endl;
  }

  // output labels
  out << S.label(0);
  for (int i = 1; i < S.k(); ++i)
  {
    out << " " << S.label(i);
  }
  out << std::endl;

  return out;
}

std::istream& operator>>(std::istream& in, StateTree& S)
{
  typedef std::vector<std::string> StringVector;

  std::string line;
  getline(in, line);
  std::stringstream ss(line);

  StringVector s;
  boost::split(s, line, boost::is_any_of(" \t"));

  IntVector pi(s.size(), -1);
  for (int i = 0; i < s.size(); ++i)
  {
    ss >> pi[i];
  }

  S = StateTree(pi);

  for (int i = 0; i < S.k(); ++i)
  {
    getline(in, line);
    ss.clear();
    ss.str(line);

    Node v_i = S.node(i);
    StateTree::CopyNumberState& cnState_i = S._cnState[v_i];

    boost::split(s, line, boost::is_any_of(" "));

    cnState_i._x = boost::lexical_cast<int>(s[0]);
    cnState_i._y = boost::lexical_cast<int>(s[1]);
    cnState_i._z = boost::lexical_cast<int>(s[2]);

    for (int j = 3; j < s.size(); ++j)
    {
      cnState_i._s.push_back(boost::lexical_cast<double>(s[j]));
    }
  }

  getline(in, line);
  ss.clear();
  ss.str(line);
  for (int i = 0; i < S.k(); ++i)
  {
    ss >> S._label[S._stateToNode[i]];
  }

  return in;
}

std::ostream& operator<<(std::ostream& out, const StateTreeVector& vecS)
{
  out << vecS.size() << " #state trees" << std::endl;
  for (const StateTree& S : vecS)
  {
    out << S;
  }

  return out;
}

std::istream& operator>>(std::istream& in, StateTreeVector& vecS)
{
  g_lineNumber = 0;

  std::string line;
  getline(in, line);
  std::stringstream ss(line);

  int nrStateTrees = -1;
  ss >> nrStateTrees;

  if (nrStateTrees < 1)
  {
    throw std::runtime_error(getLineNumber() + "Error: incorrect number of state trees");
  }

  for (int i = 0; i < nrStateTrees; ++i)
  {
    StateTree S;
    in >> S;
    vecS.push_back(S);
  }

  return in;
}


#include "datadeck.hh"

#include <armadillo>

#include <iostream>

/**
 * @brief Read table & store table's data
 */
Datadeck::Datadeck(const char *file_name) {
    char line_clear[CHARL];
    char temp[CHARN];   // buffer for table data
    std::string table_deck_title;
    int table_count(0);
    double value(0);
    int var_dim[3]={1, 1, 1};
    int tt(0);
    Table *table;
    // opening aero-deck file stream
    std::ifstream tbl_stream(file_name);

    if (tbl_stream.fail()) {
        std::cerr << "*** Error: File stream '" << file_name << "' failed to open (check spelling) ***\n";
        exit(1);
    }

    // determing the total # of tbl_stream
    while (!tbl_stream.eof()) {
        tbl_stream >> temp;
        if (!strcmp(temp, "TITLE")) {
            tbl_stream.getline(line_clear, CHARL, '\n');
            table_deck_title = line_clear;
        }
        if (strstr(temp, "DIM"))
            table_count++;
    }  // EOF reached of aero_deck

    // removing EOF bit (-1)
    tbl_stream.clear();
    // rewinding to beginning
    tbl_stream.seekg(std::ios::beg);

    // creating pointer array table
    this->set_title(table_deck_title);
    this->set_capacity(table_count);
    this->alloc_mem();

    // discarding all entries until first DIM
    do {
        tbl_stream >> temp;
    }while (!strstr(temp, "DIM"));

    // loading tables one at a time
    for (int t=0; t < table_count; t++) {
        // creating and allocating memory to object 'table'
        table = new Table;

        // extracting table dimension
        // at this point 'temp' is holding xDIM
        char dim_buff[2];
        int table_dim(0);
        strncpy(dim_buff, temp, 1);
        dim_buff[1] = '\0';
        // converting character to integer
        sscanf(dim_buff, "%d", &table_dim);
        table->set_dim(table_dim);

        // extracting table name
        tbl_stream >> temp;
        table->set_name(temp);
        tbl_stream.getline(line_clear, CHARL, '\n');

        // extracting dimensions of independent variables
        var_dim[0] = 1;
        var_dim[1] = 1;
        var_dim[2] = 1;
        for (tt=0; tt < table_dim; tt++) {
            tbl_stream >> temp;
            tbl_stream >> var_dim[tt];
            if (tt == 0)
                table->set_var1_dim(var_dim[tt]);
            if (tt == 1)
                table->set_var2_dim(var_dim[tt]);
            if (tt == 2)
                table->set_var3_dim(var_dim[tt]);
        }
        tbl_stream.getline(line_clear, CHARL, '\n');

        // allocating memory for variables and data arrays
        table->var1_values = std::vector<double>(var_dim[0]);
        table->var2_values = std::vector<double>(var_dim[1]);
        table->var3_values = std::vector<double>(var_dim[2]);
        table->data = std::vector<double>(var_dim[0] * var_dim[1] * var_dim[2]);

        // determining max number of rows of data
        int num_rows = var_dim[0];
        if (var_dim[0] < var_dim[1]) num_rows=var_dim[1];
        if (var_dim[2] > num_rows) num_rows=var_dim[2];

        // reading num_row of data
        for (tt=0; tt < num_rows; tt++) {
            // loading 1.variable values
            if (tt < var_dim[0]) {
                tbl_stream >> value;
                table->set_var1_value(tt, value);
            }

            // loading 2.variable values, but bypass if default dimension one
            if (tt < var_dim[1]&&var_dim[1] != 1) {
                tbl_stream >> value;
                table->set_var2_value(tt, value);
            }

            // loading 3.variable values, but bypass if default dimension one
            if (tt < var_dim[2]&&var_dim[2] != 1) {
                tbl_stream >> value;
                table->set_var3_value(tt, value);
            }

            // loading tabular data, which in all cases has only 'var_dim[0]' rows
            if (tt < var_dim[0]) {
                // read one row of data
                for (int ttt=0; ttt < var_dim[1]*var_dim[2]; ttt++) {
                    tbl_stream >> value;
                    table->set_data(tt*var_dim[1]*var_dim[2]+ttt, value);
                }
            }
        }  // end of reading data

        // loading table into 'Datadeck' pointer array 'Table **tabel_ptr'
        this->set_counter(t);
        this->add_table(*table);
        tbl_stream >> temp;  // reading next DIM entry
    }  // end of 'for' loop, finished loading all tables
}

/**
 * @brief Single independent variable look-up
 * constant extrapolation at the upper end, slope extrapolation at the lower end
 * flag = 1 means if input value hit upper bound use max value
 * @author 030717 Created by Peter H Zipfel
 */
double Datadeck::look_up(std::string name, double value1, unsigned int flag) {
    // finding slot of table in table pointer array (Table **table_ptr)
    int slot(-1);
    std::string tbl_name;
    do {
        slot++;
        tbl_name = get_tbl(slot)->get_name();
    }while (name != tbl_name);

    // getting table index locater of discrete value just below of variable value
    int var1_dim = get_tbl(slot)->get_var1_dim();
    int loc1 = find_index(var1_dim-1, value1, get_tbl(slot)->var1_values);
    if (flag == 1) {
        if (loc1 == (var1_dim - 1)) value1 = get_tbl(slot)->var1_values[loc1];
        if (loc1 == 0) {
            if (value1 < get_tbl(slot)->var1_values[loc1]) value1 = get_tbl(slot)->var1_values[loc1];
        }
    }

    // using max discrete value if value is outside table
    // if (loc1 == (var1_dim-1)) return get_tbl(slot)->data[loc1];
    if (loc1 == (var1_dim-1)) return interpolate(loc1-1, loc1, slot, value1);

    return interpolate(loc1, loc1+1, slot, value1);
}

/**
 * @brief Two independent variables look-up
 * Constant extrapolation at the upper end, slope extrapolation at the lower end
 * flag = 1 means if input value hit upper bound use max value
 * @author 030717 Created by Peter H Zipfel
 */
double Datadeck::look_up(std::string name, double value1, double value2, unsigned int flag) {
    // finding slot of table in table pointer array (Table **table_ptr)
    int slot(-1);
    std::string tbl_name;
    do {
        slot++;
        tbl_name = get_tbl(slot)->get_name();
    }while (name != tbl_name);

    // getting table index (off-set) locater of discrete value just below or equal of the variable value
    int var1_dim = get_tbl(slot)->get_var1_dim();
    int loc1 = find_index(var1_dim-1, value1, get_tbl(slot)->var1_values);

    int var2_dim = get_tbl(slot)->get_var2_dim();
    int loc2 = find_index(var2_dim-1, value2, get_tbl(slot)->var2_values);

    if (flag == 1) {
        if (loc1 == (var1_dim - 1)) value1 = get_tbl(slot)->var1_values[loc1];
        if (loc2 == (var2_dim - 1)) value2 = get_tbl(slot)->var2_values[loc2];
        if (loc1 == 0) {
            if (value1 < get_tbl(slot)->var1_values[loc1]) value1 = get_tbl(slot)->var1_values[loc1];
        }
        if (loc2 == 0) {
            if (value2 < get_tbl(slot)->var2_values[loc2]) value2 = get_tbl(slot)->var2_values[loc2];
        }
    }

    if (loc1 == (var1_dim - 1) && loc2 == (var2_dim - 1)) {
        return interpolate(loc1 - 1, loc1, loc2 - 1, loc2, slot, value1, value2);
    } else if (loc2 == (var2_dim - 1)) {
        return interpolate(loc1, loc1 + 1, loc2 - 1, loc2, slot, value1, value2);
    } else if (loc1 == (var1_dim - 1)) {
        return interpolate(loc1 - 1, loc1, loc2, loc2 + 1, slot, value1, value2);
    } else {
        return interpolate(loc1, loc1+1, loc2, loc2+1, slot, value1, value2);
    }
}

/**
 * @brief Three independent variables look-up
 * constant extrapolation at the upper end, slope extrapolation at the lower end
 * flag = 1 means if input value hit upper bound use max value
 * @author 030723 Created by Peter H Zipfel
 */
double Datadeck::look_up(std::string name, double value1, double value2, double value3, unsigned int flag) {
    // finding slot of table in table pointer array (Table **table_ptr)
    int slot(-1);
    std::string tbl_name;
    do {
        slot++;
        tbl_name = get_tbl(slot)->get_name();
    }while (name != tbl_name);

    // getting table index locater of discrete value just below of variable value
    int var1_dim = get_tbl(slot)->get_var1_dim();
    int loc1 = find_index(var1_dim-1, value1, get_tbl(slot)->var1_values);

    int var2_dim = get_tbl(slot)->get_var2_dim();
    int loc2 = find_index(var2_dim-1, value2, get_tbl(slot)->var2_values);

    int var3_dim = get_tbl(slot)->get_var3_dim();
    int loc3 = find_index(var3_dim-1, value3, get_tbl(slot)->var3_values);

    if (flag == 1) {
        if (loc1 == (var1_dim - 1)) value1 = get_tbl(slot)->var1_values[loc1];
        if (loc2 == (var2_dim - 1)) value2 = get_tbl(slot)->var2_values[loc2];
        if (loc3 == (var3_dim - 1)) value3 = get_tbl(slot)->var3_values[loc3];
        if (loc1 == 0) {
            if (value1 < get_tbl(slot)->var1_values[loc1]) value1 = get_tbl(slot)->var1_values[loc1];
        }
        if (loc2 == 0) {
            if (value2 < get_tbl(slot)->var2_values[loc2]) value2 = get_tbl(slot)->var2_values[loc2];
        }
        if (loc3 == 0) {
            if (value3 < get_tbl(slot)->var3_values[loc3]) value3 = get_tbl(slot)->var3_values[loc3];
        }
    }

    if (loc1 == (var1_dim-1) && loc2 == (var2_dim-1) && loc3 == (var3_dim-1)) {
        return interpolate(loc1-1, loc1, loc2-1, loc2, loc3-1, loc3, slot, value1, value2, value3);
    } else if (loc3 == (var3_dim-1)) {
        return interpolate(loc1, loc1+1, loc2, loc2+1, loc3-1, loc3, slot, value1, value2, value3);
    } else if (loc2 == (var2_dim-1)) {
        return interpolate(loc1, loc1+1, loc2-1, loc2, loc3, loc3+1, slot, value1, value2, value3);
    } else if (loc1 == (var1_dim-1)) {
        return interpolate(loc1-1, loc1, loc2, loc2+1, loc3, loc3+1, slot, value1, value2, value3);
    } else if (loc3 == (var3_dim-1) && loc2 == (var2_dim-1)) {
        return interpolate(loc1, loc1+1, loc2-1, loc2, loc3-1, loc3, slot, value1, value2, value3);
    } else if (loc3 == (var3_dim-1) && loc1 == (var1_dim-1)) {
        return interpolate(loc1-1, loc1, loc2, loc2+1, loc3-1, loc3, slot, value1, value2, value3);
    } else if (loc1 == (var1_dim-1) && loc2 == (var2_dim-1)) {
        return interpolate(loc1-1, loc1, loc2-1, loc2, loc3, loc3+1, slot, value1, value2, value3);
    } else {
        return interpolate(loc1, loc1+1, loc2, loc2+1, loc3, loc3+1, slot, value1, value2, value3);
    }
}

/**
 * @brief Table index finder
 * This is a binary search method it is O(lgN)
 *
 * @returns array locater of the discrete_variable just below variable
 * Keeps max or min array locater if variable is outside those max or min
 *
 * @author 010628 Created by Peter H Zipfel
 */
int Datadeck::find_index(int max, double value, std::vector<double> list) {
    if (value >= list[max]) {
        return max;
    } else if (value <= list[0]) {
        return 0;
    } else {
        int index = 0;
        int mid;
        while (index <= max) {
            mid = (index+max)/2;      // integer division
            if (value < list[mid])
                max = mid-1;
            else if (value > list[mid])
                index = mid+1;
            else
                return mid;
        }
        return max;
    }
}

/**
 * @brief Linear one-dimensional interpolation
 * Data deck must contain table in the following format:
 *
 * X1       Table Values(X1)
 *
 * X11      Y11
 * X12      Y12
 * X13      Y13
 *
 * Constant extrapolation beyond max values of X1
 * Slope extrapolation beyond min values of X1
 *
 * @author 030717 Created by Peter H Zipfel
 */
double Datadeck::interpolate(int ind1, int ind2, int slot, double val) {
    double dx(0), dy(0);
    double dumx(0);

    double diff = val-get_tbl(slot)->var1_values[ind1];
    dx = get_tbl(slot)->var1_values[ind2]-get_tbl(slot)->var1_values[ind1];
    dy = get_tbl(slot)->data[ind2]-get_tbl(slot)->data[ind1];

    if (fabs(dx) > arma::datum::eps) dumx = diff/dx;
    dy = dumx*dy;

    return get_tbl(slot)->data[ind1]+dy;
}

/**
 * @brief Linear, two-dimensional interpolation
 * File must contain table in the following form:
 *
 *  X1  X2  //Table Values(X1-row, X2-column)
 *            ---------------
 *  X11 X21   |Y11  Y12  Y13|
 *  X12 X22   |Y21  Y22  Y23|    <- data
 *  X13 X23   |Y31  Y32  Y33|
 *  X14       |Y41  Y42  Y43|
 *            ---------------
 * Constant extrapolation beyond max values of X1 and X2
 * Slope extrapolation beyond min values of X1 and X2
 *
 * @author 030718 Created by Peter H Zipfel
 */
double Datadeck::interpolate(int ind10, int ind11, int ind20, int ind21, int slot, double value1,
                    double value2) {
    double dx1(0), dx2(0);
    double dumx1(0), dumx2(0);

    int var1_dim = get_tbl(slot)->get_var1_dim();
    int var2_dim = get_tbl(slot)->get_var2_dim();

    double diff1 = value1-get_tbl(slot)->var1_values[ind10];
    double diff2 = value2-get_tbl(slot)->var2_values[ind20];

    // if (ind10 == (var1_dim-1))  // Assures constant upper extrapolation of first variable
    //     ind11 = ind10;
    // else
        dx1 = get_tbl(slot)->var1_values[ind11]-get_tbl(slot)->var1_values[ind10];

    // if (ind20 == (var2_dim-1))  // Assures constant upper extrapolation of second variable
    //     ind21 = ind20;
    // else
        dx2 = get_tbl(slot)->var2_values[ind21]-get_tbl(slot)->var2_values[ind20];

    if (fabs(dx1) > arma::datum::eps) dumx1 = diff1/dx1;
    if (fabs(dx2) > arma::datum::eps) dumx2 = diff2/dx2;

    double y11 = get_tbl(slot)->data[ind10*var2_dim+ind20];
    double y12 = get_tbl(slot)->data[ind10*var2_dim+ind21];
    double y21 = get_tbl(slot)->data[ind11*var2_dim+ind20];
    double y22 = get_tbl(slot)->data[ind11*var2_dim+ind21];
    double y1 = dumx1*(y21-y11)+y11;
    double y2 = dumx1*(y22-y12)+y12;

    return dumx2*(y2-y1)+y1;
}
/**
 * @brief Linear, three-dimensional interpolation
 * File must contain table in the following form:
 *
 *  X1  X2  X3    Table Values(X1-row, X2-block, X3-column) <- don't type (illustration only)
 *
 *                (X1 x X3) (X1 x X3) (X1 x X3) (X1 x X3)   <- don't type
 *                 for X21   for X22   for X23   for X24    <- don't type
 *               -----------------------------------------
 *  X11 X21 X31  |Y111 Y112|Y121 Y122|Y131 Y132|Y141 Y142|
 *  X12 X22 X32  |Y211 Y212|Y221 Y222|Y231 Y232|Y241 Y242|  <- data; don't type: '|'
 *  X13 X23      |Y311 Y312|Y321 Y322|Y331 Y332|Y341 Y342|
 *      X24      -----------------------------------------
 *
 * Constant extrapolation beyond max values of X1, X2 and X3
 * Slope extrapolation beyond min values of X1, X2 and X3
 *
 * @author 030723 Created and corrected by Peter Zipfel
 */
double Datadeck::interpolate(int ind10, int ind11, int ind20, int ind21, int ind30, int ind31,
                             int slot, double value1, double value2, double value3) {
    double dx1(0), dx2(0), dx3(0);
    double dumx1(0), dumx2(0), dumx3(0);

    int var1_dim = get_tbl(slot)->get_var1_dim();
    int var2_dim = get_tbl(slot)->get_var2_dim();
    int var3_dim = get_tbl(slot)->get_var3_dim();

    double diff1 = value1-get_tbl(slot)->var1_values[ind10];
    double diff2 = value2-get_tbl(slot)->var2_values[ind20];
    double diff3 = value3-get_tbl(slot)->var3_values[ind30];

    if (ind10 != (var1_dim-1))  // Assures constant upper extrapolation of first variable
        dx1 = get_tbl(slot)->var1_values[ind11]-get_tbl(slot)->var1_values[ind10];

    if (ind20 == (var2_dim-1))  // Assures constant upper extrapolation of second variable
        ind21 = ind20;
    else
        dx2 = get_tbl(slot)->var2_values[ind21]-get_tbl(slot)->var2_values[ind20];

    if (ind30 == (var3_dim-1))  // Assures constant upper extrapolation of third variable
        ind31 = ind30;
    else
        dx3 = get_tbl(slot)->var3_values[ind31]-get_tbl(slot)->var3_values[ind30];

    if (dx1 > arma::datum::eps) dumx1 = diff1/dx1;
    if (dx2 > arma::datum::eps) dumx2 = diff2/dx2;
    if (dx3 > arma::datum::eps) dumx3 = diff3/dx3;
    // int ind10,int ind11,int ind20,int ind21,int ind30,int ind31
    //      i        i+1        j         j+1       k        k+1
    // Use innner x1 and outer variable x3 for 2DIM interpolation, middle variable x2 is parameter
    // For parameter ind20
    double y11 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind20*var3_dim+ind30];
    double y12 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind20*var3_dim+ind30+var2_dim*var3_dim];
    double y31 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind20*var3_dim+ind31];
    double y32 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind20*var3_dim+ind31+var2_dim*var3_dim];
    // 2DIM interpolation
    double y1 = dumx1*(y12-y11)+y11;
    double y3 = dumx1*(y32-y31)+y31;
    double y21 = dumx3*(y3-y1)+y1;

    // For parameter ind21
    y11 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind21*var3_dim+ind30];
    y12 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind21*var3_dim+ind30+var2_dim*var3_dim];
    y31 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind21*var3_dim+ind31];
    y32 = get_tbl(slot)->data[ind10*var2_dim*var3_dim+ind21*var3_dim+ind31+var2_dim*var3_dim];
    // 2DIM interpolation
    y1 = dumx1*(y12-y11)+y11;
    y3 = dumx1*(y32-y31)+y31;
    double y22 = dumx3*(y3-y1)+y1;

    // 1DIM interpolation between the middle variable
    return dumx2*(y22-y21)+y21;
}


#include "gather_scatter.h"
#include "config.h"
#include "use_lua.h"
#include "io_utilities.h"
#include "particle_groups.h"
#include "particles.h"
#include "check_particle.h"
#include "call_lua_function.h"

#include <boost/format.hpp>
#include <boost/ref.hpp>
#include <luabind/luabind.hpp>
#include <cmath>
#include <iostream>
#include <stdexcept>

namespace PIC {

double R(double x, double h)
{
    return ((fabs(x) < h) ? (1.0 - fabs(x)/h) : 0.0);
}

double R(double x, double y, double z, double h)
{
    return (R(x, h)*R(y, h)*R(z, h));
}

/*****************************************************************************
* Gather edge-centered values.                                               *
*    at_point - specified position                                           *
*    val_x, val_y, val_z - Cell required values                              *
*    ret_vec - result gathered value                                         *
* Used to interpolate Bx, By, Bz for specified position                      *
*****************************************************************************/
void gather_edge(const Grid& grid, const DblVector& at_point,
                 CellVectorValue val, DblVector& ret_vec)
{
    ret_vec.x = gather_vector<EdgeXCentering>(grid, at_point, val, &DblVector::x);
    ret_vec.y = gather_vector<EdgeYCentering>(grid, at_point, val, &DblVector::y);
    ret_vec.z = gather_vector<EdgeZCentering>(grid, at_point, val, &DblVector::z);
}

/*****************************************************************************
* Gather face-centered values: Ex, Ey, Ez, UPx, UPy, UPz, UEx, UEy, UEz      *
*    at_point - specified position                                           *
*    val_x, val_y, val_z - Pointers to cell members required                 *
*    ret_vec - result gathered value                                         *
* Used to interpolate Ex, Ey, Ez, UPx, UPy, UPz, UEx, UEy, UEz values        *
* for specified position.                                                    *
*****************************************************************************/
void gather_face(const Grid& grid, const DblVector& at_point,
                 CellVectorValue val, DblVector& ret_vec)

{
    ret_vec.x = gather_vector<FaceXCentering>(grid, at_point, val, &DblVector::x);
    ret_vec.y = gather_vector<FaceYCentering>(grid, at_point, val, &DblVector::y);
    ret_vec.z = gather_vector<FaceZCentering>(grid, at_point, val, &DblVector::z);
}

/*****************************************************************************
* Gather cell-centered value.                                                *
*   x, y, z - specified position                                             *
*   val - Pointer to cell member required                                    *
*   ret_val - result gathered value                                          *
* Used to interpolate density NP values for specified position.              *
*                                                                            *
*****************************************************************************/
double gather_center(const Grid& grid, const DblVector& at_point, CellScalarValue val)
{
    return gather_scalar<CellCentering>(grid, at_point, val);
}

/******************************************************
* interpolates grid values to specified point (x,y,z) *
******************************************************/
void from_grid_to_point(const Grid& grid, const DblVector& vec_to_point, Grid::NodeType& val_at_point)
{
    // interpolate NP
    val_at_point.NP = gather_center(grid, vec_to_point, &Cell::NP);

    // interpolate B
    gather_edge(grid, vec_to_point, &Cell::B, val_at_point.B);

    //interpolate E
    gather_face(grid, vec_to_point, &Cell::E, val_at_point.E);

    // interpolate UP
    gather_face(grid, vec_to_point, &Cell::UP, val_at_point.UP);

    // interpolate UE
    gather_face(grid, vec_to_point, &Cell::UE, val_at_point.UE);
}

void set_boundary_conditions(const std::string& lua_func_name)
{
    if (!call_lua_function(lua_func_name.c_str()))
    {
        std::string msg_fmt = "Please check function \"%s\" in \"%s\" and restart.";

        std::string msg = str(boost::format(msg_fmt) % lua_func_name
                                                     % PIC::Config::cfg_script_name());

        throw std::domain_error(msg);
    }
}

void set_boundary_conditions(const std::string& lua_func_name, DensityGrid& dg)
{
    UseLua lua;

    try
    {
        luabind::call_function<void>(lua, lua_func_name.c_str(), boost::ref(dg));
    }
    catch (...)
    {
        const std::string fmt = "Error: in function \"%s\" not defined or invalid in \"%s\".";
        std::string msg = str(boost::format(fmt)
                              % lua_func_name
                              % PIC::Config::cfg_script_name());
        throw std::domain_error(msg);
    }
}

void scatter_particle_std(const Particle& particle, DensityGrid& grid, const DblVector& dr)
{
    const double h = Config::h();

    ParticleGroups part_groups;
    const double q = particle.ni*part_groups[particle.group_name].charge;

    const index_t pi = static_cast<index_t>((particle.r.x + dr.x)/h);
    const index_t pj = static_cast<index_t>((particle.r.y + dr.y)/h);
    const index_t pk = static_cast<index_t>((particle.r.z + dr.z)/h);

    typedef DensityGrid::NodeType DensityNode;

    for (index_t i = pi - 1; i != pi + 2; ++i)
    for (index_t j = pj - 1; j != pj + 2; ++j)
    for (index_t k = pk - 1; k != pk + 2; ++k)
    {
        DensityNode& cell = grid(i, j, k);

        cell.NP += q*R((i + 0.5)*h - particle.r.x,
                       (j + 0.5)*h - particle.r.y,
                       (k + 0.5)*h - particle.r.z, h);

        cell.UP.x += q*particle.v.x*R((i)*h       - particle.r.x,
                                      (j + 0.5)*h - particle.r.y,
                                      (k + 0.5)*h - particle.r.z, h);

        cell.UP.y += q*particle.v.y*R((i + 0.5)*h - particle.r.x,
                                      (j)*h       - particle.r.y,
                                      (k + 0.5)*h - particle.r.z, h);

        cell.UP.z += q*particle.v.z*R((i + 0.5)*h - particle.r.x,
                                      (j + 0.5)*h - particle.r.y,
                                      (k)*h       - particle.r.z, h);
    }
}

void scatter_particle_zigzag(const Particle& particle, DensityGrid& grid, const DblVector& dr)
{
    const double h = Config::h();

    const double tau2 = Config::tau_2();

    const double x1 = particle.r.x;
    const double x2 = x1 + dr.x;
    const index_t i[2] = { static_cast<index_t>(x1/h), static_cast<index_t>(x2/h) };

    const double y1 = particle.r.y;
    const double y2 = y1 + dr.y;
    const index_t j[2] = { static_cast<index_t>(y1/h), static_cast<index_t>(y2/h) };

    const double z1 = particle.r.z;
    const double z2 = z1 + dr.z;
    const index_t k[2] = { static_cast<index_t>(z1/h), static_cast<index_t>(z2/h) };
    ParticleGroups part_groups;
    const double q = particle.ni*part_groups[particle.group_name].charge;

    {
        // scatter charge Q and store it in NP
        const double wi[2] = { (i[1] + 1 - x2/h), (x2/h - i[1]) };
        const double wj[2] = { (j[1] + 1 - y2/h), (y2/h - j[1]) };
        const double wk[2] = { (k[1] + 1 - z2/h), (z2/h - k[1]) };

        for (int l = 0; l != 2; ++l)
        for (int m = 0; m != 2; ++m)
        for (int n = 0; n != 2; ++n)
        {
            grid(i[1] + l, j[1] + m, k[1] + n).NP += q*wi[l]*wj[m]*wk[n];
        }
    }

    // scatter currents Jx, Jy, Jz and store them  in UPx, UPy, UPz
    DblVector r(min(min(i[0]*h, i[1]*h) + h, max(max(i[0]*h, i[1]*h), 0.5*(x1 + x2))),
                min(min(j[0]*h, j[1]*h) + h, max(max(j[0]*h, j[1]*h), 0.5*(y1 + y2))),
                min(min(k[0]*h, k[1]*h) + h, max(max(k[0]*h, k[1]*h), 0.5*(z1 + z2))));

    DblVector F[2] = { DblVector(q*(r.x - x1)/tau2, q*(r.y - y1)/tau2, q*(r.z - z1)/tau2),
                       DblVector(q*(x2 - r.x)/tau2, q*(y2 - r.y)/tau2, q*(z2 - r.z)/tau2) };

    DblVector W[2] = { DblVector(0.5*(x1 + r.x)/h - i[0], 0.5*(y1 + r.y)/h - j[0], 0.5*(z1 + r.z)/h - k[0]),
                       DblVector(0.5*(r.x + x2)/h - i[1], 0.5*(r.y + y2)/h - j[1], 0.5*(r.z + z2)/h - k[1]) };

    for (int m = 0; m != 2; ++m)
    {
        grid(i[m], j[m], k[m]).UP.x += F[m].x*(1.0 - W[m].y)*(1.0 - W[m].z);
        grid(i[m], j[m] + 1, k[m]).UP.x += F[m].x*W[m].y*(1.0 - W[m].z);
        grid(i[m], j[m], k[m] + 1).UP.x += F[m].x*(1.0 - W[m].y)*W[m].z;
        grid(i[m], j[m] + 1, k[m] + 1).UP.x += F[m].x*W[m].y*W[m].z;

        grid(i[m], j[m], k[m]).UP.y += F[m].y*(1.0 - W[m].x)*(1.0 - W[m].z);
        grid(i[m] + 1, j[m], k[m]).UP.y += F[m].y*W[m].x*(1.0 - W[m].z);
        grid(i[m], j[m], k[m] + 1).UP.y += F[m].y*(1.0 - W[m].x)*W[m].z;
        grid(i[m] + 1, j[m], k[m] + 1).UP.y += F[m].y*W[m].x*W[m].z;

        grid(i[m], j[m], k[m]).UP.z += F[m].z*(1.0 - W[m].x)*(1.0 - W[m].y);
        grid(i[m] + 1, j[m], k[m]).UP.z += F[m].z*W[m].x*(1.0 - W[m].y);
        grid(i[m], j[m] + 1, k[m]).UP.z += F[m].z*(1.0 - W[m].x)*W[m].y;
        grid(i[m] + 1, j[m] + 1, k[m]).UP.z += F[m].z*W[m].x*W[m].y;
    }
}

} // end of namespace PIC


#pragma once
#include <type_traits>
#include <Eigen/Dense>
#include "math.h"

namespace Eigen
{
	namespace internal
	{
		template<typename PacketType>
		struct to_int_packet
		{
			typedef PacketType type;
		};

		template<typename PacketType>
		struct to_float_packet
		{
			typedef PacketType type;
		};
	}
}

#ifdef EIGEN_VECTORIZE_AVX
#include <immintrin.h>
#include "avx_gamma.h"

namespace Eigen
{
	namespace internal
	{
		template<> struct to_int_packet<Packet8f>
		{
			typedef Packet8i type;
		};

		template<> struct to_float_packet<Packet8i>
		{
			typedef Packet8f type;
		};

		EIGEN_STRONG_INLINE Packet8f p_to_f32(const Packet8i& a)
		{
			return _mm256_cvtepi32_ps(a);
		}

		EIGEN_STRONG_INLINE Packet8f p_bool2float(const Packet8f& a)
		{
			return _mm256_and_ps(_mm256_cmp_ps(a, _mm256_set1_ps(0), _CMP_NEQ_OQ), _mm256_set1_ps(1));
		}

		EIGEN_STRONG_INLINE Packet8f p_bool2float(const Packet8i& a)
		{
			return p_bool2float(_mm256_castsi256_ps(a));
		}
	}
}
#endif
#if defined(EIGEN_VECTORIZE_SSE2)
#include <xmmintrin.h>
#include "sse_gamma.h"

namespace Eigen
{
	namespace internal
	{
		template<> struct to_int_packet<Packet4f>
		{
			typedef Packet4i type;
		};

		template<> struct to_float_packet<Packet4i>
		{
			typedef Packet4f type;
		};

		EIGEN_STRONG_INLINE Packet4f p_to_f32(const Packet4i& a)
		{
			return _mm_cvtepi32_ps(a);
		}

		EIGEN_STRONG_INLINE Packet4f p_bool2float(const Packet4f& a)
		{
			return _mm_and_ps(_mm_cmpneq_ps(a, _mm_set1_ps(0)), _mm_set1_ps(1));
		}

		EIGEN_STRONG_INLINE Packet4f p_bool2float(const Packet4i& a)
		{
			return p_bool2float(_mm_castsi128_ps(a));
		}
	}
}
#endif

namespace Eigen
{
	namespace internal
	{
		template<typename Scalar, typename Scalar2> struct scalar_lgamma_subt_op {
			EIGEN_EMPTY_STRUCT_CTOR(scalar_lgamma_subt_op)
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& z, const Scalar2& a) const { return tomoto::math::lgammaSubt(z, a); }
			template<typename Packet>
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& z, const Packet& a) const
			{
				return lgamma_subt(z, a);
			}
		};

		template<typename Scalar, typename Scalar2>
		struct functor_traits<scalar_lgamma_subt_op<Scalar, Scalar2> >
		{
			enum {
				Cost = HugeCost,
				PacketAccess = 1
			};
		};

		template<>
		struct scalar_cast_op<int32_t, float> {
			EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
			typedef float result_type;
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const float operator() (const int32_t& a) const { return cast<int32_t, float>(a); }

			template<typename Packet>
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const typename to_int_packet<typename std::remove_const<Packet>::type>::type& a) const
			{
				return p_to_f32(a);
			}
		};

		template<>
		struct functor_traits<scalar_cast_op<int32_t, float> >
		{
			enum { Cost = NumTraits<float>::AddCost, PacketAccess = 1 };
		};

		template<typename ArgType>
		struct unary_evaluator<CwiseUnaryOp<scalar_cast_op<int32_t, float>, ArgType>, IndexBased >
			: evaluator_base<CwiseUnaryOp<scalar_cast_op<int32_t, float>, ArgType> >
		{
			typedef CwiseUnaryOp<scalar_cast_op<int32_t, float>, ArgType> XprType;

			enum {
				CoeffReadCost = evaluator<ArgType>::CoeffReadCost + functor_traits<scalar_cast_op<int32_t, float>>::Cost,

				Flags = evaluator<ArgType>::Flags
				& (HereditaryBits | LinearAccessBit | (functor_traits<scalar_cast_op<int32_t, float>>::PacketAccess ? PacketAccessBit : 0)),
				Alignment = evaluator<ArgType>::Alignment
			};

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				explicit unary_evaluator(const XprType& op)
				: m_functor(op.functor()),
				m_argImpl(op.nestedExpression())
			{
				EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<float>::AddCost);
				EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
			}

			typedef typename XprType::CoeffReturnType CoeffReturnType;

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				CoeffReturnType coeff(Index row, Index col) const
			{
				return m_functor(m_argImpl.coeff(row, col));
			}

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				CoeffReturnType coeff(Index index) const
			{
				return m_functor(m_argImpl.coeff(index));
			}

			template<int LoadMode, typename PacketType>
			EIGEN_STRONG_INLINE
				PacketType packet(Index row, Index col) const
			{
				return m_functor.packetOp<PacketType>(m_argImpl.template packet<LoadMode, typename to_int_packet<PacketType>::type>(row, col));
			}

			template<int LoadMode, typename PacketType>
			EIGEN_STRONG_INLINE
				PacketType packet(Index index) const
			{
				return m_functor.packetOp<PacketType>(m_argImpl.template packet<LoadMode, typename to_int_packet<PacketType>::type>(index));
			}

		protected:
			const scalar_cast_op<int32_t, float> m_functor;
			evaluator<ArgType> m_argImpl;
		};

		struct scalar_bool2float
		{
			EIGEN_EMPTY_STRUCT_CTOR(scalar_bool2float)
			typedef float result_type;
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const float operator() (const int32_t& a) const { return a ? 1.f : 0.f; }

			template<typename Packet>
			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename to_float_packet<Packet>::type packetOp(const Packet& a) const
			{
				return p_bool2float(a);
			}
		};

		template<>
		struct functor_traits<scalar_bool2float>
		{
			enum { Cost = NumTraits<float>::AddCost, PacketAccess = 1 };
		};

		template<typename ArgType>
		struct unary_evaluator<CwiseUnaryOp<scalar_bool2float, ArgType>, IndexBased >
			: evaluator_base<CwiseUnaryOp<scalar_bool2float, ArgType> >
		{
			typedef CwiseUnaryOp<scalar_bool2float, ArgType> XprType;

			enum {
				CoeffReadCost = evaluator<ArgType>::CoeffReadCost + functor_traits<scalar_bool2float>::Cost,

				Flags = evaluator<ArgType>::Flags
				& (HereditaryBits | LinearAccessBit | (functor_traits<scalar_bool2float>::PacketAccess ? PacketAccessBit : 0)),
				Alignment = evaluator<ArgType>::Alignment
			};

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				explicit unary_evaluator(const XprType& op)
				: m_functor(op.functor()),
				m_argImpl(op.nestedExpression())
			{
				EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<float>::AddCost);
				EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
			}

			typedef typename XprType::CoeffReturnType CoeffReturnType;

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				CoeffReturnType coeff(Index row, Index col) const
			{
				return m_functor(m_argImpl.coeff(row, col));
			}

			EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
				CoeffReturnType coeff(Index index) const
			{
				return m_functor(m_argImpl.coeff(index));
			}

			template<int LoadMode, typename PacketType>
			EIGEN_STRONG_INLINE
				PacketType packet(Index row, Index col) const
			{
				return m_functor.packetOp(m_argImpl.template packet<LoadMode, typename to_int_packet<PacketType>::type>(row, col));
			}

			template<int LoadMode, typename PacketType>
			EIGEN_STRONG_INLINE
				PacketType packet(Index index) const
			{
				return m_functor.packetOp(m_argImpl.template packet<LoadMode, typename to_int_packet<PacketType>::type>(index));
			}

		protected:
			const scalar_bool2float m_functor;
			evaluator<ArgType> m_argImpl;
		};

	}

	template <typename Derived, typename T> EIGEN_DEVICE_FUNC inline
		const CwiseBinaryOp<internal::scalar_lgamma_subt_op< typename internal::traits<Derived>::Scalar, T >, const Derived,
		const typename internal::plain_constant_type<Derived, T>::type>
		lgamma_subt(const Eigen::ArrayBase<Derived>& x, const T& scalar)  {

		return CwiseBinaryOp<internal::scalar_lgamma_subt_op< typename internal::traits<Derived>::Scalar, T >, const Derived,
			const typename internal::plain_constant_type<Derived, T>::type>(x.derived(),
				typename internal::plain_constant_type<Derived, T>::type(x.derived().rows(), x.derived().cols(), internal::scalar_constant_op<T>(scalar))
			);
	}

	template<typename Derived, typename Derived2>
	inline const CwiseBinaryOp<internal::scalar_lgamma_subt_op<typename Derived::Scalar, typename Derived2::Scalar>, const Derived, const Derived2>
		lgamma_subt(const Eigen::ArrayBase<Derived>& x, const Eigen::ArrayBase<Derived2>& y)
	{
		return CwiseBinaryOp<internal::scalar_lgamma_subt_op<typename Derived::Scalar, typename Derived2::Scalar>, const Derived, const Derived2>(
			x.derived(),
			y.derived()
		);
	}

	template<typename Derived>
	inline const CwiseUnaryOp<internal::scalar_bool2float, const Derived>
		bool2float(const Eigen::ArrayBase<Derived>& x)
	{
		return CwiseUnaryOp<internal::scalar_bool2float, const Derived>(
			x.derived()
		);
	}
}