//
// $Id$
//
//
// Original author: William French <william.r.french <a.t> vanderbilt.edu>
//
// Copyright 2008 Vanderbilt University - Nashville, TN 37232
//
// 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.
//

#define PWIZ_SOURCE

#include "SpectrumList_ChargeFromIsotope.hpp"
#include "pwiz/analysis/spectrum_processing/SpectrumList_PeakPicker.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include "pwiz/utility/misc/IntegerSet.hpp"
#include "pwiz/utility/chemistry/Ion.hpp"
#include <boost/range/algorithm/remove_if.hpp>

// Predicate for sorting score data structures
bool sortScoresByMZ (scoreChain i, scoreChain j) { return (i.mzPvalue < j.mzPvalue); }
bool sortScoresByKLScore (scoreChain i, scoreChain j) { return (i.intensityPvalue < j.intensityPvalue); }
bool sortScoresByIntensitySum (scoreChain i, scoreChain j) { return (i.intensitySumPvalue < j.intensitySumPvalue); }
bool sortScoresByOverallPvalue (scoreChain i, scoreChain j) { return (i.overallPvalue < j.overallPvalue); }
bool sortScoresBySumOfRanks (scoreChain i, scoreChain j) { return (i.sumRanks < j.sumRanks); }
// Predicate for sorting retention time data structure
bool sortByRetentionTime (rtimeMap i, rtimeMap j) { return (i.rtime < j.rtime); }
// Comparator for running upper_bound on retention time data structure
bool rtimeComparator(double a, rtimeMap i) { return (i.rtime > a ); }

bool pairSortFunc (pairData i, pairData j) { return (i.mz<j.mz); } // comparator for sorting of parentIon by m/z
bool pairCompare (pairData i, double mz) { return (i.mz<mz); } // comparator for searching parentIon by m/z

using namespace pwiz::msdata;
using namespace pwiz::cv;
using namespace pwiz::util;

namespace pwiz {
namespace analysis {

PWIZ_API_DECL SpectrumList_ChargeFromIsotope::SpectrumList_ChargeFromIsotope(
    const msdata::MSData& msd,
    int maxCharge,
    int minCharge,
    int parentsBefore,
    int parentsAfter,
    double isolationWidth,
    int defaultChargeMax,
    int defaultChargeMin)
:   SpectrumListWrapper(msd.run.spectrumListPtr),
    maxCharge_(maxCharge),
    minCharge_(minCharge),
    parentsBefore_(parentsBefore),
    parentsAfter_(parentsAfter),
    defaultIsolationWidth_(isolationWidth),
    defaultChargeMax_(defaultChargeMax),
    defaultChargeMin_(defaultChargeMin)
{

    srand( 1234 ); // using the same seed ensures consistency between runs, otherwise we can get different charges and precursors with the exact same settings

    // set parameters
    override_ = true;
    sigVal_ = 0.30;
    nChainsCheck_ = 8;
    mzTol = 0.06;
    maxIsotopePeaks = 5; // 2,3,4,5 (monoisotope included in this count)
    minIsotopePeaks = 2;
    massNeutron = 1.00335; // massC13 - massC12
    nSamples = 10000;
    upperLimitPadding = 1.25;
    maxNumberPeaks = 40;
    minNumberPeaks = 2;
    int nCharges = maxCharge_ - minCharge_ + 1;
    int nIsotopePeakPossibilities = maxIsotopePeaks - minIsotopePeaks + 1;

    // fill out MS1retentionTimes
    getMS1RetentionTimes();
    int surveyCnt = MS1retentionTimes.size();
    double finalRetentionTime = MS1retentionTimes[surveyCnt-1].rtime;

    // simulate the m/z spacing score (sum of squared errors)
    simulateSSE(nCharges,nIsotopePeakPossibilities);

    // simulte the relative intensity distribution (Kullback-Leibler divergence of Poisson-modeled intensities)
    simulateKL(finalRetentionTime,nIsotopePeakPossibilities,surveyCnt>=9?9:surveyCnt);

    // simulate the total peak intensity
    simulateTotIntensity(nIsotopePeakPossibilities);

}

PWIZ_API_DECL SpectrumPtr SpectrumList_ChargeFromIsotope::spectrum(size_t index, bool getBinaryData) const
{

    SpectrumPtr s = inner_->spectrum(index, true);

    // return non-MS/MS as-is
    CVParam spectrumType = s->cvParamChild(MS_spectrum_type);
    if (spectrumType != MS_MSn_spectrum)
        return s;

    // return MS1 as-is
    if (!s->hasCVParam(MS_ms_level) ||
        s->cvParam(MS_ms_level).valueAs<int>() < 2)
        return s;

    // return peakless spectrum as-is
    if (s->defaultArrayLength == 0)
        return s;

    // return precursorless MS/MS as-is
    if (s->precursors.empty() ||
        s->precursors[0].selectedIons.empty())
        return s;

    //cout << "Performing Turbocharger analysis!" << endl;

    // use first selected ion in first precursor
    // TODO: how to deal with multiple precursors and/or selected ions?
    Precursor& precursor = s->precursors[0];
    SelectedIon& selectedIon = precursor.selectedIons[0];

    //int vendorCharge = selectedIon.cvParam(MS_charge_state).valueAs<int>();

    // erase any existing charge-state-related CV params
    vector<CVParam>& cvParams = selectedIon.cvParams;
    IntegerSet possibleChargeStates;
    for(vector<CVParam>::iterator itr = cvParams.begin(); itr != cvParams.end(); ++itr)
    {
        if (itr->cvid == MS_charge_state ||
            itr->cvid == MS_possible_charge_state)
        {
            // some files may have a bogus "0" charge state
            if (override_ || itr->value == "0")
            {
                selectedIon.userParams.push_back(UserParam("old charge state", itr->value));
                itr = --cvParams.erase(itr);
            }
            else if (itr->cvid == MS_possible_charge_state)
                possibleChargeStates.insert(itr->valueAs<int>());
            else if (itr->cvid == MS_charge_state)
                return s;
        }
    }

    double precursorMZ = selectedIon.cvParam(MS_selected_ion_m_z).valueAs<double>();
    int nPossibleChargeStates = maxCharge_ - minCharge_ + 1;
    int nIsotopePeakPossibilities = maxIsotopePeaks - minIsotopePeaks + 1;

    // Get the upper/lower bounds of the precursor isolation window.
    // Of the data I've tested, only Thermo lists isolation window info.
    double upperIsoWidth = precursor.isolationWindow.cvParam(MS_isolation_window_upper_offset).valueAs<double>();
    upperIsoWidth = upperIsoWidth > 0.0 ? upperIsoWidth : defaultIsolationWidth_;
    double lowerIsoWidth = precursor.isolationWindow.cvParam(MS_isolation_window_lower_offset).valueAs<double>();
    lowerIsoWidth = lowerIsoWidth > 0.0 ? lowerIsoWidth : defaultIsolationWidth_;
    double targetIsoMZ = precursor.isolationWindow.cvParam(MS_isolation_window_target_m_z).valueAs<double>();
    targetIsoMZ = targetIsoMZ > 0.0 ? targetIsoMZ : precursorMZ;

    vector <int> parentIndex;
    getParentIndices(s,parentIndex);

    // info about chains/score across all parent scans
    vector <scoreChain> allScores;
    vector <isotopeChain> allChains;
    vector < vector<double> > allMZs;
    vector < vector<double> > allIntensities;
    int allChainsCnt=0;
    int assignedCharge = 0;
    double assignedMZ = targetIsoMZ;

    getParentPeaks(s,parentIndex,targetIsoMZ,lowerIsoWidth,upperIsoWidth,allMZs,allIntensities);

    // loop over peaks in parent spectra, build isotope chains and score them
    for (int i=0, iend=parentIndex.size(); i < iend; ++i)
    {

        int nPeaks = allMZs[i].size();
        vector <double> peakMZs(nPeaks);
        vector <double> peakIntensities(nPeaks);
        for (int j=0, jend=nPeaks; j < jend; ++j)
        {
            peakMZs[j] = allMZs[i][j];
            peakIntensities[j] = allIntensities[i][j];
        }
        vector <double> sortedPeakIntensities = peakIntensities;
        sort(sortedPeakIntensities.begin(),sortedPeakIntensities.end());

        vector <isotopeChain> chains;

        if ( nPeaks > 1 ) // need at least two peaks to perform analysis
        {

            for (int j=0,cnt=0; j<nPeaks; j++)
            {

                if ( peakMZs[j] > targetIsoMZ + upperIsoWidth ) break; // don't let the monoisotope move outside the isolation width

                for (int k = j+1; k<nPeaks; k++,cnt++)
                {

                    double mzDiff = peakMZs[k] - peakMZs[j]; // guaranteed to give positive value
                    if (mzDiff > massNeutron + mzTol) break; // subsequent sets of peaks will have spacing that is too large

                    double recip = massNeutron / mzDiff;
                    int possibleCharge = int(recip + 0.50);
                    if ( possibleCharge > maxCharge_ || possibleCharge < minCharge_ ) continue; // Not going to generate chain extension

                    if ( abs( massNeutron / double(possibleCharge) - mzDiff) < mzTol )
                    {

                        // We have a hit
                        // Start by checking if this can be connected to previous chains
                        for (int w=0, wend=chains.size(); w < wend; ++w)
                        {

                            if ( possibleCharge != chains[w].charge ) continue;
                            int nPeaksInChain = chains[w].indexList.size();
                            if ( nPeaksInChain >= maxIsotopePeaks ) continue;

                            int finalIndex = chains[w].indexList[chains[w].indexList.size()-1];
                            if ( j == finalIndex ) // connect it and save previous chain
                            {
                                chains.push_back(chains[w]); // push back copy of previous chain with current size
                                chains[w].indexList.push_back(k); // now extend the size of the chain
                            }

                        }

                        // Also create a new chain of length 2
                        isotopeChain newChain;
                        newChain.charge = possibleCharge;
                        newChain.indexList.push_back(j);
                        newChain.indexList.push_back(k);
                        newChain.parentIndex = i;
                        chains.push_back(newChain);

                    } // end if peak is within tolerance

                } // end for loop k

            } // end for loop j

        } // end if peak list size at least 2

        // Filter any chains that are not the required length
        if (chains.size() > 0)
        {

            vector<isotopeChain>::iterator it = chains.begin();
            while ( it != chains.end() )
            {
                if ( it->charge > 4 && it->indexList.size() < 3 )
                {
                    it = chains.erase(it);
                }
                else
                {
                    ++it;
                }

            }

        }

        scoreChain initializeScore;
        vector <scoreChain> scores(chains.size(),initializeScore);
        if (chains.size() > 0)
        {

            // Now perform the scoring
            vector<isotopeChain>::iterator chainIt = chains.begin();
            while ( chainIt != chains.end() )
            {

                //////////////////////////////////////////////////////////////////////////////////////
                // calculate the relative intensity score, based on the K-L score from a poisson distribution
                int j = chainIt - chains.begin();
                double KLscore = getKLscore( chains[j], peakMZs, peakIntensities );
                int startIndex = (chains[j].indexList.size()-minIsotopePeaks)*nSamples;
                vector <double> simValues;
                simValues.assign(&simulatedKLs[startIndex],&simulatedKLs[startIndex+nSamples-1]);
                vector< double >::iterator i1 = upper_bound(simValues.begin(),simValues.end(),KLscore); // returns iterator to first value that's > KLscore
                int klVectorIndex = i1 - simValues.begin();
                scores[j].intensityPvalue = double(klVectorIndex+1) / double(nSamples+1);
                //////////////////////////////////////////////////////////////////////////////////////

                //////////////////////////////////////////////////////////////////////////////////////
                // calculate average monoisotopic mass based on m/z positions in chain
                double average=0.0;
                for (int k=0, kend=chains[j].indexList.size(); k < kend; k++)
                    average += peakMZs[chains[j].indexList[k]] - double(k)*massNeutron/double(chains[j].charge);
                average /= double(chains[j].indexList.size());

                // calculate summed square error of m/z positions in chain relative to the average monoisotopic m/z
                double sse=0.0;
                for (int k=0, kend=chains[j].indexList.size(); k < kend; k++)
                    sse += pow( average - (peakMZs[chains[j].indexList[k]] - double(k)*massNeutron/double(chains[j].charge)),2);

                // get the P value for the m/z sse
                startIndex = (chains[j].indexList.size()-minIsotopePeaks)*nPossibleChargeStates*nSamples + (chains[j].charge-minCharge_)*nSamples;
                simValues.assign(&simulatedSSEs[startIndex],&simulatedSSEs[startIndex+nSamples-1]);
                i1 = upper_bound(simValues.begin(),simValues.end(),sse); // returns iterator to first value that's > sse
                int mzVectorIndex = i1 - simValues.begin();
                //////////////////////////////////////////////////////////////////////////////////////

                //////////////////////////////////////////////////////////////////////////////////////
                // now calculate the sum of intensity rank score
                if ( nPeaks > maxNumberPeaks )
                    throw runtime_error("[SpectrumList_chargeFromIsotope] nPeaks exceeds maxNumberPeaks for scoring.");
                int intensitySumRank = 0;
                for (int k=0, kend=chains[j].indexList.size(); k < kend; k++)
                {
                    double intensity = peakIntensities[chains[j].indexList[k]];
                    i1 = upper_bound(sortedPeakIntensities.begin(),sortedPeakIntensities.end(),intensity);
                    //i1--;
                    //int rank = nPeaks - (i1 - sortedPeakIntensities.begin()); // remember sortedPeakIntensities sorted from least to most intense
                    //int rank = nPeaks - 1 - (i1 - sortedPeakIntensities.begin()); // remember sortedPeakIntensities sorted from least to most intense
                    int rank = (i1 - 1) - sortedPeakIntensities.begin();
                    rank = nPeaks - rank;
                    intensitySumRank += rank;
                }
                startIndex = (nPeaks - minNumberPeaks)*nIsotopePeakPossibilities*nSamples + (chains[j].indexList.size()-minIsotopePeaks)*nSamples;

                // need to know how many samples were generated in the event that N choose k was less than nSamples
                int combos;
                if ( nPeaks > 40 && chains[j].indexList.size() > 2 )
                {
                    combos = nSamples;
                }
                else
                {
                    int combinations = nChoosek(nPeaks,chains[j].indexList.size());
                    combos = combinations > nSamples ? nSamples : combinations;
                }
                vector <int> simIntValues;
                simIntValues.assign(&simulatedIntensityRankSum[startIndex],&simulatedIntensityRankSum[startIndex+combos-1]);
                vector<int>::iterator i2 = upper_bound(simIntValues.begin(),simIntValues.end(),intensitySumRank); // returns iterator to first value that's > KLscore
                int intensitySumVectorIndex = i2 - simIntValues.begin();
                //////////////////////////////////////////////////////////////////////////////////////
                //////////////////////////////////////////////////////////////////////////////////////

                // probability of having lower sse by random chance
                scores[j].mzPvalue = double(mzVectorIndex+1) / double(nSamples+1);
                // K-L score is listed above
                scores[j].intensitySumPvalue = double(intensitySumVectorIndex) / double(combos); // don't add one here because we cannot beat the top rank sum
                scores[j].overallPvalue = scores[j].mzPvalue * scores[j].intensityPvalue * scores[j].intensitySumPvalue;
                scores[j].chainIndex = allChainsCnt++;

                chainIt++;

            } // end for through all chains

            allChains.insert(allChains.end(),chains.begin(),chains.end());
            allScores.insert(allScores.end(),scores.begin(),scores.end());

        } // end if chains.size() > 0

    } // end for over parent spectra

    if ( allScores.size() > 0 )
    {

        // rank scoring data structures, first by m/z
        sort(allScores.begin(),allScores.end(),sortScoresByMZ);
        for (int j=0, jend=allScores.size(); j < jend; ++j) allScores[j].mzRank = j+1;

        // now rank by relative intensity K-L score
        sort(allScores.begin(),allScores.end(),sortScoresByKLScore);
        for (int j=0, jend=allScores.size(); j < jend; ++j) allScores[j].intensityRank = j+1;

        // now rank by intensity rank sum score
        sort(allScores.begin(),allScores.end(),sortScoresByIntensitySum);
        for (int j=0, jend=allScores.size(); j < jend; ++j) allScores[j].intensitySumRank = j+1;

        // sum all the ranks
        for (int j=0, jend=allScores.size(); j < jend; ++j)
            allScores[j].sumRanks = allScores[j].mzRank + allScores[j].intensityRank + allScores[j].intensitySumRank;

        // Finally, sort by the sum of ranks. This appears to work better than
        // sorting by the overall p-value.
        sort(allScores.begin(),allScores.end(),sortScoresBySumOfRanks);

        int j,jend;
        int scoreListLength = allScores.size();
        int bestChainIndex = -1;
        for (j=0, jend = scoreListLength > nChainsCheck_ ? nChainsCheck_ : scoreListLength; j < jend; ++j)
        {
            if ( allScores[j].intensityPvalue < sigVal_ )
            {
                bestChainIndex = allScores[j].chainIndex;
                break;
            }
        }

        if ( bestChainIndex != -1 )
        {

            double mzTolppm = 100.0;
            double mzTolparts = mzTolppm / 1000000.0;
            // search for longer version of chain
            bool updateChain = true;
            while ( updateChain )
            {
                updateChain = false;
                for (int k=j+1, kend = scoreListLength > nChainsCheck_ ? nChainsCheck_ : scoreListLength; k < kend; ++k)
                {

                    if ( allScores[k].intensityPvalue > sigVal_ ) continue;
                    //
                    // trying to match chain x-y-z to w-x-y-z
                    //
                    // what's acceptable?
                    // have: x-y-z
                    //     - match: w-x-y-z, w-x-y
                    //     - not a match: w-x
                    //
                    // have x-y
                    //     - match: w-x-y
                    //     - not a match w-x
                    //
                    // So basically require at least two m/z matches to the chain you want to replace
                    //
                    //
                    int mapIndexK = allScores[k].chainIndex;
                    if ( allChains[bestChainIndex].charge != allChains[mapIndexK].charge ) continue;
                    int largeChainSize = allChains[mapIndexK].indexList.size();
                    if ( largeChainSize < 3 ) continue;

                    bool relatedChains = true;
                    double epsilon = mzTolparts * allMZs[allChains[mapIndexK].parentIndex][allChains[mapIndexK].indexList[0]];
                    for ( int w=0; w < 2; ++w )
                    {
                        double smallChainMZ = allMZs[allChains[bestChainIndex].parentIndex][allChains[bestChainIndex].indexList[w]];
                        double largeChainMZ = allMZs[allChains[mapIndexK].parentIndex][allChains[mapIndexK].indexList[w+1]];
                        if ( abs( smallChainMZ - largeChainMZ ) > epsilon )
                        {
                                relatedChains = false;
                                break;
                        }
                    }

                    if ( relatedChains )
                    {
                            updateChain = true;
                            bestChainIndex = mapIndexK;
                            j = k;
                            break;
                    } // end if relatedChains
                } // loop over remaining chains
            } // while update chain

            assignedCharge = allChains[bestChainIndex].charge;
            assignedMZ = allMZs[allChains[bestChainIndex].parentIndex][allChains[bestChainIndex].indexList[0]];

        } // endif best chain != -1

    }

    // make sure the possible charge states are erased if we want to override vendor charges
    if (override_ && !possibleChargeStates.empty())
                cvParams.erase(boost::range::remove_if(cvParams, CVParamIs(MS_possible_charge_state)));

    if ( assignedCharge != 0 ) // output single charge and m/z value
    {
        cvParams.push_back(CVParam(override_ ? MS_charge_state : MS_possible_charge_state, assignedCharge));
        s->precursors[0].selectedIons[0].set(MS_selected_ion_m_z, assignedMZ);
    }
    else if ( defaultChargeMin_ > 0 ) // output default charges, if requested by user
    {
        for (int z = defaultChargeMin_; z <= defaultChargeMax_; ++z)
            if (!possibleChargeStates.contains(z) && z != 1)
                cvParams.push_back(CVParam(MS_possible_charge_state, z));
    }

    return s;
}

void SpectrumList_ChargeFromIsotope::getMS1RetentionTimes()
{
    //cout << "Turbocharger initialization, storing and sorting survey scans by retention time. This may take a few minutes for large files." << endl << endl;

    int nScans = inner_->size();
    MS1retentionTimes.reserve( nScans );
    vector <int> ms1Indices;
    for (int i=0,iend=nScans; i<iend; ++i)
    {

        // Using FullMetadata rather than FullData avoids invoking nested filters unnecessarily
        DetailLevel detailLevel = DetailLevel_FullMetadata;
        SpectrumPtr s = inner_->spectrum(i,detailLevel);

        // Waters: scanConfig refers to the function number
        // Thermo: scanConfig corresponds to the msLevel
        // Agilent: scanConfig always returns zero
        // AB Sciex: scanConfig corresponds to the experiment number (for each "cycle" there is a MS1
        //           scan and then a variable number of MS2s...the MS1 is experiment 1 and then all
        //           subsequent scans are 2,3,4,...)
        // Bruker: untested
        if ( s->scanList.scans[0].empty() )
        {
            throw runtime_error("SpectrumList_chargeFromIsotope: no scanEvent present in raw data!");
        }
        int scanConfig = s->scanList.scans[0].cvParam(MS_preset_scan_configuration).valueAs<int>();
        if ( scanConfig == 0 )
        {
            int level = s->cvParam(MS_ms_level).valueAs<int>();
            if ( level != 1 ) continue;
        }
        else if ( scanConfig != 1 ) continue;

        double rTime = s->scanList.scans[0].cvParam(MS_scan_start_time).timeInSeconds();
        rtimeMap newRtime; newRtime.rtime = rTime; newRtime.indexMap = i;
        MS1retentionTimes.push_back(newRtime);
    }
    int surveyCnt = MS1retentionTimes.size();
    if ( surveyCnt == 0 )
        throw runtime_error("[SpectrumList_chargeFromIsotope] No survey scan found!");
    sort(MS1retentionTimes.begin(),MS1retentionTimes.end(),sortByRetentionTime); // these are generally already sorted, but just in case
}

void SpectrumList_ChargeFromIsotope::simulateSSE(const int nCharges,const int nIsotopePeakPossibilities)
{
    // generate a sample of random spacings to simulate the distribution
    // of summed square errors in m/z space due to random variations
    simulatedSSEs.resize( nSamples*nCharges*nIsotopePeakPossibilities, 0.0 );

    for (int i=0; i < nIsotopePeakPossibilities; ++i)
    {

        int chainLength = i + minIsotopePeaks;

        for (int w=0; w<nCharges; ++w)
        {

            int charge = minCharge_+w;
            vector <double> spacings(nSamples,0.0);

            for (int j=0; j<nSamples ; ++j)
            {
                vector <double> mzPoints(chainLength,0.0);
                double averageMonoisotope = 0.0;
                for (int k=1; k < chainLength; ++k)
                {
                    double theoreticalMZ = double(k) * massNeutron / double(charge);
                    double randomVar = -mzTol + ( (double)rand() / RAND_MAX ) * 2 * mzTol; // between -mzTol and +mzTol
                    mzPoints[k] = theoreticalMZ + randomVar;
                    averageMonoisotope += randomVar;
                }
                averageMonoisotope /= double(chainLength);

                double sse = 0.0;
                for (int k=0; k < chainLength; ++k)
                {
                    sse += pow( averageMonoisotope - (mzPoints[k] - double(k)*massNeutron/double(charge)),2);
                }
                spacings[j] = sse;

            } // end for over nSamples
            sort(spacings.begin(),spacings.end());
            int startIndex = i * nCharges * nSamples + w * nSamples;
            for (int j=0; j<nSamples ; ++j) simulatedSSEs[startIndex+j] = spacings[j];
        } // end for over nCharges
    } // end for over maxIsotopePeaks

}

void SpectrumList_ChargeFromIsotope::simulateKL(const double finalRetentionTime,const int nIsotopePeakPossibilities,const int nMS1sims)
{

    // grab nine survey scans from which to perform simulations
    vector <int> nineMS1indices(nMS1sims,-1);
    for (int i=1; i<=nMS1sims; i++)
    {
        double rTime = double(i) * finalRetentionTime / double(nMS1sims+1);
        vector< rtimeMap >::iterator i1 = upper_bound(MS1retentionTimes.begin(),MS1retentionTimes.end(),rTime,rtimeComparator); // returns iterator to first value that's > rTime
        int nearestMS1scanVectorIndex;
        if ( i1 == MS1retentionTimes.begin() )
            nearestMS1scanVectorIndex = 0;
        else
            nearestMS1scanVectorIndex = (i1 - 1) - MS1retentionTimes.begin();
        if (nearestMS1scanVectorIndex<0) nearestMS1scanVectorIndex = 0;
        nineMS1indices[i-1] = MS1retentionTimes[nearestMS1scanVectorIndex].indexMap;
    }

    ///////////////////////////////////////////////
    // now simulate the K-L score
    simulatedKLs.resize( nIsotopePeakPossibilities * nSamples, 0.0 );
    SpectrumListPtr CWTpeakPicker = instantiatePeakPicker( nineMS1indices );
    vector < vector<double> > allIntensity, allMZ;
    vector < vector<int> > mostIntensePeaks;
    int numberIntensePeaks = 200;

    for (int i=0,iend=nineMS1indices.size(); i < iend ; ++i)
    {

        SpectrumPtr s = CWTpeakPicker->spectrum( i, true ); // this applies no filtering
        BinaryData<double> peakIntensities = s->getIntensityArray()->data;
        BinaryData<double> peakMZs = s->getMZArray()->data;

        allIntensity.push_back( peakIntensities );
        allMZ.push_back( peakMZs );

        vector<double> sortedPeakIntensities = peakIntensities;
        sort( sortedPeakIntensities.begin(), sortedPeakIntensities.end() );
        double intensityCutoff = 0.0;
        int MS1Peaks = sortedPeakIntensities.size();
        if ( MS1Peaks >= numberIntensePeaks )
            intensityCutoff = sortedPeakIntensities[ peakIntensities.size() - numberIntensePeaks ];

        vector<int> highIntensityIndices;
        for (int j=0, jend=peakIntensities.size(); j < jend; ++j)
        {
            if ( peakIntensities[j] >= intensityCutoff )
                highIntensityIndices.push_back(j);
        }

        mostIntensePeaks.push_back( highIntensityIndices );

    }

    for (int j=0; j < nIsotopePeakPossibilities; ++j)
    {

        int chainLength = j + minIsotopePeaks;
        vector <double> poolA(chainLength,0.0);
        vector <double> poolB(chainLength,0.0);
        vector <double> KLscores(nSamples,0.0);

        for (int k=0; k<nSamples; ++k)
        {

            // get a random spectrum
            int randomSpectrum = rand() % nMS1sims; // random spectrum index between 0 and 8
            int peakCnt = allMZ[randomSpectrum].size();
            vector<double> peakMZs(peakCnt), peakIntensities(peakCnt);
            for ( int w=0, wend = peakMZs.size(); w < wend; ++w )
            {
                peakMZs[w] = allMZ[randomSpectrum][w];
                peakIntensities[w] = allIntensity[randomSpectrum][w];
            }

            int randomPeakIndex = rand() % mostIntensePeaks[randomSpectrum].size(); // from 0 to .size()-1
            int randomMZpoint = mostIntensePeaks[randomSpectrum][randomPeakIndex];
            double targetMZvalue = peakMZs[randomMZpoint];

            std::vector<double>::iterator lowerLimit = lower_bound( peakMZs.begin(), peakMZs.end(), targetMZvalue - defaultIsolationWidth_ );
            lowerLimit = lowerLimit == peakMZs.end() ? peakMZs.begin() : lowerLimit; // in case value is out of bounds
            std::vector<double>::iterator upperLimit = lower_bound( peakMZs.begin(), peakMZs.end(), targetMZvalue + defaultIsolationWidth_ + upperLimitPadding );
            upperLimit = upperLimit == peakMZs.end() ? peakMZs.end() - 1 : upperLimit; // in case value is out of bounds

            vector <double> windowIntensity, windowMZ;
            windowMZ.assign( lowerLimit, upperLimit );

            // this is possible if we chose the last peak of the spetrum and there is nothing around it
            if ( windowMZ.size() == 0 ) // no peaks found
            {
                KLscores[k] = 100; // just apply a really bad KL score
                continue;
            }

            int lowerLimitInt = lowerLimit - peakMZs.begin(), upperLimitInt = upperLimit - peakMZs.begin(); // convert iterators to ints
            windowIntensity.assign( &peakIntensities[lowerLimitInt], &peakIntensities[upperLimitInt] );

            // remove any zero-intensity peaks, which will lead to NAN results in KL scoring
            vector<double>::iterator winIt = windowIntensity.begin();
            while ( winIt != windowIntensity.end() )
            {
                if ( *winIt == 0.0 )
                {
                    windowIntensity.erase( winIt );
                    int mzIndex = winIt - windowIntensity.begin();
                    windowMZ.erase( windowMZ.begin() + mzIndex );
                }
                else
                {
                    ++winIt;
                }
            }

            // filter the peaks
            int peaksInWindow = windowIntensity.size();
            while ( peaksInWindow > maxNumberPeaks )
            {

                vector<double>::iterator minIt = min_element( windowIntensity.begin(), windowIntensity.end() );
                vector<double>::iterator maxIt = max_element( windowIntensity.begin(), windowIntensity.end() );
                vector<int> elementsForDeletion;

                for (int w=0, wend = windowIntensity.size(); w < wend; ++w)
                {
                    if ( windowIntensity[w] < *maxIt * 0.05 )
                        elementsForDeletion.push_back( w );
                    else if ( windowIntensity[w] == *minIt )
                        elementsForDeletion.push_back( w ); // remove if element equal to min or
                }

                sort(elementsForDeletion.rbegin(), elementsForDeletion.rend());
                for (int elementToDelete : elementsForDeletion)
                {
                    windowIntensity.erase( windowIntensity.begin() + elementToDelete );
                    windowMZ.erase( windowMZ.begin() + elementToDelete );
                }

                peaksInWindow = windowIntensity.size();

            }

            int windowSize = windowIntensity.size();
            isotopeChain localChain;
            localChain.charge = rand() % ( maxCharge_ - minCharge_ + 1 ) + minCharge_; // goes from minCharge_ to maxCharge_

            vector<int> randomIndicesInWindow( chainLength );

            for (int w=0; w<chainLength; ++w) randomIndicesInWindow[ w ] = rand() % windowSize;
            sort( randomIndicesInWindow.begin(), randomIndicesInWindow.end() );
            for (int w=0; w<chainLength; ++w) localChain.indexList.push_back( randomIndicesInWindow[w] );
            KLscores[k] = getKLscore( localChain, windowMZ, windowIntensity );

        }
        sort(KLscores.begin(),KLscores.end());
        int startIndex = j * nSamples;
        for (int k=0; k<nSamples ; ++k) simulatedKLs[startIndex+k] = KLscores[k];

    }

}

void SpectrumList_ChargeFromIsotope::simulateTotIntensity(const int nIsotopePeakPossibilities)
{

    // If there are fewer than 10,000 possible combinations of peaks, then
    // enumerate the combinations manually. Otherwise sample 10,000 random
    // combinations to approximate the distribution.

    int peakNumber = maxNumberPeaks - minNumberPeaks + 1;
    simulatedIntensityRankSum.resize( nIsotopePeakPossibilities * nSamples * peakNumber, 0 );

    for (int nPeaks = minNumberPeaks; nPeaks <= maxNumberPeaks; ++nPeaks)
    {

        int maxRandomRank = nPeaks;

        for (int w=0; w < nIsotopePeakPossibilities; ++w)
        {

            int chainLength = w + minIsotopePeaks;
            if (chainLength > nPeaks) break;

            int combinations; int sampleSize;
            if ( nPeaks > 40 && chainLength > 2 )
            {
                // all possible combinations could be too large to be held by a normal int
                combinations = nSamples;
            }
            else
            {
                combinations = nChoosek(nPeaks,chainLength);
            }
            sampleSize = combinations > nSamples ? nSamples : combinations;
            vector <int> intensityRankSum(sampleSize,0);

            if ( combinations >= nSamples ) // sample nSamples random combinations
            {

                for (int j=0; j<nSamples ; ++j)
                {

                    for (int k=0; k<chainLength; ++k)
                    {
                        int randomRank = rand() % maxRandomRank + 1; // generates random int between 1 and maxRandomRank
                        intensityRankSum[j] += randomRank;
                    }

                } // end for over nSamples
            }
            else // enumerate all possible combinations
            {
                int combinationCnt=0;
                vector<bool> v(nPeaks);
                fill(v.begin() + chainLength, v.end(), true);
                do {
                    for (int j = 0; j < nPeaks; ++j) {
                        if (!v[j]) {
                            intensityRankSum[combinationCnt] += j+1;
                        }
                    }
                    combinationCnt++;
                } while (next_permutation(v.begin(), v.end()));
            }

            sort(intensityRankSum.begin(),intensityRankSum.end());
            int startIndex = (nPeaks-minNumberPeaks)*nIsotopePeakPossibilities*nSamples + w * nSamples;
            for (int j=0; j<sampleSize ; ++j) simulatedIntensityRankSum[startIndex+j] = intensityRankSum[j];

        } // end for over maxIsotopePeaks

    } // end for over nPeaks

}

void SpectrumList_ChargeFromIsotope::getParentIndices( const SpectrumPtr s, vector <int> & parents ) const
{
    int nMS1scans = MS1retentionTimes.size();

    if ( nMS1scans > 0 )
    {
        int parentCnt = 0;
        double rTime = s->scanList.scans[0].cvParam(MS_scan_start_time).timeInSeconds();
        vector< rtimeMap >::const_iterator i1 = upper_bound(MS1retentionTimes.begin(),MS1retentionTimes.end(),rTime,rtimeComparator); // returns iterator to first value that's > rTime

		int nearestMS1scanVectorIndex;
		if ( i1 == MS1retentionTimes.begin() )
			nearestMS1scanVectorIndex = 0;
		else
			nearestMS1scanVectorIndex = (i1 - 1) - MS1retentionTimes.begin();

        while (nearestMS1scanVectorIndex >= 0 && parentCnt < parentsBefore_)
        {
            parents.push_back(MS1retentionTimes[nearestMS1scanVectorIndex].indexMap);
            nearestMS1scanVectorIndex--;
            parentCnt++;
        }
        nearestMS1scanVectorIndex = i1 - MS1retentionTimes.begin();
        parentCnt = 0; // resetting this
        //while (i1 != MS1retentionTimes.end() && parentCnt < parentsAfter_)
        while (nearestMS1scanVectorIndex < nMS1scans && parentCnt < parentsAfter_)
        {
            parents.push_back(MS1retentionTimes[nearestMS1scanVectorIndex].indexMap);
            //i1--;
            nearestMS1scanVectorIndex++;
            parentCnt++;
        }

    }
}

void SpectrumList_ChargeFromIsotope::getParentPeaks(const SpectrumPtr s,const vector <int> & parents,const double targetIsoMZ,const double lowerIsoWidth,const double upperIsoWidth,
                                                    vector< vector <double> > & mzs,vector< vector <double> > & intensities) const
{

    // grab the peaks from the parent scans
    for (int i=0, iend=parents.size(); i < iend; ++i)
    {

        vector< int > currentParent(1,parents[i]);
        DetailLevel detailLevel = DetailLevel_FullMetadata;
        SpectrumPtr sSurvey = inner_->spectrum(parents[i], detailLevel);
        vector<CVParam>& cvParams = sSurvey->cvParams;
        vector<CVParam>::iterator itr;
        itr = std::find(cvParams.begin(), cvParams.end(), MS_centroid_spectrum);

        if ( itr != cvParams.end() ) // MS1 spectrum already centroided, just grab the peaks in the isolation window
        {

            SpectrumPtr sPar = inner_->spectrum( parents[i], true );
            BinaryData<double>& peakMZs = sPar->getMZArray()->data;
            BinaryData<double>& peakIntensities = sPar->getIntensityArray()->data;
            vector<int> elementsForDeletion;

            for (int j=0, jend = peakIntensities.size(); j < jend; ++j)
            {
                if ( peakMZs[j] < targetIsoMZ - lowerIsoWidth || peakMZs[j] > targetIsoMZ + upperIsoWidth + upperLimitPadding )
                    elementsForDeletion.push_back( j );
            }

            sort(elementsForDeletion.rbegin(), elementsForDeletion.rend());
            for (int elementToDelete : elementsForDeletion)
            {
                peakIntensities.erase( peakIntensities.begin() + elementToDelete );
                peakMZs.erase( peakMZs.begin() + elementToDelete );
            }

            /**
            Reduce number of peaks in window to maxNumberPeaks if needed
            **/
            int peaksInWindow = peakIntensities.size();
            while ( peaksInWindow > maxNumberPeaks )
            {

                BinaryData<double>::iterator minIt = min_element( peakIntensities.begin(), peakIntensities.end() );

                elementsForDeletion.clear();
                for (int w=0, wend = peakIntensities.size(); w < wend; ++w)
                {
                    if ( peakIntensities[w] == *minIt )
                        elementsForDeletion.push_back( w );
                }

                sort(elementsForDeletion.rbegin(), elementsForDeletion.rend());
                for (int elementToDelete : elementsForDeletion)
                {
                    peakIntensities.erase( peakIntensities.begin() + elementToDelete );
                    peakMZs.erase( peakMZs.begin() + elementToDelete );
                }

                peaksInWindow = peakIntensities.size();

            }

            mzs.push_back( peakMZs );
            intensities.push_back( peakIntensities );

        }
        else // need to perform CWT peak-picking to grab the peaks
        {

            SpectrumListPtr parentPeakPicker = instantiatePeakPicker( currentParent, targetIsoMZ, lowerIsoWidth, upperIsoWidth + upperLimitPadding ); // add an additional dalton on the right to check for heavy isotopes; don't let monoisotope exceed the isolation window, though. See below.
            SpectrumPtr sPar = parentPeakPicker->spectrum( 0, true );
            BinaryData<double>& peakMZs = sPar->getMZArray()->data;
            BinaryData<double>& peakIntensities = sPar->getIntensityArray()->data;

            BinaryData<double>::iterator minIt = min_element(peakIntensities.begin(), peakIntensities.end());
            BinaryData<double>::iterator maxIt = max_element(peakIntensities.begin(), peakIntensities.end());
            vector<int> elementsForDeletion;

            for (int j=0, jend = peakIntensities.size(); j < jend; ++j)
            {
                if ( peakIntensities[j] < *maxIt * 0.05 )
                    elementsForDeletion.push_back( j );
                else if ( peakIntensities[j] == *minIt )
                    elementsForDeletion.push_back( j ); // remove if element equal to min or
            }

            sort(elementsForDeletion.rbegin(), elementsForDeletion.rend());
            for (int elementToDelete : elementsForDeletion)
            {
                peakIntensities.erase( peakIntensities.begin() + elementToDelete);
                peakMZs.erase( peakMZs.begin() + elementToDelete);
            }

            /**
            Reduce number of peaks in window to maxNumberPeaks if needed
            **/
            int peaksInWindow = peakIntensities.size();
            while ( peaksInWindow > maxNumberPeaks )
            {

                BinaryData<double>::iterator minIt = min_element( peakIntensities.begin(), peakIntensities.end() );

                elementsForDeletion.clear();
                for (int w=0, wend = peakIntensities.size(); w < wend; ++w)
                {
                    if ( peakIntensities[w] == *minIt )
                        elementsForDeletion.push_back( w );
                }

                sort(elementsForDeletion.rbegin(), elementsForDeletion.rend());
                for (int elementToDelete : elementsForDeletion)
                {
                    peakIntensities.erase( peakIntensities.begin() + elementToDelete );
                    peakMZs.erase( peakMZs.begin() + elementToDelete);
                }

                peaksInWindow = peakIntensities.size();

            }

            mzs.push_back( peakMZs );
            intensities.push_back( peakIntensities );
        }

    }
}

SpectrumListPtr SpectrumList_ChargeFromIsotope::instantiatePeakPicker( const vector <int> & indices ) const
{
    shared_ptr<SpectrumListSimple> smallSpectrumList(new SpectrumListSimple);

    for (int i=0,iend=indices.size(); i < iend ; ++i)
    {
        SpectrumPtr sParent = inner_->spectrum( indices[i], true);
        smallSpectrumList->spectra.push_back(sParent);
    }

    string msLevelSets = "1";
    IntegerSet msLevelsToCentroid;
    msLevelsToCentroid.parse(msLevelSets);
    bool preferVendor = false;
    double snr = 0.0;
    double mzTol = 0.05;
    int fixedPeaksKeep = 0; // this is for generating peaks to simulate the K-L score
    SpectrumListPtr parentPeakPicker(new SpectrumList_PeakPicker(smallSpectrumList,
                                        PeakDetectorPtr(new CwtPeakDetector(snr,fixedPeaksKeep,mzTol)),
                                        preferVendor, msLevelsToCentroid ) );

    return parentPeakPicker;

}

SpectrumListPtr SpectrumList_ChargeFromIsotope::instantiatePeakPicker( const vector <int> & indices, const double targetIsoMZ,
    const double lowerIsoWidth, const double upperIsoWidth ) const
{

    shared_ptr<SpectrumListSimple> smallSpectrumList(new SpectrumListSimple);

    // parameters for re-sampling via linear interpolation
    SpectrumPtr s = inner_->spectrum( indices[0], true);
    vector<double> summedIntensity;
    vector<double> summedMZ;

    // Grab the binary data for the parent spectrum
    SpectrumPtr sParent = inner_->spectrum( indices[0], true);
    BinaryData<double>& parentMz = sParent->getMZArray()->data;
    BinaryData<double>& parentIntensity = sParent->getIntensityArray()->data;

    // Get window of data around the target m/z value
    vector<double> windowMZ,windowIntensity;
    BinaryData<double>::iterator lowerLimit = lower_bound(parentMz.begin(), parentMz.end(), targetIsoMZ - lowerIsoWidth);
    lowerLimit = lowerLimit == parentMz.end() ? parentMz.begin() : lowerLimit; // in case value is out of bounds
    BinaryData<double>::iterator upperLimit = lower_bound(parentMz.begin(), parentMz.end(), targetIsoMZ + upperIsoWidth);
    upperLimit = upperLimit == parentMz.end() ? parentMz.end() - 1 : upperLimit; // in case value is out of bounds
    windowMZ.assign( lowerLimit, upperLimit );

    if ( windowMZ.size() > 1 )
    {

        int lowerLimitInt = lowerLimit - parentMz.begin(), upperLimitInt = upperLimit - parentMz.begin(); // convert interators to ints
        windowIntensity.assign( &parentIntensity[lowerLimitInt], &parentIntensity[upperLimitInt] );

        summedMZ.assign( lowerLimit, upperLimit );
        summedIntensity.assign( &parentIntensity[lowerLimitInt], &parentIntensity[upperLimitInt] );

    }

    // Now fill in the spectrum data structure that will be fed to the peak-picker
    smallSpectrumList->spectra.push_back(SpectrumPtr(new Spectrum));
    Spectrum& pSpec = *smallSpectrumList->spectra[0];
    pSpec.index = 0;
    pSpec.set(MS_ms_level, 1);
    pSpec.set(MS_profile_spectrum);
    BinaryDataArrayPtr pSpec_mz(new BinaryDataArray), pSpec_intensity(new BinaryDataArray);
    pSpec_mz->set(MS_m_z_array, "", MS_m_z), pSpec_intensity->set(MS_intensity_array, "", MS_number_of_detector_counts);
    pSpec_mz->data.resize( summedIntensity.size() ), pSpec_intensity->data.resize( summedIntensity.size() );

    for (size_t j=0, jend=summedIntensity.size() ; j < jend; ++j)
    {
        pSpec_mz->data[j] = summedMZ[j];
        pSpec_intensity->data[j] = summedIntensity[j];
    }

    pSpec.binaryDataArrayPtrs.push_back( pSpec_mz ), pSpec.binaryDataArrayPtrs.push_back( pSpec_intensity );
    pSpec.defaultArrayLength = pSpec_mz->data.size();

    string msLevelSets = "1";
    IntegerSet msLevelsToCentroid;
    msLevelsToCentroid.parse(msLevelSets);
    bool preferVendor = false;
    double snr = 0.0;
    double mzTol = 0.05;
    int fixedPeaksKeep = maxNumberPeaks;
    SpectrumListPtr parentPeakPicker(new SpectrumList_PeakPicker(smallSpectrumList,
                                        PeakDetectorPtr(new CwtPeakDetector(snr,fixedPeaksKeep,mzTol)),
                                        preferVendor, msLevelsToCentroid ) );

    return parentPeakPicker;

}

} // namespace analysis
} // namespace pwiz

int nChoosek(int n,int k)
{
    int numerator=1;
    int denominator=1;
    // Multiplicative formula
    for (int i=0;i<k;++i)
    {
        numerator *= n-i;
        denominator *= i+1;
    }
    return numerator/denominator;
}

double getKLscore( const isotopeChain chain, const vector <double> & mzs, const vector <double> & intensities )
{

    if ( mzs.size() != intensities.size() )
         throw runtime_error("[SpectrumList_chargeFromIsotope, getKLscore] m/z and intensity vectors must be equal in size.");

	//cout << "computing the KL score" << endl;

    // First, convert from molecular weight of ion to Mstar, which is linear mapping
    double lambda = 1.0 / 1800.0; // parameter for poisson model
    double Mstar = lambda * mzs[chain.indexList[0]] * double(chain.charge); // from msInspect paper
    double Mexp = exp( -Mstar );

    double poissonSum = 0.0; // sum up all the poisson values to normalize
    double observedIntensitySum = 0.0; // initialize this sum
    double KLscore = 0.0;

    vector <double> poissonVals(chain.indexList.size(),0.0);

    // calculate poisson distribution and sum up the intensities for normalization
    for (int k=0,kend=chain.indexList.size(); k < kend ; ++k)
    {
        // probability of seeing isotope with k additional ions relative to monoisotope
        double poisson = Mexp * pow(Mstar,k);
        for (int w=k;w>1;w--) poisson /= double(w);
        poissonVals[k] = poisson; // store value

        // sums for normalization
        poissonSum += poisson;
        observedIntensitySum += intensities[chain.indexList[k]];
    }

    // calculate the K-L score
    for (int k=0,kend=chain.indexList.size(); k < kend ; ++k)
    {
        poissonVals[k] /= poissonSum; // normalize these values to use in the K-L score
        double normObservedIntensity = intensities[chain.indexList[k]] / observedIntensitySum;
        KLscore += normObservedIntensity * log10( normObservedIntensity / poissonVals[k]  );
    }

	//cout << "done computing the KL score" << endl;

    return KLscore;

}


//-----------------------------------------------------------------------------
// Created on: 28 November 2016
//-----------------------------------------------------------------------------
// Copyright (c) 2017, Sergey Slyadnev
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder(s) nor the
//      names of all contributors may be used to endorse or promote products
//      derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//-----------------------------------------------------------------------------

// Own include
#include <asiAlgo_BaseCloud.h>

// asiAlgo includes
#include <asiAlgo_PointCloudUtils.h>

// OpenCascade includes
#include <gp_Ax1.hxx>
#include <gp_Ax3.hxx>
#include <gp_Vec.hxx>

// Eigen includes
#include <Eigen/Dense>

// Instantiate for allowed types
template class asiAlgo_BaseCloud<double>;
template class asiAlgo_BaseCloud<float>;

//-----------------------------------------------------------------------------

namespace
{
  bool compare(const std::pair<double, int>& p1,
               const std::pair<double, int>& p2)
  {
    return p1.first > p2.first;
  }
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
asiAlgo_BaseCloud<TCoordType>::asiAlgo_BaseCloud() {}

//-----------------------------------------------------------------------------

template <typename TCoordType>
asiAlgo_BaseCloud<TCoordType>::asiAlgo_BaseCloud(const std::vector<TCoordType>& coords)
: m_coords(coords) {}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::CopyTo(asiAlgo_BaseCloud<TCoordType>& copy) const
{
  copy.m_coords = m_coords;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::Reserve(const int nElems)
{
  m_coords.resize(nElems*3);
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
int asiAlgo_BaseCloud<TCoordType>::GetNumberOfElements() const
{
  return (int) (m_coords.size() / 3);
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
bool asiAlgo_BaseCloud<TCoordType>::IsEmpty() const
{
  return m_coords.size() == 0;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::AddElement(const TCoordType x,
                                               const TCoordType y,
                                               const TCoordType z)
{
  m_coords.push_back(x);
  m_coords.push_back(y);
  m_coords.push_back(z);
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::AddElement(const gp_XYZ& xyz)
{
  this->AddElement( TCoordType( xyz.X() ), TCoordType( xyz.Y() ), TCoordType( xyz.Z() ) );
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::AddElement(const gp_Pnt& xyz)
{
  this->AddElement( TCoordType( xyz.X() ), TCoordType( xyz.Y() ), TCoordType( xyz.Z() ) );
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::SetElement(const int        elemIndex,
                                               const TCoordType x,
                                               const TCoordType y,
                                               const TCoordType z)
{
  m_coords[elemIndex*3 + 0] = x;
  m_coords[elemIndex*3 + 1] = y;
  m_coords[elemIndex*3 + 2] = z;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::GetElement(const int   elemIndex,
                                               TCoordType& x,
                                               TCoordType& y,
                                               TCoordType& z) const
{
  x = m_coords[3*elemIndex + 0];
  y = m_coords[3*elemIndex + 1];
  z = m_coords[3*elemIndex + 2];
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
gp_XYZ asiAlgo_BaseCloud<TCoordType>::GetElement(const int elemIndex) const
{
  TCoordType x, y, z;
  this->GetElement(elemIndex, x, y, z);

  return gp_XYZ(x, y, z);
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
const std::vector<TCoordType>& asiAlgo_BaseCloud<TCoordType>::GetCoords() const
{
  return m_coords;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
Handle(TColStd_HArray1OfReal) asiAlgo_BaseCloud<TCoordType>::GetCoordsArray() const
{
  return asiAlgo_PointCloudUtils::AsRealArray(this);
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
std::vector<TCoordType>& asiAlgo_BaseCloud<TCoordType>::ChangeCoords()
{
  return m_coords;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::ComputeBoundingBox(TCoordType& xMin, TCoordType& xMax,
                                                       TCoordType& yMin, TCoordType& yMax,
                                                       TCoordType& zMin, TCoordType& zMax) const
{
  TCoordType _xMin = std::numeric_limits<TCoordType>::max();
  TCoordType _yMin = _xMin;
  TCoordType _zMin = _xMin;
  TCoordType _xMax = std::numeric_limits<TCoordType>::min();
  TCoordType _yMax = _xMax;
  TCoordType _zMax = _xMax;

  const int nElems = this->GetNumberOfElements();
  //
  for ( int e = 0; e < nElems; ++e )
  {
    TCoordType x, y, z;
    this->GetElement(e, x, y, z);

    if ( x > _xMax )
      _xMax = x;
    if ( x < _xMin )
      _xMin = x;
    if ( y > _yMax )
      _yMax = y;
    if ( y < _yMin )
      _yMin = y;
    if ( z > _zMax )
      _zMax = z;
    if ( z < _zMin )
      _zMin = z;
  }

  xMin = _xMin;
  xMax = _xMax;
  yMin = _yMin;
  yMax = _yMax;
  zMin = _zMin;
  zMax = _zMax;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
Handle(asiAlgo_BaseCloud<TCoordType>)
  asiAlgo_BaseCloud<TCoordType>::ExtractRegion(const asiAlgo_CloudRegion& region) const
{
  // Extracted base cloud
  Handle(asiAlgo_BaseCloud<TCoordType>) result  = new asiAlgo_BaseCloud<TCoordType>;

  // Extract
  for ( asiAlgo_CloudRegion::Iterator it(region); it.More(); it.Next() )
  {
    const int pidx = it.Key();

    TCoordType x, y, z;
    this->GetElement(pidx, x, y, z);
    result->AddElement(x, y, z);
  }

  return result;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::Merge(const Handle(asiAlgo_BaseCloud<TCoordType>)& cloud)
{
  const int nElems2Add = cloud->GetNumberOfElements();
  //
  for ( int e = 0; e < nElems2Add; ++e )
  {
    TCoordType x, y, z;
    cloud->GetElement(e, x, y, z);

    this->AddElement(x, y, z);
  }
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
bool asiAlgo_BaseCloud<TCoordType>::ComputeInertiaAxes(gp_Ax3& axes) const
{
  TCoordType xCenter = 0.0;
  TCoordType yCenter = 0.0;
  TCoordType zCenter = 0.0;
  TCoordType x, y, z;
  gp_XYZ meanVertex;

  for ( int i = 0; i < this->GetNumberOfElements(); ++i )
  {
    this->GetElement(i, x, y, z);
    xCenter += x;
    yCenter += y;
    zCenter += z;
  }

  meanVertex.SetX(xCenter);
  meanVertex.SetY(yCenter);
  meanVertex.SetZ(zCenter);
  meanVertex /= this->GetNumberOfElements();

  Eigen::Matrix3d C;
  for (int j = 0; j < 3; ++j)
  {
    for (int k = 0; k < 3; ++k)
      C(j, k) = 0.0; // TODO: is that necessary?
  }

  for (int i = 0; i < this->GetNumberOfElements(); ++i)
  {
    this->GetElement(i, x, y, z);
    gp_XYZ p = gp_XYZ(x, y, z);
    gp_XYZ p_dash = p - meanVertex;

    for (int j = 0; j < 3; ++j)
    {
      for (int k = 0; k < 3; ++k)
        C(j, k) += p_dash.Coord(j + 1) * p_dash.Coord(k + 1);
    }
  }

  for (int j = 0; j < 3; ++j)
  {
    for (int k = 0; k < 3; ++k)
      C(j, k) /= (this->GetNumberOfElements());
  }

  Eigen::EigenSolver<Eigen::Matrix3d> EigenSolver(C);

  Eigen::Vector3d v[3] { EigenSolver.eigenvectors().col(0).real(),
                         EigenSolver.eigenvectors().col(1).real(),
                         EigenSolver.eigenvectors().col(2).real() };

  // Make result stable. Eigen may return vector multiplied by -1.0 for
  // almost equivalent matrices with deviations (1e-14).
  for (int i = 0; i < 3; ++i)
  {
    Eigen::Vector3d& vec = v[i];

    int maxId = -1;
    double maxValue = -1.0;
    for (int j = 0; j < 3; ++j)
    {
      const double value = Abs(vec(j));
      if (value > maxValue)
      {
        maxId = j;
        maxValue = value;
      }
    }
    if (vec[maxId] < 0.0)
      vec *= -1.0;
  }

  gp_Vec V[3] = { gp_Vec(v[0].x(), v[0].y(), v[0].z()),
                  gp_Vec(v[1].x(), v[1].y(), v[1].z()),
                  gp_Vec(v[2].x(), v[2].y(), v[2].z()) };

  std::vector< std::pair<double, int> >
    lambda{ std::pair<double, int>(EigenSolver.eigenvalues()(0).real(), 0),
            std::pair<double, int>(EigenSolver.eigenvalues()(1).real(), 1),
            std::pair<double, int>(EigenSolver.eigenvalues()(2).real(), 2) };
  //
  std::sort(lambda.begin(), lambda.end(), compare);
  //
  gp_Ax1 axisX(meanVertex, V[lambda[0].second]);
  gp_Ax1 axisY(meanVertex, V[lambda[1].second]);
  gp_Ax1 axisZ(meanVertex, V[lambda[2].second]);

  // Check if the system is right-handed
  const double ang = axisX.Direction().AngleWithRef(axisY.Direction(), axisZ.Direction());
  if (ang < 0)
    axisZ.Reverse();

  axes = gp_Ax3( meanVertex, axisZ.Direction(), axisX.Direction() );

  return true;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
void asiAlgo_BaseCloud<TCoordType>::Clear()
{
  m_coords.clear();
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
bool asiAlgo_BaseCloud<TCoordType>::Load(const char* filename)
{
  std::ifstream FILE(filename);
  if ( !FILE.is_open() )
    return false;

  while ( !FILE.eof() )
  {
    char str[256];
    FILE.getline(str, 256);

    std::vector<std::string> tokens;
    std::istringstream iss(str);
    std::copy( std::istream_iterator<std::string>(iss),
               std::istream_iterator<std::string>(),
               std::back_inserter< std::vector<std::string> >(tokens) );

    if ( tokens.empty() || tokens.size() < 3 )
      continue;

    TCoordType x = (TCoordType) ( atof(tokens[0].c_str()) ),
               y = (TCoordType) ( atof(tokens[1].c_str()) ),
               z = (TCoordType) ( atof(tokens[2].c_str()) );

    this->AddElement(x, y, z);
  }

  FILE.close();
  return true;
}

//-----------------------------------------------------------------------------

template <typename TCoordType>
bool asiAlgo_BaseCloud<TCoordType>::SaveAs(const char* filename) const
{
  std::ofstream FILE(filename);
  if ( !FILE.is_open() )
    return false;

  for ( int e = 0; e < this->GetNumberOfElements(); ++e )
  {
    TCoordType x, y, z;
    this->GetElement(e, x, y, z);

    FILE << x << " " << y << " " << z << "\n";
  }

  FILE.close();
  return true;
}


/**********************************************************************************/
/* This file is part of spla project                                              */
/* https://github.com/JetBrains-Research/spla                                     */
/**********************************************************************************/
/* MIT License                                                                    */
/*                                                                                */
/* Copyright (c) 2021 JetBrains-Research                                          */
/*                                                                                */
/* Permission is hereby granted, free of charge, to any person obtaining a copy   */
/* of this software and associated documentation files (the "Software"), to deal  */
/* in the Software without restriction, including without limitation the rights   */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      */
/* copies of the Software, and to permit persons to whom the Software is          */
/* furnished to do so, subject to the following conditions:                       */
/*                                                                                */
/* The above copyright notice and this permission notice shall be included in all */
/* copies or substantial portions of the Software.                                */
/*                                                                                */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  */
/* SOFTWARE.                                                                      */
/**********************************************************************************/
#ifndef SPLA_SPLAMERGEBYKEY_HPP
#define SPLA_SPLAMERGEBYKEY_HPP

#include <functional>

#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/meta_kernel.hpp>
#include <boost/hana.hpp>

namespace spla {

    /**
     * @addtogroup Internal
     * @{
     */

    namespace detail {
        class MergeByKeyPathKernel : public boost::compute::detail::meta_kernel {
        public:
            unsigned int tileSize;

            MergeByKeyPathKernel()
                : meta_kernel("__spla_merge_path"), tileSize(4) {}

            template<
                    typename OutputIterator1, typename OutputIterator2,
                    typename BinaryIndicesOperator>
            void SetRange(const std::size_t aCount,
                          const std::size_t bCount,
                          OutputIterator1 resultA,
                          OutputIterator2 resultB,
                          BinaryIndicesOperator isFirstGt) {
                namespace compute = boost::compute;
                using compute::uint_;

                mACount = aCount;
                mACountArg = add_arg<uint_>("a_count");

                mBCount = bCount;
                mBCountArg = add_arg<uint_>("b_count");

                *this << "uint i = get_global_id(0);\n"
                      << "uint target = (i+1)*" << tileSize << ";\n"
                      << "uint start = max(convert_int(0),convert_int(target)-convert_int(b_count));\n"
                      << "uint end = min(target,a_count);\n"
                      << "uint a_index, b_index;\n"
                      << "while(start<end)\n"
                      << "{\n"
                      << "   a_index = (start + end)/2;\n"
                      << "   b_index = target - a_index - 1;\n"
                      << "   if(!(";
                isFirstGt(*this, expr<uint_>("a_index"), expr<uint_>("b_index"));
                *this << "))\n"
                      << "       start = a_index + 1;\n"
                      << "   else end = a_index;\n"
                      << "}\n"
                      << resultA[expr<uint_>("i")] << " = start;\n"
                      << resultB[expr<uint_>("i")] << " = target - start;\n";
            }

            boost::compute::event exec(boost::compute::command_queue &queue) {
                namespace compute = boost::compute;

                if ((mACount + mBCount) / tileSize == 0) {
                    return {};
                }

                set_arg(mACountArg, compute::uint_(mACount));
                set_arg(mBCountArg, compute::uint_(mBCount));

                return exec_1d(queue, 0, (mACount + mBCount) / tileSize);
            }

        private:
            std::size_t mACount{};
            std::size_t mACountArg{};
            std::size_t mBCount{};
            std::size_t mBCountArg{};
        };

        class SerialMergeByKeyKernel : boost::compute::detail::meta_kernel {
        public:
            unsigned int tileSize;

            SerialMergeByKeyKernel()
                : meta_kernel("__spla_serial_merge"),
                  tileSize(4) {}

            template<
                    typename InputIterator1,
                    typename InputIterator2,
                    typename Operator1,
                    typename Operator2,
                    typename Operator3>
            void SetRange(InputIterator1 tile_first1,
                          InputIterator1 tile_last1,
                          InputIterator2 tile_first2,
                          Operator1 writeIsSecondGt,
                          Operator2 writeAssignResultToFirst,
                          Operator3 writeAssignResultToSecond) {
                namespace compute = boost::compute;
                using compute::uint_;

                mCount = compute::detail::iterator_range_size(tile_first1, tile_last1) - 1;

                *this << "uint i = get_global_id(0);\n"
                      << "uint start1 = " << tile_first1[expr<uint_>("i")] << ";\n"
                      << "uint end1 = " << tile_first1[expr<uint_>("i+1")] << ";\n"
                      << "uint start2 = " << tile_first2[expr<uint_>("i")] << ";\n"
                      << "uint end2 = " << tile_first2[expr<uint_>("i+1")] << ";\n"
                      << "uint index = i*" << tileSize << ";\n"
                      << "while(start1<end1 && start2<end2)\n"
                      << "{\n"
                      << "   if(!(";
                writeIsSecondGt(*this, expr<uint_>("start1"), expr<uint_>("start2"));
                *this << "))\n"
                      << "   {\n";
                writeAssignResultToFirst(*this, expr<uint_>("index"), expr<uint_>("start1"));
                *this << ";\n"
                      << "       index++;\n"
                      << "       start1++;\n"
                      << "   }\n"
                      << "   else\n"
                      << "   {\n";
                writeAssignResultToSecond(*this, expr<uint_>("index"), expr<uint_>("start2"));
                *this << ";\n"
                      << "       index++;\n"
                      << "       start2++;\n"
                      << "   }\n"
                      << "}\n"
                      << "while(start1<end1)\n"
                      << "{\n";
                writeAssignResultToFirst(*this, expr<uint_>("index"), expr<uint_>("start1"));
                *this << ";\n"
                      << "   index++;\n"
                      << "   start1++;\n"
                      << "}\n"
                      << "while(start2<end2)\n"
                      << "{\n";
                writeAssignResultToSecond(*this, expr<uint_>("index"), expr<uint_>("start2"));
                *this << ";\n"
                      << "   index++;\n"
                      << "   start2++;\n"
                      << "}\n";
            }

            boost::compute::event exec(boost::compute::command_queue &queue) {
                if (mCount == 0) {
                    return boost::compute::event{};
                }

                return exec_1d(queue, 0, mCount);
            }

        private:
            std::size_t mCount{};
        };

        template<
                typename Operator1,
                typename Operator2,
                typename Operator3>
        std::ptrdiff_t MergeByKey(
                const std::size_t count1,
                const std::size_t count2,
                Operator1 compareFirstToSecond,
                Operator2 assignResultToFirst,
                Operator3 assignResultToSecond,
                boost::compute::command_queue &queue) {
            namespace compute = boost::compute;
            using compute::uint_;

            const std::size_t tileSize = 1024;

            compute::vector<uint_> tileA((count1 + count2 + tileSize - 1) / tileSize + 1, queue.get_context());
            compute::vector<uint_> tileB((count1 + count2 + tileSize - 1) / tileSize + 1, queue.get_context());

            // Tile the sets
            MergeByKeyPathKernel tilingKernel;
            tilingKernel.tileSize = static_cast<unsigned int>(tileSize);
            tilingKernel.SetRange(count1, count2, tileA.begin() + 1, tileB.begin() + 1, compareFirstToSecond);
            fill_n(tileA.begin(), 1, uint_(0), queue);
            fill_n(tileB.begin(), 1, uint_(0), queue);
            tilingKernel.exec(queue);

            fill_n(tileA.end() - 1, 1, static_cast<uint_>(count1), queue);
            fill_n(tileB.end() - 1, 1, static_cast<uint_>(count2), queue);

            // Merge
            SerialMergeByKeyKernel mergeKernel;
            mergeKernel.tileSize = static_cast<unsigned int>(tileSize);
            mergeKernel.SetRange(tileA.begin(), tileA.end(), tileB.begin(),
                                 compareFirstToSecond, assignResultToFirst, assignResultToSecond);

            mergeKernel.exec(queue);

            return static_cast<std::ptrdiff_t>(count1 + count2);
        }

        namespace binop {
            using MetaKernel = boost::compute::detail::meta_kernel;
            using MetaIdx = boost::compute::detail::meta_kernel_variable<boost::compute::uint_>;

            template<typename ItA, typename ItB>
            class IsFirstGt {
                ItA a;
                ItB b;

            public:
                explicit IsFirstGt(ItA itA, ItB itB) : a(std::move(itA)), b(std::move(itB)) {}

                void operator()(MetaKernel &k, const MetaIdx &iFst, const MetaIdx &iSnd) {
                    k << a[iFst] << " > " << b[iSnd];
                }
            };

            template<typename ItA, typename ItB, typename ItC, typename ItD>
            class IsFirstPairGt {
                ItA aFst;
                ItB aSnd;
                ItC bFst;
                ItD bSnd;

            public:
                explicit IsFirstPairGt(ItA itAFst, ItB itASnd, ItC itBFst, ItD itBSnd)
                    : aFst(std::move(itAFst)), aSnd(std::move(itASnd)), bFst(std::move(itBFst)), bSnd(std::move(itBSnd)) {}

                void operator()(MetaKernel &k, const MetaIdx &iFst, const MetaIdx &iSnd) {
                    k << aFst[iFst] << " > " << bFst[iSnd] << " || "
                      << "("
                      << aFst[iFst] << " == " << bFst[iSnd] << " && "
                      << aSnd[iFst] << " > " << bSnd[iSnd]
                      << ")";
                }
            };

            template<typename... Assignees>
            class Assign {
                boost::hana::tuple<Assignees...> mToAssign;

            public:
                explicit Assign(Assignees... toAssign)
                    : mToAssign(toAssign...) {}

                void operator()(MetaKernel &k, const MetaIdx &iRes, const MetaIdx &i) {
                    boost::hana::for_each(mToAssign, [&](auto &p) {
                        k << p.first[iRes] << " = " << p.second[i] << ";";
                    });
                }
            };
        }// namespace binop

    }// namespace detail

    /**
     * @brief Merges two sorted (by key) sequences of values by given keys.
     *
     * @param keysABegin Begin of the first key sequence
     * @param keysAEnd End of the first key sequence
     * @param valuesA Begin of the first value sequence
     * @param keysBBegin Begin of the second key sequence
     * @param keysBEnd End of the second key sequence
     * @param valuesB Begin of the second value sequence
     * @param keysResult Begin of the keys result
     * @param valuesResult Begin of the values result
     * @param queue OpenCL Command queue
     *
     * @return Size of the merged sequence
     */
    template<
            typename ItKeysABegin,
            typename ItKeysAEnd,
            typename ItValuesA,
            typename ItKeysBBegin,
            typename ItKeysBEnd,
            typename ItValuesB,
            typename ItKeysResult,
            typename ItValuesResult>
    inline std::ptrdiff_t MergeByKeys(
            ItKeysABegin keysABegin,
            ItKeysAEnd keysAEnd,
            ItValuesA valuesA,
            ItKeysBBegin keysBBegin,
            ItKeysBEnd keysBEnd,
            ItValuesB valuesB,
            ItKeysResult keysResult,
            ItValuesResult valuesResult,
            boost::compute::command_queue &queue) {

        return detail::MergeByKey(
                std::distance(keysABegin, keysAEnd),
                std::distance(keysBBegin, keysBEnd),
                detail::binop::IsFirstGt{keysABegin, keysBBegin},
                detail::binop::Assign{std::pair{keysResult, keysABegin}, std::pair{valuesResult, valuesA}},
                detail::binop::Assign{std::pair{keysResult, keysBBegin}, std::pair{valuesResult, valuesB}},
                queue);
    }

    /**
     * @brief Merges two sorted sequences.
     *
     * @param keysABegin Begin of the first sequence
     * @param keysAEnd End of the first sequence
     * @param keysBBegin Begin of the second sequence
     * @param keysBEnd End of the second sequence
     * @param keysResult Begin of the result
     * @param queue OpenCL Command queue
     *
     * @return Size of the merged sequence
     */
    template<
            typename ItKeysABegin,
            typename ItKeysAEnd,
            typename ItKeysBBegin,
            typename ItKeysBEnd,
            typename ItKeysResult>
    inline std::ptrdiff_t MergeKeys(
            ItKeysABegin keysABegin,
            ItKeysAEnd keysAEnd,
            ItKeysBBegin keysBBegin,
            ItKeysBEnd keysBEnd,
            ItKeysResult keysResult,
            boost::compute::command_queue &queue) {

        return detail::MergeByKey(
                std::distance(keysABegin, keysAEnd),
                std::distance(keysBBegin, keysBEnd),
                detail::binop::IsFirstGt{keysABegin, keysBBegin},
                detail::binop::Assign{std::pair{keysResult, keysABegin}},
                detail::binop::Assign{std::pair{keysResult, keysBBegin}},
                queue);
    }

    /**
     * @brief Merges two sorted (by pair key)
     *
     * @desc In this function, key pair sequence is actually
     * two sequences of keys. Hence, nth key of the first
     * sequence is
     * {@p keysFirstABegin[n], @p keysSecondABegin[n]},
     * and of the second second is
     * {@p keysFirstBBegin[n], @p keysSecondBBegin[n]}.
     *
     * @return Size of the merged sequence.
     *
     * @note Pair key - a key, which consists of two keys.
     *       It is compared lexicographically: first it compares
     *       by the first element, and then by the second.
     */
    template<
            typename ItInput1,
            typename ItInput2,
            typename ItInput3,
            typename ItInput4,
            typename ItInput5,
            typename ItInput6,
            typename ItOutput1,
            typename ItOutput2,
            typename ItOutput3>
    inline std::ptrdiff_t MergeByPairKeys(
            ItInput1 keysFirstABegin, ItInput1 keysFirstAEnd, ItInput2 keysSecondABegin, ItInput3 valuesA,
            ItInput4 keysFirstBBegin, ItInput4 keysFirstBEnd, ItInput5 keysSecondBBegin, ItInput6 valuesB,
            ItOutput1 keysFirstOut, ItOutput2 keysSecondOut, ItOutput3 valuesOut,
            boost::compute::command_queue &queue) {

        return detail::MergeByKey(
                std::distance(keysFirstABegin, keysFirstAEnd),
                std::distance(keysFirstBBegin, keysFirstBEnd),
                detail::binop::IsFirstPairGt{keysFirstABegin, keysSecondABegin, keysFirstBBegin, keysSecondBBegin},
                detail::binop::Assign{
                        std::pair{keysFirstOut, keysFirstABegin},
                        std::pair{keysSecondOut, keysSecondABegin},
                        std::pair{valuesOut, valuesA}},
                detail::binop::Assign{
                        std::pair{keysFirstOut, keysFirstBBegin},
                        std::pair{keysSecondOut, keysSecondBBegin},
                        std::pair{valuesOut, valuesB}},
                queue);
    }

    /**
     * @brief Merges two sorted (by pair key)
     *
     * @desc In this function, key pair sequence is actually
     * two sequences of keys. Hence, nth key of the first
     * sequence is
     * {@p keysFirstABegin[n], @p keysSecondABegin[n]},
     * and of the second second is
     * {@p keysFirstBBegin[n], @p keysSecondBBegin[n]}.
     *
     * @return Size of the merged sequence.
     *
     * @note Pair key - a key, which consists of two keys.
     *       It is compared lexicographically: first it compares
     *       by the first element, and then by the second.
     */
    template<
            typename ItInput1,
            typename ItInput2,
            typename ItInput4,
            typename ItInput5,
            typename ItOutput1,
            typename ItOutput2>
    inline std::ptrdiff_t MergePairKeys(
            ItInput1 keysFirstABegin, ItInput1 keysFirstAEnd, ItInput2 keysSecondABegin,
            ItInput4 keysFirstBBegin, ItInput4 keysFirstBEnd, ItInput5 keysSecondBBegin,
            ItOutput1 keysFirstOut, ItOutput2 keysSecondOut,
            boost::compute::command_queue &queue) {

        return detail::MergeByKey(
                std::distance(keysFirstABegin, keysFirstAEnd),
                std::distance(keysFirstBBegin, keysFirstBEnd),
                detail::binop::IsFirstPairGt{keysFirstABegin, keysSecondABegin, keysFirstBBegin, keysSecondBBegin},
                detail::binop::Assign{std::pair{keysFirstOut, keysFirstABegin}, std::pair{keysSecondOut, keysSecondABegin}},
                detail::binop::Assign{std::pair{keysFirstOut, keysFirstBBegin}, std::pair{keysSecondOut, keysSecondBBegin}},
                queue);
    }

    /**
     * @}
     */

}// namespace spla

#endif//SPLA_SPLAMERGEBYKEY_HPP


// Copyright (C) 2004-2006 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

/**************************************************************************
 * This source file implements a variation on distributed Dijkstra's      *
 * algorithm that can expose additional parallelism by permitting         *
 * vertices within a certain distance from the minimum to be processed,   *
 * even though they may not be at their final distance. This can          *
 * introduce looping, but the algorithm will still terminate so long as   *
 * there are no negative loops.                                           *
 **************************************************************************/
#ifndef BOOST_GRAPH_EAGER_DIJKSTRA_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_EAGER_DIJKSTRA_SHORTEST_PATHS_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/assert.hpp>
#include <boost/graph/distributed/detail/dijkstra_shortest_paths.hpp>
#include <boost/property_map/parallel/caching_property_map.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/distributed/detail/remote_update_set.hpp>
#include <vector>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/parallel/container_traits.hpp>

#ifdef PBGL_ACCOUNTING
#  include <boost/graph/accounting.hpp>
#  include <numeric>
#endif // PBGL_ACCOUNTING

#ifdef MUTABLE_QUEUE
#  include <boost/pending/mutable_queue.hpp>
#endif

namespace boost { namespace graph { namespace distributed {

#ifdef PBGL_ACCOUNTING
struct eager_dijkstra_shortest_paths_stats_t
{
  /* The value of the lookahead parameter. */
  double lookahead;

  /* Total wall-clock time used by the algorithm.*/
  accounting::time_type execution_time;

  /* The number of vertices deleted in each superstep. */
  std::vector<std::size_t> deleted_vertices;

  template<typename OutputStream>
  void print(OutputStream& out)
  {
    double avg_deletions = std::accumulate(deleted_vertices.begin(),
                                           deleted_vertices.end(),
                                           0.0);
    avg_deletions /= deleted_vertices.size();

    out << "Problem = \"Single-Source Shortest Paths\"\n"
        << "Algorithm = \"Eager Dijkstra\"\n"
        << "Function = eager_dijkstra_shortest_paths\n"
        << "(P) Lookahead = " << lookahead << "\n"
        << "Wall clock time = " << accounting::print_time(execution_time)
        << "\nSupersteps = " << deleted_vertices.size() << "\n"
        << "Avg. deletions per superstep = " << avg_deletions << "\n";
  }
};

static eager_dijkstra_shortest_paths_stats_t eager_dijkstra_shortest_paths_stats;
#endif

namespace detail {

// Borrowed from BGL's dijkstra_shortest_paths
template <class UniformCostVisitor, class Queue,
          class WeightMap, class PredecessorMap, class DistanceMap,
          class BinaryFunction, class BinaryPredicate>
 struct parallel_dijkstra_bfs_visitor : bfs_visitor<>
{
  typedef typename property_traits<DistanceMap>::value_type distance_type;

  parallel_dijkstra_bfs_visitor(UniformCostVisitor vis, Queue& Q,
                                WeightMap w, PredecessorMap p, DistanceMap d,
                                BinaryFunction combine, BinaryPredicate compare,
                                distance_type zero)
    : m_vis(vis), m_Q(Q), m_weight(w), m_predecessor(p), m_distance(d),
      m_combine(combine), m_compare(compare), m_zero(zero)  { }

  template <class Vertex, class Graph>
  void initialize_vertex(Vertex u, Graph& g)
    { m_vis.initialize_vertex(u, g); }
  template <class Vertex, class Graph>
  void discover_vertex(Vertex u, Graph& g) { m_vis.discover_vertex(u, g); }
  template <class Vertex, class Graph>
  void examine_vertex(Vertex u, Graph& g) { m_vis.examine_vertex(u, g); }

  /* Since the eager formulation of Parallel Dijkstra's algorithm can
     loop, we may relax on *any* edge, not just those associated with
     white and gray targets. */
  template <class Edge, class Graph>
  void examine_edge(Edge e, Graph& g) {
    if (m_compare(get(m_weight, e), m_zero))
        boost::throw_exception(negative_edge());

    m_vis.examine_edge(e, g);

    boost::parallel::caching_property_map<PredecessorMap> c_pred(m_predecessor);
    boost::parallel::caching_property_map<DistanceMap> c_dist(m_distance);

    distance_type old_distance = get(c_dist, target(e, g));

    bool m_decreased = relax(e, g, m_weight, c_pred, c_dist,
                             m_combine, m_compare);

    /* On x86 Linux with optimization, we sometimes get into a
       horrible case where m_decreased is true but the distance hasn't
       actually changed. This occurs when the comparison inside
       relax() occurs with the 80-bit precision of the x87 floating
       point unit, but the difference is lost when the resulting
       values are written back to lower-precision memory (e.g., a
       double). With the eager Dijkstra's implementation, this results
       in looping. */
    if (m_decreased && old_distance != get(c_dist, target(e, g))) {
      m_Q.update(target(e, g));
      m_vis.edge_relaxed(e, g);
    } else
      m_vis.edge_not_relaxed(e, g);
  }
  template <class Vertex, class Graph>
  void finish_vertex(Vertex u, Graph& g) { m_vis.finish_vertex(u, g); }

  UniformCostVisitor m_vis;
  Queue& m_Q;
  WeightMap m_weight;
  PredecessorMap m_predecessor;
  DistanceMap m_distance;
  BinaryFunction m_combine;
  BinaryPredicate m_compare;
  distance_type m_zero;
};

  /**********************************************************************
   * Dijkstra queue that implements arbitrary "lookahead"               *
   **********************************************************************/
  template<typename Graph, typename Combine, typename Compare,
           typename VertexIndexMap, typename DistanceMap,
           typename PredecessorMap>
  class lookahead_dijkstra_queue
    : public graph::detail::remote_update_set<
               lookahead_dijkstra_queue<
                 Graph, Combine, Compare, VertexIndexMap, DistanceMap,
                 PredecessorMap>,
               typename boost::graph::parallel::process_group_type<Graph>::type,
               typename dijkstra_msg_value<DistanceMap, PredecessorMap>::type,
               typename property_map<Graph, vertex_owner_t>::const_type>
  {
    typedef typename graph_traits<Graph>::vertex_descriptor
      vertex_descriptor;
    typedef lookahead_dijkstra_queue self_type;
    typedef typename boost::graph::parallel::process_group_type<Graph>::type
      process_group_type;
    typedef dijkstra_msg_value<DistanceMap, PredecessorMap> msg_value_creator;
    typedef typename msg_value_creator::type msg_value_type;
    typedef typename property_map<Graph, vertex_owner_t>::const_type
      OwnerPropertyMap;

    typedef graph::detail::remote_update_set<self_type, process_group_type,
                                             msg_value_type, OwnerPropertyMap>
      inherited;

    // Priority queue for tentative distances
    typedef indirect_cmp<DistanceMap, Compare> queue_compare_type;

    typedef typename property_traits<DistanceMap>::value_type distance_type;

#ifdef MUTABLE_QUEUE
    typedef mutable_queue<vertex_descriptor, std::vector<vertex_descriptor>,
                          queue_compare_type, VertexIndexMap> queue_type;

#else
    typedef relaxed_heap<vertex_descriptor, queue_compare_type,
                         VertexIndexMap> queue_type;
#endif // MUTABLE_QUEUE

    typedef typename process_group_type::process_id_type process_id_type;

  public:
    typedef vertex_descriptor value_type;

    lookahead_dijkstra_queue(const Graph& g,
                             const Combine& combine,
                             const Compare& compare,
                             const VertexIndexMap& id,
                             const DistanceMap& distance_map,
                             const PredecessorMap& predecessor_map,
                             distance_type lookahead)
      : inherited(boost::graph::parallel::process_group(g), get(vertex_owner, g)),
        queue(num_vertices(g), queue_compare_type(distance_map, compare), id),
        distance_map(distance_map),
        predecessor_map(predecessor_map),
        min_distance(0),
        lookahead(lookahead)
#ifdef PBGL_ACCOUNTING
        , local_deletions(0)
#endif
    { }

    void push(const value_type& x)
    {
      msg_value_type msg_value =
        msg_value_creator::create(get(distance_map, x),
                                  predecessor_value(get(predecessor_map, x)));
      inherited::update(x, msg_value);
    }

    void update(const value_type& x) { push(x); }

    void pop()
    {
      queue.pop();
#ifdef PBGL_ACCOUNTING
      ++local_deletions;
#endif
    }

    value_type&       top()       { return queue.top(); }
    const value_type& top() const { return queue.top(); }

    bool empty()
    {
      inherited::collect();

      // If there are no suitable messages, wait until we get something
      while (!has_suitable_vertex()) {
        if (do_synchronize()) return true;
      }

      // Return true only if nobody has any messages; false if we
      // have suitable messages
      return false;
    }

  private:
    vertex_descriptor predecessor_value(vertex_descriptor v) const
    { return v; }

    vertex_descriptor
    predecessor_value(property_traits<dummy_property_map>::reference) const
    { return graph_traits<Graph>::null_vertex(); }

    bool has_suitable_vertex() const
    {
      return (!queue.empty()
              && get(distance_map, queue.top()) <= min_distance + lookahead);
    }

    bool do_synchronize()
    {
      using boost::parallel::all_reduce;
      using boost::parallel::minimum;

      inherited::synchronize();

      // TBD: could use combine here, but then we need to stop using
      // minimum<distance_type>() as the function object.
      distance_type local_distance =
        queue.empty()? (std::numeric_limits<distance_type>::max)()
        : get(distance_map, queue.top());

      all_reduce(this->process_group, &local_distance, &local_distance + 1,
                 &min_distance, minimum<distance_type>());

#ifdef PBGL_ACCOUNTING
      std::size_t deletions = 0;
      all_reduce(this->process_group, &local_deletions, &local_deletions + 1,
                 &deletions, std::plus<std::size_t>());
      if (process_id(this->process_group) == 0)
        eager_dijkstra_shortest_paths_stats.deleted_vertices
          .push_back(deletions);
      local_deletions = 0;
      BOOST_ASSERT(deletions > 0);
#endif

      return min_distance == (std::numeric_limits<distance_type>::max)();
    }

  public:
    void
    receive_update(process_id_type source, vertex_descriptor vertex,
                   distance_type distance)
    {
      // Update the queue if the received distance is better than
      // the distance we know locally
      if (distance <= get(distance_map, vertex)) {

        // Update the local distance map
        put(distance_map, vertex, distance);

        bool is_in_queue = queue.contains(vertex);

        if (!is_in_queue)
          queue.push(vertex);
        else
          queue.update(vertex);
      }
    }

    void
    receive_update(process_id_type source, vertex_descriptor vertex,
                   std::pair<distance_type, vertex_descriptor> p)
    {
      if (p.first <= get(distance_map, vertex)) {
        put(predecessor_map, vertex, p.second);
        receive_update(source, vertex, p.first);
      }
    }

  private:
    queue_type     queue;
    DistanceMap    distance_map;
    PredecessorMap predecessor_map;
    distance_type  min_distance;
    distance_type  lookahead;
#ifdef PBGL_ACCOUNTING
    std::size_t    local_deletions;
#endif
  };
  /**********************************************************************/
} // end namespace detail

template<typename DistributedGraph, typename DijkstraVisitor,
         typename PredecessorMap, typename DistanceMap, typename WeightMap,
         typename IndexMap, typename ColorMap, typename Compare,
         typename Combine, typename DistInf, typename DistZero>
void
eager_dijkstra_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance,
   typename property_traits<DistanceMap>::value_type lookahead,
   WeightMap weight, IndexMap index_map, ColorMap color_map,
   Compare compare, Combine combine, DistInf inf, DistZero zero,
   DijkstraVisitor vis)
{
#ifdef PBGL_ACCOUNTING
  eager_dijkstra_shortest_paths_stats.deleted_vertices.clear();
  eager_dijkstra_shortest_paths_stats.lookahead = lookahead;
  eager_dijkstra_shortest_paths_stats.execution_time = accounting::get_time();
#endif

  // Initialize local portion of property maps
  typename graph_traits<DistributedGraph>::vertex_iterator ui, ui_end;
  for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
    put(distance, *ui, inf);
    put(predecessor, *ui, *ui);
  }
  put(distance, s, zero);

  // Dijkstra Queue
  typedef detail::lookahead_dijkstra_queue
            <DistributedGraph, Combine, Compare, IndexMap, DistanceMap,
             PredecessorMap> Queue;

  Queue Q(g, combine, compare, index_map, distance,
          predecessor, lookahead);

  // Parallel Dijkstra visitor
  detail::parallel_dijkstra_bfs_visitor
    <DijkstraVisitor, Queue, WeightMap, PredecessorMap, DistanceMap, Combine,
     Compare> bfs_vis(vis, Q, weight, predecessor, distance, combine, compare,
                      zero);

  set_property_map_role(vertex_color, color_map);
  set_property_map_role(vertex_distance, distance);

  breadth_first_search(g, s, Q, bfs_vis, color_map);

#ifdef PBGL_ACCOUNTING
  eager_dijkstra_shortest_paths_stats.execution_time =
    accounting::get_time()
    - eager_dijkstra_shortest_paths_stats.execution_time;
#endif
}

template<typename DistributedGraph, typename DijkstraVisitor,
         typename PredecessorMap, typename DistanceMap, typename WeightMap>
void
eager_dijkstra_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance,
   typename property_traits<DistanceMap>::value_type lookahead,
   WeightMap weight)
{
  typedef typename property_traits<DistanceMap>::value_type distance_type;

  std::vector<default_color_type> colors(num_vertices(g), white_color);

  eager_dijkstra_shortest_paths(g, s, predecessor, distance, lookahead, weight,
                                get(vertex_index, g),
                                make_iterator_property_map(&colors[0],
                                                           get(vertex_index,
                                                               g)),
                                std::less<distance_type>(),
                                closed_plus<distance_type>(),
                                distance_type(),
                                (std::numeric_limits<distance_type>::max)(),
                                dijkstra_visitor<>());
}

template<typename DistributedGraph, typename DijkstraVisitor,
         typename PredecessorMap, typename DistanceMap>
void
eager_dijkstra_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance,
   typename property_traits<DistanceMap>::value_type lookahead)
{
  eager_dijkstra_shortest_paths(g, s, predecessor, distance, lookahead,
                               get(edge_weight, g));
}
} // end namespace distributed

#ifdef PBGL_ACCOUNTING
using distributed::eager_dijkstra_shortest_paths_stats;
#endif

using distributed::eager_dijkstra_shortest_paths;

} } // end namespace boost::graph

#endif // BOOST_GRAPH_EAGER_DIJKSTRA_SHORTEST_PATHS_HPP