/*
  Author(s):      Robert I A Patterson
  Project:        sweepc (population balance solver)
  Sourceforge:    http://sourceforge.net/projects/mopssuite

  Copyright (C) 2009 Robert I A Patterson.

  File purpose:
    Implementation of transition regime coagulation kernel
	for hybrid particle-number/particle model

  Licence:
    This file is part of "sweepc".

    sweepc is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/
#include "swp_hybrid_transcoag.h"

#include "swp_params.h"
#include "swp_cell.h"
#include "swp_mechanism.h"
#include "swp_PAH_primary.h"
#include <boost/random/uniform_01.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>

using namespace Sweep::Processes;

// Default constructor.
Sweep::Processes::HybridTransitionCoagulation::HybridTransitionCoagulation(const Sweep::Mechanism &mech)
: Coagulation(mech), m_efm(mech.GetEnhancementFM())
{
    m_name = "HybridTransitionRegimeCoagulation";
}

Sweep::Processes::HybridTransitionCoagulation* const Sweep::Processes::HybridTransitionCoagulation::Clone() const
{
	return new HybridTransitionCoagulation(*this);
}

// Stream-reading constructor.
Sweep::Processes::HybridTransitionCoagulation::HybridTransitionCoagulation(std::istream &in, const Sweep::Mechanism &mech)
: Coagulation(mech), m_efm(mech.GetEnhancementFM())
{
    m_name = "HybridTransitionRegimeCoagulation";
    Deserialize(in, mech);
}

// TOTAL RATE CALCULATION.

// Returns the rate of the process for the given system.
double Sweep::Processes::HybridTransitionCoagulation::Rate(double t, const Cell &sys,
                                                          const Geometry::LocalGeometry1d &local_geom) const
{
    // Get the number of particles in the system.
    unsigned int n = sys.ParticleCount() + sys.Particles().GetTotalParticleNumber();

    // Check that there are at least 2 particles before calculating rate.
    if (n > 1) {
        // Get system properties required to calculate coagulation rate.
        double T = sys.GasPhase().Temperature();
        double P = sys.GasPhase().Pressure();

        // Get the particle-number contributions
        fvector props(6, 0);
        props[0] = sys.Particles().GetTotalDiameter();
        props[1] = sys.Particles().GetTotalDiameter2();
        props[2] = sys.Particles().GetTotalDiameter_1();
        props[3] = sys.Particles().GetTotalDiameter_2();
        props[4] = sys.Particles().GetTotalMass_1_2();
        props[5] = sys.Particles().GetTotalDiameter2_mass_1_2();

        // Calculate the rate.
        double rate = Rate(sys.Particles().GetSums(), (double)n, sqrt(T),
                    T/sys.GasPhase().Viscosity(), MeanFreePathAir(T,P),
                    sys.SampleVolume(), props);

        props.clear();
        fvector().swap(props);

        return rate;
    } else {
        return 0.0;
    }
}

// More efficient rate routine for coagulation only.
// All parameters required to calculate rate passed
// as arguments.
double Sweep::Processes::HybridTransitionCoagulation::Rate(const Ensemble::particle_cache_type &data, double n, double sqrtT,
                       double T_mu, double MFP, double vol, fvector & props) const
{
    // Some prerequisites.
    double n_1 = n - 1.0;
    double a = CSF * T_mu * A();
    double b = a * MFP * 1.257 * A();
    double c = CFMMAJ * m_efm * CFM * sqrtT * A();

    // Summed particle properties required for coagulation rate.
    double d       = data.Property(Sweep::iDcol);
    double d2      = data.Property(Sweep::iD2);
    double d_1     = data.Property(Sweep::iD_1);
    double d_2     = data.Property(Sweep::iD_2);
    double m_1_2   = data.Property(Sweep::iM_1_2);
    double d2m_1_2 = data.Property(Sweep::iD2_M_1_2);

    // Add particle-number contributions
    d += props[0];
    d2 += props[1];
    d_1 += props[2];
    d_2 += props[3];
    m_1_2 += props[4];
    d2m_1_2 += props[5];

    // Get individual terms.
    double terms[TYPE_COUNT];
    // Slip-flow.
    terms[0] = n * n_1 * a / vol;
    terms[1] = ((d * d_1) - n) * a / vol;
    terms[2] = d_1 * n_1 * b / vol;
    terms[3] = ((d * d_2) - d_1) * b / vol;
    // Free-molecular.
    terms[4] = n_1 * d2m_1_2  * c / vol;
    terms[5] = (m_1_2 * d2 - d2m_1_2) * c / vol;

    // Sum up total coagulation rates for different regimes.
    double sf = terms[0] + terms[1] + terms[2] + terms[3];
    double fm = terms[4] + terms[5];

    if ((sf>0.0) || (fm>0.0)) {
        // There is some coagulation.
        if (sf > fm) {
            // Use free-mol majorant.
            return fm;
        } else {
            // Use slip-flow majorant.
            return sf;
        }
    }
    return 0.0;
}

// RATE TERM CALCULATION.

// Returns the number of rate terms for this process.
unsigned int Sweep::Processes::HybridTransitionCoagulation::TermCount(void) const { return TYPE_COUNT; }

/**
 * Calculate the terms in the sum of the majorant kernel over all particle
 * pairs, placing each term in successive positions of the sequence
 * beginning at iterm and return the sum of the terms added to that
 * vector.
 *
 * @param[in]     t          Time for which rates are requested
 * @param[in]     sys        Details of the particle population and environment
 * @param[in]     local_geom Details of spatial position and boundaries
 * @param[in,out] iterm      Pointer to start of sequence to hold the rate terms, returned as one past the end.
 *
 * @return      Sum of all rate terms for this process
 */
double Sweep::Processes::HybridTransitionCoagulation::RateTerms(double t, const Cell &sys,
                            const Geometry::LocalGeometry1d &local_geom,
                            fvector::iterator &iterm) const
{
    // Get the number of particles in the system.
    unsigned int n = sys.ParticleCount() + sys.Particles().GetTotalParticleNumber();

    // Check that there are at least 2 particles before calculating rate.
    if (n > 1) {
        // Get system properties required to calculate coagulation rate.
        double T = sys.GasPhase().Temperature();
        double P = sys.GasPhase().Pressure();

        // Add contributions from particle-number list
        fvector props(6, 0);
        props[0] = sys.Particles().GetTotalDiameter();
        props[1] = sys.Particles().GetTotalDiameter2();
        props[2] = sys.Particles().GetTotalDiameter_1();
        props[3] = sys.Particles().GetTotalDiameter_2();
        props[4] = sys.Particles().GetTotalMass_1_2();
        props[5] = sys.Particles().GetTotalDiameter2_mass_1_2();

        // Calculate the rate terms.
        double rate = RateTerms(sys.Particles().GetSums(), (double)n, sqrt(T), T/sys.GasPhase().Viscosity(),
                         MeanFreePathAir(T,P), sys.SampleVolume(), iterm, props);
        props.clear();
        fvector().swap(props);
        return rate;
    } else {
        // No coagulation as there are too few particles.
        for(unsigned int i=0; i<TYPE_COUNT; i++) *(iterm++) = 0.0;
        return 0.0;
    }
}

// More efficient rate routine for coagulation only.
// All parameters required to calculate rate terms
// passed as arguments.
double Sweep::Processes::HybridTransitionCoagulation::RateTerms(const Ensemble::particle_cache_type &data, double n, double sqrtT,
                            double T_mu, double MFP, double vol,
                            fvector::iterator &iterm, fvector & props) const
{
    // Some prerequisites.
    double n_1 = n - 1.0;
    double a   = CSF * T_mu * A();
    double b   = a * MFP * 1.257 * 2.0;
    double c   = CFMMAJ * m_efm * CFM * sqrtT * A();

    // Summed particle properties required for coagulation rate.
    double d       = data.Property(Sweep::iDcol);
    double d2      = data.Property(Sweep::iD2);
    double d_1     = data.Property(Sweep::iD_1);
    double d_2     = data.Property(Sweep::iD_2);
    double m_1_2   = data.Property(Sweep::iM_1_2);
    double d2m_1_2 = data.Property(Sweep::iD2_M_1_2);

    // Add particle-number contributions
    d       += props[0];
    d2      += props[1];
    d_1     += props[2];
    d_2     += props[3];
    m_1_2   += props[4];
    d2m_1_2 += props[5];

    fvector::iterator isf = iterm;
    fvector::iterator ifm = iterm+4;

    // Slip-flow.
    *(iterm)   = n * n_1 * a / vol;
    *(++iterm) = ((d * d_1) - n) * a / vol;
    *(++iterm) = d_1 * n_1 * b / vol;
    *(++iterm) = ((d * d_2) - d_1) * b / vol;
    // Free-molecular.
    *(++iterm) = n_1 * d2m_1_2  * c / vol;
    *(++iterm) = (m_1_2 * d2 - d2m_1_2) * c / vol;

    // Return iterator to next term after the coagulation terms.
    ++iterm;

    // Sum up total coagulation rates for different regimes.
    double sf = *(isf) + *(isf+1) + *(isf+2) + *(isf+3);
    double fm = *(ifm) + *(ifm+1);

    if ((sf>0.0) || (fm>0.0)) {
        // There is some coagulation.
        if (sf > fm) {
            // Use free-mol majorant.
            *(isf) = 0.0;
            *(isf+1) = 0.0;
            *(isf+2) = 0.0;
            *(isf+3) = 0.0;
            return fm;
        } else {
            // Use slip-flow majorant.
            *(ifm) = 0.0;
            *(ifm+1) = 0.0;
            return sf;
        }
    } else {
        // Something went wrong with the rate calculation.
        *(isf)   = 0.0;
        *(isf+1) = 0.0;
        *(isf+2) = 0.0;
        *(isf+3) = 0.0;
        *(ifm)   = 0.0;
        *(ifm+1) = 0.0;
        return 0.0;
    }
}

// PERFORMING THE PROCESS.

/*!
 *
 *
 * \param[in]       t           Time
 * \param[in,out]   sys         System to update
 * \param[in]       local_geom  Details of local phsyical layout
 * \param[in]       iterm       Process term responsible for this event
 * \param[in,out]   rng         Random number generator
 *
 * \return      0 on success, otherwise negative.
 */
int HybridTransitionCoagulation::Perform(double t, Sweep::Cell &sys,
                                   const Geometry::LocalGeometry1d& local_geom,
                                   unsigned int iterm,
                                   Sweep::rng_type &rng) const
{
	PartPtrVector dummy;

    int ip1=-1, ip2=-1;
    MajorantType maj;
    TermType term = (TermType)iterm;

    // Store number of particles in each particle model
    double n_incep = sys.Particles().GetTotalParticleNumber();
    double n_other = sys.ParticleCount();
    double n_total = n_incep + n_other;
    unsigned int index1 = 0, index2 = 0, n_index1 = 0;

    // Hybrid particle model flags
    bool hybrid_flag = m_mech->IsHybrid() && n_incep > 0.0; // Check if particle-number list is relevant
    bool ip1_flag = false;      // Flag particle 1 as a particle-number particle
    bool ip2_flag = false;      // Flag particle 2 as a particle-number particle
    bool coag_in_place = false; // Flag for coagulation in particle-number list

    // Choose and get first particle.
    Particle *sp1 = NULL;
    Particle *sp2 = NULL;

    // Hybrid model: if < 2 SPs but incepting class >= 2, we can still act
    if (n_total < 2)
        return 1;
    else
    {
        // Select properties by which to choose particles.
        // Note we need to choose 2 particles.  There are six possible
        // rate terms to choose from; 4 slip-flow and 2 free molecular.
        boost::uniform_01<rng_type&, double> unifDistrib(rng);

        // Get property sums for number list
        double dc_incep = sys.Particles().GetTotalDiameter();
        double dc2_incep = sys.Particles().GetTotalDiameter2();
        double dc_1_incep = sys.Particles().GetTotalDiameter_1();
        double dc_2_incep = sys.Particles().GetTotalDiameter_2();
        double m_1_2_incep = sys.Particles().GetTotalMass_1_2();
        double dc2_m_1_2_incep = sys.Particles().GetTotalDiameter2_mass_1_2();

        // Get property sums for ensemble
        double dc_other = sys.Particles().GetSum(iDcol);
        double dc2_other = sys.Particles().GetSum(iD2);
        double dc_1_other = sys.Particles().GetSum(iD_1);
        double dc_2_other = sys.Particles().GetSum(iD_2);
        double m_1_2_other = sys.Particles().GetSum(iM_1_2);
        double dc2_m_1_2_other = sys.Particles().GetSum(iD2_M_1_2);

        // Select the first particle and note the majorant type
        double alpha1 = 0.0, alpha2 = 0.0;
        switch (term) {
            case SlipFlow1:
                alpha1 = unifDistrib() * n_total;
                if (alpha1 <= n_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iUniform, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iUniform, alpha1 - n_incep, rng);
                }
                maj = SlipFlow;
                break;
            case SlipFlow2:
                alpha1 = unifDistrib() * (dc_incep + dc_other);
                if (alpha1 <= dc_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iDcol, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iDcol, alpha1 - dc_incep, rng);
                }
                maj = SlipFlow;
                break;
            case SlipFlow3:
                alpha1 = unifDistrib() * n_total;
                if (alpha1 <= n_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iUniform, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iUniform, alpha1 - n_incep, rng);
                }
                maj = SlipFlow;
                break;
            case SlipFlow4:
                alpha1 = unifDistrib() * (dc_incep + dc_other);
                if (alpha1 <= dc_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iDcol, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iDcol, alpha1 - dc_incep, rng);
                }
                maj = SlipFlow;
                break;
            case FreeMol1:
                alpha1 = unifDistrib() * n_total;
                if (alpha1 <= n_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iUniform, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iUniform, alpha1 - n_incep, rng);
                }
                maj = FreeMol;
                break;
            case FreeMol2:
                alpha1 = unifDistrib() * (dc2_incep + dc2_other);
                if (alpha1 <= dc2_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iD2, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iD2, alpha1 - dc2_incep, rng);
                }
                maj = FreeMol;
                break;
            default:
                alpha1 = unifDistrib() * n_total;
                if (alpha1 <= n_incep)
                {
                    index1 = m_mech->SetRandomParticle(sys.Particles(), t, alpha1, iUniform, rng);
                    ip1 = -2;
                } else {
                    ip1 = sys.Particles().Select_usingGivenRand(iUniform, alpha1 - n_incep, rng);
                }
                maj = SlipFlow;
                break;
        }

        // Is this a particle-number particle?
        if (ip1 == -2)
        {
            if (index1 == 0)
            {
		// Property sums update triggered - round-off was too severe and
                // no suitable particle could be found. The easiest option
                // is not to perform this coagulation event.
		return -1;
            }
	    n_index1 = sys.Particles().NumberAtIndex(index1);
            // Flag sp1 as a particle-number particle
            ip1_flag = true;
            // Clone a template particle of the correct size for sp1
            sp1 = sys.Particles().GetPNParticleAt(index1)->Clone();
            sp1->SetTime(t);
        } else {
            // An ensemble particle
            if (ip1 >= 0) {
                sp1 = sys.Particles().At(ip1);
            } else {
            // Failed to choose a particle.
            return -1;
            }
        }

        // Choose and get unique second particle.  Note, we are allowed to do
        // this even if the first particle was invalidated.
        ip2 = ip1;
        index2 = index1;
        unsigned int guard = 0;
        bool mustSwitch = (ip1 == -2) && (n_incep == 1); // sp1 was the only list particle
        bool unsuitableChoice = true;

        switch (term) {
           case SlipFlow1:
               while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * n_total;
                    if (alpha2 <= n_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iUniform, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iUniform, alpha2 - n_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            case SlipFlow2:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * (dc_1_incep + dc_1_other);
                    if (alpha2 <= dc_1_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iD_1, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iD_1, alpha2 - dc_1_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            case SlipFlow3:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * (dc_1_incep + dc_1_other);
                    if (alpha2 <= dc_1_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iD_1, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iD_1, alpha2 - dc_1_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            case SlipFlow4:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * (dc_2_incep + dc_2_other);
                    if (alpha2 <= dc_2_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iD_2, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iD_2, alpha2 - dc_2_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            case FreeMol1:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * (dc2_m_1_2_incep + dc2_m_1_2_other);
                    if (alpha2 <= dc2_m_1_2_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iD2_M_1_2, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iD2_M_1_2, alpha2 - dc2_m_1_2_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            case FreeMol2:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * (m_1_2_incep + m_1_2_other);
                    if (alpha2 <= m_1_2_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iM_1_2, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iM_1_2, alpha2 - m_1_2_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                        }
                }
                break;
            default:
                while (unsuitableChoice && (++guard < 1000))
                {
                    alpha2 = unifDistrib() * n_total;
                    if (alpha2 <= n_incep && !mustSwitch)
                    {
                        index2 = m_mech->SetRandomParticle(sys.Particles(), t, alpha2, iUniform, rng);
                        if (!(index2 == index1 && n_index1 == 1))
                        {
                            unsuitableChoice = false;
                            ip2 = -2;
                        }
                    } else {
                        ip2 = sys.Particles().Select_usingGivenRand(iUniform, alpha2 - n_incep, rng);
                        if (!(ip2 == ip1))
                            unsuitableChoice = false;
                    }
                }
                break;
            }
        }

        // Is this a particle-number particle?
        if (ip2 == -2)
        {
            // Note this could be factored into the loops above
            // but that would require constantly checking for something
            // that should very infrequently be a problem.
            // Easiest option is to exit without performing the coagulation event.
            if (index2 == 0)
            {
                if (ip1_flag)
                {
                    delete sp1;
                    sp1 = NULL;
                }
                // Property sums updated
                return -1;
            }
            ip2_flag = true; // Flag sp2 as a particle-number particle
            sp2 = sys.Particles().GetPNParticleAt(index2)->Clone();
            sp2->SetTime(t);
        }
        else
        {
            // An ensemble particle
            if ((ip2 >= 0) && (ip2 != ip1)) {
                sp2 = sys.Particles().At(ip2);
            } else {
                // Failed to select a unique particle.
                return -1;
            }
        }

        //Calculate the majorant rate before updating the particles
        double majk = MajorantKernel(*sp1, *sp2, sys, maj);

        //Update the particles
        if (t > sp1->LastUpdateTime())
            m_mech->UpdateParticle(*sp1, sys, t, ip1, rng, dummy);
        // Check that particle is still valid.  If not,
        // remove it and cease coagulating.
        if (!sp1->IsValid()) {
            if (!ip1_flag) {
                // Must remove first particle now.
                sys.Particles().Remove(ip1);
            } else {
                delete sp1;
                sp1 = NULL;
            }
            // Invalidating the index tells this routine not to perform coagulation.
            ip1 = -1;
            return 0;
        }

        if (t > sp2->LastUpdateTime())
            m_mech->UpdateParticle(*sp2, sys, t, ip2, rng, dummy);
        // Check validity of particles after update.
        if (!sp2->IsValid()) {
            // Tell the ensemble to update particle one before we confuse things
            // by removing particle 2
            if (!ip1_flag)
                sys.Particles().Update(ip1);
            // Must remove second particle now.
            if (!ip2_flag)
                sys.Particles().Remove(ip2);
            else
            {
                // Particle sp2 is not in the ensemble, must manually delete it
                delete sp2;
                sp2 = NULL;
            }
            // Invalidating the index tells this routine not to perform coagulation.
            ip2 = -1;
            return 0;
        }

        // Check that both the particles are still valid.
        if ((ip1 != -1) && (ip2 != -1))
        {
            // Must check for ficticious event now by comparing the original
            // majorant rate and the current (after updates) true rate.
            double truek = CoagKernel(*sp1, *sp2, sys);
            double ceff = 0;
            if (majk<truek)
                std::cout << "maj< true" << std::endl;

            //added by ms785 to include the collision efficiency in the calculation of the rate
            if (sys.ParticleModel()->AggModel() == AggModels::PAH_KMC_ID)
            {
                ceff = sys.ParticleModel()->CollisionEff(sp1, sp2);
                truek *= ceff;
            }

            //! For the purpose of checking consistency with the spherical soot
            //! model solved using the method of moments with interpolative closure
            //! which assumes that only pyrene (A4) is able to incept and condense.
            else if (sys.ParticleModel()->AggModel() == AggModels::BinTree_ID || sys.ParticleModel()->AggModel() == AggModels::Spherical_ID) {
                if (sp1->Primary()->InceptedPAH() && sp2->Primary()->InceptedPAH()) {
                    ceff = 1;
                }
                else if (sp1->Primary()->InceptedPAH() && sp2->NumCarbon() > 16 || sp1->NumCarbon() > 16 && sp2->Primary()->InceptedPAH()) {
                    ceff = 1;
                } else {
                    ceff = 1;
                }
                truek *= ceff;
            }

            if (!Fictitious(majk, truek, rng)) {
                if (ip1_flag)
                {
                    // We are removing the particle from the PN model and
                    // adding it to the ensemble
                    sys.Particles().UpdateTotalsWithIndex(index1, -1.0);
                    sys.Particles().UpdateNumberAtIndex(index1, -1);
                    sys.Particles().UpdateTotalParticleNumber(-1);
                    unsigned int index12 = index1 + index2;
                    // Allow for coagulation in place if the combined particle is small enough
                    if ((m_mech->CoagulateInList()) && ip2_flag && (index12 < sys.Particles().GetHybridThreshold()))
                    {
                        coag_in_place = true;
                        sys.Particles().UpdateTotalsWithIndex(index12, 1.0);
                        sys.Particles().UpdateNumberAtIndex(index12, 1);
                        sys.Particles().UpdateTotalParticleNumber(1);
                        if (sp1 != NULL)
                        {
                            delete sp1;
                            sp1 = NULL;
                        }
                    } else
                        ip1 = sys.Particles().Add(*sp1, rng, ip2, true);
                }
                if (ip2_flag)
                {
                    // We are removing the particle from the PN model and
                    // adding it to the ensemble
                    sys.Particles().UpdateTotalsWithIndex(index2, -1.0);
                    sys.Particles().UpdateNumberAtIndex(index2, -1);
                    sys.Particles().UpdateTotalParticleNumber(-1);
                }
                if (!coag_in_place)
                    JoinParticles(t, ip1, sp1, ip2, sp2, sys, rng);

                if (ip2_flag && sp2 != NULL)                                                     // Particle sp2 is not in the ensemble, must manually delete it
                {
                    delete sp2;
                    sp2 = NULL;
                }
            } else {
                if (!ip1_flag)
                    sys.Particles().Update(ip1);
                else if (sp1 != NULL)                                                            // Particle sp1 is not in the ensemble, must manually delete it
                {
                    delete sp1;
                    sp1 = NULL;
                }
                if (!ip2_flag)
                    sys.Particles().Update(ip2);
                else if (sp2 != NULL)                                                            // Particle sp2 is not in the ensemble, must manually delete it
                {
                    delete sp2;
                    sp2 = NULL;
                }
                return 1; // Ficticious event.
            }
        } else {
            // One or both particles were invalidated on update,
            // but that's not a problem.  Information on the update
            // of valid particles must be propagated into the binary
            // tree
            if (ip1 != -1)
                if (!ip1_flag)
                    sys.Particles().Update(ip1);
            if (ip2 != -1)
            {
                if (!ip2_flag)
                    sys.Particles().Update(ip2);
            }
            if (ip1_flag && sp1 != NULL)                                                         // Particle sp2 is not in the ensemble, must manually delete it
            {
                delete sp1;
                sp1 = NULL;
            }
            if (ip2_flag && sp2 != NULL)                                                         // Particle sp2 is not in the ensemble, must manually delete it
            {
                delete sp2;
                sp2 = NULL;
            }
    	}
    return 0;
}

// COAGULATION KERNELS.

/**
 * Calculate the coagulation kernel between two particles in the given environment.
 *
 *@param[in]    sp1         First particle
 *@param[in]    sp2         Second particle
 *@param[in]    sys         Details of the environment, including temperature and pressure
 *
 *@return       Value of kernel
 */
double Sweep::Processes::HybridTransitionCoagulation::CoagKernel(const Particle &sp1,
                                                                const Particle &sp2,
                                                                const Cell &sys) const
{
    const double T = sys.GasPhase().Temperature();
    const double P = sys.GasPhase().Pressure();
    const double fm = FreeMolKernel(sp1, sp2, T, P, false);
    const double sf = SlipFlowKernel(sp1, sp2, T, P, sys.GasPhase().Viscosity(), false);
    return (fm*sf)/(fm+sf);
}

/**
 * Calculate the majorant kernel between two particles in the given environment.
 *
 *@param[in]    sp1         First particle
 *@param[in]    sp2         Second particle
 *@param[in]    sys         Details of the environment, including temperature and pressure
 *@param[in]    maj         Flag to indicate which majorant kernel is required
 *
 *@return       Value of kernel
 */
double Sweep::Processes::HybridTransitionCoagulation::MajorantKernel(const Particle &sp1,
                                                                    const Particle &sp2,
                                                                    const Cell &sys,
                                                                    const MajorantType maj) const
{
    // This routine calculates the coagulation kernel for two particles.  The kernel
    // type is chosen by the majorant type requested.
    switch (maj) {
        case Default:
            // This should never happen for the transition coagulation kernel
            assert(maj != Default);
            break;
        case FreeMol:
            // Free molecular majorant.
            return FreeMolKernel(sp1, sp2, sys.GasPhase().Temperature(),
                    sys.GasPhase().Pressure(), true);
        case SlipFlow:
            // Slip-flow majorant.
            return SlipFlowKernel(sp1, sp2, sys.GasPhase().Temperature(),
                    sys.GasPhase().Pressure(), sys.GasPhase().Viscosity(), true);
    }

    // Invalid majorant, return zero.
    return 0.0;
}
// Returns the free-molecular coagulation kernel value for the
// two given particles.  Can return either the majorant or
// true kernel.
double Sweep::Processes::HybridTransitionCoagulation::FreeMolKernel(const Particle &sp1, const Particle &sp2,
                                double T, double P, bool maj) const
{
    // This routine calculate the free molecular coagulation kernel for two particles.
    // There are two forms of kernel; a majorant form and a non-majorant form.

    // Collect the particle properties
    const double d1 = sp1.CollDiameter();
    const double d2 = sp2.CollDiameter();
    const double invm1 = 1.0 / sp1.Mass();
    const double invm2 = 1.0 / sp2.Mass();

    if (maj) {
        // The majorant form is always >= the non-majorant form.
        return CFMMAJ * m_efm * CFM * sqrt(T) * A() *
               (std::sqrt(invm1) + std::sqrt(invm2)) *
               (d1 * d1 + d2 * d2);
    } else {
        const double dterm = d1 + d2;
        return m_efm * CFM * A() *
               sqrt(T * (invm1 + invm2)) *
               dterm * dterm;
    }
}

// Returns the slip-flow coagulation kernel value for the
// two given particles.  Can return either the majorant or
// true kernel.
double Sweep::Processes::HybridTransitionCoagulation::SlipFlowKernel(const Particle &sp1, const Particle &sp2,
                                 double T, double P, double mu, bool maj) const
{
    // Collect the particle properties
    const double d1 = sp1.CollDiameter();
    const double d2 = sp2.CollDiameter();

    // For the slip-flow kernel the majorant and non-majorant forms are identical.
    return ((1.257 * 2.0 * MeanFreePathAir(T,P) *
             (1.0 / d1 / d1 + 1.0 / d2 / d2)) +
            (1.0 / d1 + 1.0 / d2)) *
           CSF * T * (d1 + d2)
           * A() / mu;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MP_GENERIC_INTERCONVERT_HPP
#define BOOST_MP_GENERIC_INTERCONVERT_HPP

#include <boost/multiprecision/detail/default_ops.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127 6326)
#endif

namespace boost{ namespace multiprecision{ namespace detail{

template <class To, class From>
inline To do_cast(const From & from)
{
   return static_cast<To>(from);
}
template <class To, class B, ::boost::multiprecision::expression_template_option et>
inline To do_cast(const number<B, et>& from)
{
   return from.template convert_to<To>();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   using default_ops::eval_get_sign;
   using default_ops::eval_bitwise_and;
   using default_ops::eval_convert_to;
   using default_ops::eval_right_shift;
   using default_ops::eval_ldexp;
   using default_ops::eval_add;
   using default_ops::eval_is_zero;
   // smallest unsigned type handled natively by "From" is likely to be it's limb_type:
   typedef typename canonical<unsigned char, From>::type   l_limb_type;
   // get the corresponding type that we can assign to "To":
   typedef typename canonical<l_limb_type, To>::type         to_type;
   From t(from);
   bool is_neg = eval_get_sign(t) < 0;
   if(is_neg)
      t.negate();
   // Pick off the first limb:
   l_limb_type limb;
   l_limb_type mask = static_cast<l_limb_type>(~static_cast<l_limb_type>(0));
   From fl;
   eval_bitwise_and(fl, t, mask);
   eval_convert_to(&limb, fl);
   to = static_cast<to_type>(limb);
   eval_right_shift(t, std::numeric_limits<l_limb_type>::digits);
   //
   // Then keep picking off more limbs until "t" is zero:
   //
   To l;
   unsigned shift = std::numeric_limits<l_limb_type>::digits;
   while(!eval_is_zero(t))
   {
      eval_bitwise_and(fl, t, mask);
      eval_convert_to(&limb, fl);
      l = static_cast<to_type>(limb);
      eval_right_shift(t, std::numeric_limits<l_limb_type>::digits);
      eval_ldexp(l, l, shift);
      eval_add(to, l);
      shift += std::numeric_limits<l_limb_type>::digits;
   }
   //
   // Finish off by setting the sign:
   //
   if(is_neg)
      to.negate();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_integer>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   using default_ops::eval_get_sign;
   using default_ops::eval_bitwise_and;
   using default_ops::eval_convert_to;
   using default_ops::eval_right_shift;
   using default_ops::eval_left_shift;
   using default_ops::eval_bitwise_or;
   using default_ops::eval_is_zero;
   // smallest unsigned type handled natively by "From" is likely to be it's limb_type:
   typedef typename canonical<unsigned char, From>::type   limb_type;
   // get the corresponding type that we can assign to "To":
   typedef typename canonical<limb_type, To>::type         to_type;
   From t(from);
   bool is_neg = eval_get_sign(t) < 0;
   if(is_neg)
      t.negate();
   // Pick off the first limb:
   limb_type limb;
   limb_type mask = static_cast<limb_type>(~static_cast<limb_type>(0));
   From fl;
   eval_bitwise_and(fl, t, mask);
   eval_convert_to(&limb, fl);
   to = static_cast<to_type>(limb);
   eval_right_shift(t, std::numeric_limits<limb_type>::digits);
   //
   // Then keep picking off more limbs until "t" is zero:
   //
   To l;
   unsigned shift = std::numeric_limits<limb_type>::digits;
   while(!eval_is_zero(t))
   {
      eval_bitwise_and(fl, t, mask);
      eval_convert_to(&limb, fl);
      l = static_cast<to_type>(limb);
      eval_right_shift(t, std::numeric_limits<limb_type>::digits);
      eval_left_shift(l, shift);
      eval_bitwise_or(to, l);
      shift += std::numeric_limits<limb_type>::digits;
   }
   //
   // Finish off by setting the sign:
   //
   if(is_neg)
      to.negate();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_floating_point>& /*from_type*/)
{
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
   //
   // The code here only works when the radix of "From" is 2, we could try shifting by other
   // radixes but it would complicate things.... use a string conversion when the radix is other
   // than 2:
   //
   if(std::numeric_limits<number<From> >::radix != 2)
   {
      to = from.str(0, std::ios_base::fmtflags()).c_str();
      return;
   }

   typedef typename canonical<unsigned char, To>::type ui_type;

   using default_ops::eval_fpclassify;
   using default_ops::eval_add;
   using default_ops::eval_subtract;
   using default_ops::eval_convert_to;
   using default_ops::eval_get_sign;
   using default_ops::eval_is_zero;

   //
   // First classify the input, then handle the special cases:
   //
   int c = eval_fpclassify(from);

   if(c == (int)FP_ZERO)
   {
      to = ui_type(0);
      return;
   }
   else if(c == (int)FP_NAN)
   {
      to = static_cast<const char*>("nan");
      return;
   }
   else if(c == (int)FP_INFINITE)
   {
      to = static_cast<const char*>("inf");
      if(eval_get_sign(from) < 0)
         to.negate();
      return;
   }

   typename From::exponent_type e;
   From f, term;
   to = ui_type(0);

   eval_frexp(f, from, &e);

   static const int shift = std::numeric_limits<boost::intmax_t>::digits - 1;

   while(!eval_is_zero(f))
   {
      // extract int sized bits from f:
      eval_ldexp(f, f, shift);
      eval_floor(term, f);
      e -= shift;
      eval_ldexp(to, to, shift);
      typename boost::multiprecision::detail::canonical<boost::intmax_t, To>::type ll;
      eval_convert_to(&ll, term);
      eval_add(to, ll);
      eval_subtract(f, term);
   }
   typedef typename To::exponent_type to_exponent;
   if((e > (std::numeric_limits<to_exponent>::max)()) || (e < (std::numeric_limits<to_exponent>::min)()))
   {
      to = static_cast<const char*>("inf");
      if(eval_get_sign(from) < 0)
         to.negate();
      return;
   }
   eval_ldexp(to, to, static_cast<to_exponent>(e));
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_rational>& /*to_type*/, const mpl::int_<number_kind_rational>& /*from_type*/)
{
   typedef typename component_type<number<To> >::type     to_component_type;

   number<From> t(from);
   to_component_type n(numerator(t)), d(denominator(t));
   using default_ops::assign_components;
   assign_components(to, n.backend(), d.backend());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_rational>& /*to_type*/, const mpl::int_<number_kind_integer>& /*from_type*/)
{
   typedef typename component_type<number<To> >::type     to_component_type;

   number<From> t(from);
   to_component_type n(t), d(1);
   using default_ops::assign_components;
   assign_components(to, n.backend(), d.backend());
}

template <class R, class LargeInteger>
R safe_convert_to_float(const LargeInteger& i)
{
   using std::ldexp;
   if(!i)
      return R(0);
   if(std::numeric_limits<R>::is_specialized && std::numeric_limits<R>::max_exponent)
   {
      LargeInteger val(i);
      if(val.sign() < 0)
         val = -val;
      unsigned mb = msb(val);
      if(mb >= std::numeric_limits<R>::max_exponent)
      {
         int scale_factor = (int)mb + 1 - std::numeric_limits<R>::max_exponent;
         BOOST_ASSERT(scale_factor >= 1);
         val >>= scale_factor;
         R result = val.template convert_to<R>();
         if(std::numeric_limits<R>::digits == 0 || std::numeric_limits<R>::digits >= std::numeric_limits<R>::max_exponent)
         {
            //
            // Calculate and add on the remainder, only if there are more
            // digits in the mantissa that the size of the exponent, in
            // other words if we are dropping digits in the conversion
            // otherwise:
            //
            LargeInteger remainder(i);
            remainder &= (LargeInteger(1) << scale_factor) - 1;
            result += ldexp(safe_convert_to_float<R>(remainder), -scale_factor);
         }
         return i.sign() < 0 ? static_cast<R>(-result) : result;
      }
   }
   return i.template convert_to<R>();
}

template <class To, class Integer>
inline typename disable_if_c<is_number<To>::value || is_floating_point<To>::value>::type
   generic_convert_rational_to_float_imp(To& result, const Integer& n, const Integer& d, const mpl::true_&)
{
   //
   // If we get here, then there's something about one type or the other
   // that prevents an exactly rounded result from being calculated
   // (or at least it's not clear how to implement such a thing).
   //
   using default_ops::eval_divide;
   number<To> fn(safe_convert_to_float<number<To> >(n)), fd(safe_convert_to_float<number<To> >(d));
   eval_divide(result, fn.backend(), fd.backend());
}
template <class To, class Integer>
inline typename enable_if_c<is_number<To>::value || is_floating_point<To>::value>::type
   generic_convert_rational_to_float_imp(To& result, const Integer& n, const Integer& d, const mpl::true_&)
{
   //
   // If we get here, then there's something about one type or the other
   // that prevents an exactly rounded result from being calculated
   // (or at least it's not clear how to implement such a thing).
   //
   To fd(safe_convert_to_float<To>(d));
   result = safe_convert_to_float<To>(n);
   result /= fd;
}

template <class To, class Integer>
typename enable_if_c<is_number<To>::value || is_floating_point<To>::value>::type
   generic_convert_rational_to_float_imp(To& result, Integer& num, Integer& denom, const mpl::false_&)
{
   //
   // If we get here, then the precision of type To is known, and the integer type is unbounded
   // so we can use integer division plus manipulation of the remainder to get an exactly
   // rounded result.
   //
   if(num == 0)
   {
      result = 0;
      return;
   }
   bool s = false;
   if(num < 0)
   {
      s = true;
      num = -num;
   }
   int denom_bits = msb(denom);
   int shift = std::numeric_limits<To>::digits + denom_bits - msb(num);
   if(shift > 0)
      num <<= shift;
   else if(shift < 0)
      denom <<= boost::multiprecision::detail::unsigned_abs(shift);
   Integer q, r;
   divide_qr(num, denom, q, r);
   int q_bits = msb(q);
   if(q_bits == std::numeric_limits<To>::digits - 1)
   {
      //
      // Round up if 2 * r > denom:
      //
      r <<= 1;
      int c = r.compare(denom);
      if(c > 0)
         ++q;
      else if((c == 0) && (q & 1u))
      {
         ++q;
      }
   }
   else
   {
      BOOST_ASSERT(q_bits == std::numeric_limits<To>::digits);
      //
      // We basically already have the rounding info:
      //
      if(q & 1u)
      {
         if(r || (q & 2u))
            ++q;
      }
   }
   using std::ldexp;
   result = do_cast<To>(q);
   result = ldexp(result, -shift);
   if(s)
      result = -result;
}
template <class To, class Integer>
inline typename disable_if_c<is_number<To>::value || is_floating_point<To>::value>::type
   generic_convert_rational_to_float_imp(To& result, Integer& num, Integer& denom, const mpl::false_& tag)
{
   number<To> t;
   generic_convert_rational_to_float_imp(t, num, denom, tag);
   result = t.backend();
}

template <class To, class From>
inline void generic_convert_rational_to_float(To& result, const From& f)
{
   //
   // Type From is always a Backend to number<>, or an
   // instance of number<>, but we allow
   // To to be either a Backend type, or a real number type,
   // that way we can call this from generic conversions, and
   // from specific conversions to built in types.
   //
   typedef typename mpl::if_c<is_number<From>::value, From, number<From> >::type actual_from_type;
   typedef typename mpl::if_c<is_number<To>::value || is_floating_point<To>::value, To, number<To> >::type actual_to_type;
   typedef typename component_type<actual_from_type>::type integer_type;
   typedef mpl::bool_<!std::numeric_limits<integer_type>::is_specialized
                      || std::numeric_limits<integer_type>::is_bounded
                      || !std::numeric_limits<actual_to_type>::is_specialized
                      || !std::numeric_limits<actual_to_type>::is_bounded
                      || (std::numeric_limits<actual_to_type>::radix != 2)> dispatch_tag;

   integer_type n(numerator(static_cast<actual_from_type>(f))), d(denominator(static_cast<actual_from_type>(f)));
   generic_convert_rational_to_float_imp(result, n, d, dispatch_tag());
}

template <class To, class From>
inline void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_rational>& /*from_type*/)
{
   generic_convert_rational_to_float(to, from);
}

template <class To, class From>
void generic_interconvert_float2rational(To& to, const From& from, const mpl::int_<2>& /*radix*/)
{
   typedef typename mpl::front<typename To::unsigned_types>::type ui_type;
   static const int shift = std::numeric_limits<boost::long_long_type>::digits;
   typename From::exponent_type e;
   typename component_type<number<To> >::type num, denom;
   number<From> val(from);
   val = frexp(val, &e);
   while(val)
   {
      val = ldexp(val, shift);
      e -= shift;
      boost::long_long_type ll = boost::math::lltrunc(val);
      val -= ll;
      num <<= shift;
      num += ll;
   }
   denom = ui_type(1u);
   if(e < 0)
      denom <<= -e;
   else if(e > 0)
      num <<= e;
   assign_components(to, num.backend(), denom.backend());
}

template <class To, class From, int Radix>
void generic_interconvert_float2rational(To& to, const From& from, const mpl::int_<Radix>& /*radix*/)
{
   //
   // This is almost the same as the binary case above, but we have to use
   // scalbn and ilogb rather than ldexp and frexp, we also only extract
   // one Radix digit at a time which is terribly inefficient!
   //
   typedef typename mpl::front<typename To::unsigned_types>::type ui_type;
   typename From::exponent_type e;
   typename component_type<number<To> >::type num, denom;
   number<From> val(from);

   if (!val)
   {
      to = ui_type(0u);
      return;
   }

   e = ilogb(val);
   val = scalbn(val, -e);
   while(val)
   {
      boost::long_long_type ll = boost::math::lltrunc(val);
      val -= ll;
      val = scalbn(val, 1);
      num *= Radix;
      num += ll;
      --e;
   }
   ++e;
   denom = ui_type(Radix);
   denom = pow(denom, abs(e));
   if(e > 0)
   {
      num *= denom;
      denom = 1;
   }
   assign_components(to, num.backend(), denom.backend());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_rational>& /*to_type*/, const mpl::int_<number_kind_floating_point>& /*from_type*/)
{
   generic_interconvert_float2rational(to, from, mpl::int_<std::numeric_limits<number<From> >::radix>());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_integer>& /*to_type*/, const mpl::int_<number_kind_rational>& /*from_type*/)
{
   number<From> t(from);
   number<To> result(numerator(t) / denominator(t));
   to = result.backend();
}

template <class To, class From>
void generic_interconvert_float2int(To& to, const From& from, const mpl::int_<2>& /*radix*/)
{
   typedef typename From::exponent_type exponent_type;
   static const exponent_type shift = std::numeric_limits<boost::long_long_type>::digits;
   exponent_type e;
   number<To>   num(0u);
   number<From> val(from);
   val = frexp(val, &e);
   while(e > 0)
   {
      int s = (std::min)(e, shift);
      val = ldexp(val, s);
      e -= s;
      boost::long_long_type ll = boost::math::lltrunc(val);
      val -= ll;
      num <<= s;
      num += ll;
   }
   to = num.backend();
}

template <class To, class From, int Radix>
void generic_interconvert_float2int(To& to, const From& from, const mpl::int_<Radix>& /*radix*/)
{
   //
   // This is almost the same as the binary case above, but we have to use
   // scalbn and ilogb rather than ldexp and frexp, we also only extract
   // one Radix digit at a time which is terribly inefficient!
   //
   typename From::exponent_type e;
   number<To> num(0u);
   number<From> val(from);
   e = ilogb(val);
   val = scalbn(val, -e);
   while(e >= 0)
   {
      boost::long_long_type ll = boost::math::lltrunc(val);
      val -= ll;
      val = scalbn(val, 1);
      num *= Radix;
      num += ll;
      --e;
   }
   to = num.backend();
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_integer>& /*to_type*/, const mpl::int_<number_kind_floating_point>& /*from_type*/)
{
   generic_interconvert_float2int(to, from, mpl::int_<std::numeric_limits<number<From> >::radix>());
}

template <class To, class From, class tag>
void generic_interconvert_complex_to_scalar(To& to, const From& from, const mpl::true_&, const tag&)
{
   // We just want the real part, and "to" is the correct type already:
   eval_real(to, from);

   To im;
   eval_imag(im, from);
   if(!eval_is_zero(im))
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
}
template <class To, class From>
void generic_interconvert_complex_to_scalar(To& to, const From& from, const mpl::false_&, const mpl::true_&)
{
   typedef typename component_type<number<From> >::type component_number;
   typedef typename component_number::backend_type component_backend;
   //
   // Get the real part and copy-construct the result from it:
   //
   component_backend r;
   generic_interconvert_complex_to_scalar(r, from, mpl::true_(), mpl::true_());
   to = r;
}
template <class To, class From>
void generic_interconvert_complex_to_scalar(To& to, const From& from, const mpl::false_&, const mpl::false_&)
{
   typedef typename component_type<number<From> >::type component_number;
   typedef typename component_number::backend_type component_backend;
   //
   // Get the real part and use a generic_interconvert to type To:
   //
   component_backend r;
   generic_interconvert_complex_to_scalar(r, from, mpl::true_(), mpl::true_());
   generic_interconvert(to, r, mpl::int_<number_category<To>::value>(), mpl::int_<number_category<To>::value>());
}

template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_floating_point>& /*to_type*/, const mpl::int_<number_kind_complex>& /*from_type*/)
{
   typedef typename component_type<number<From> >::type component_number;
   typedef typename component_number::backend_type component_backend;

   generic_interconvert_complex_to_scalar(to, from, mpl::bool_<boost::is_same<component_backend, To>::value>(), mpl::bool_<boost::is_constructible<To, const component_backend&>::value>());
}
template <class To, class From>
void generic_interconvert(To& to, const From& from, const mpl::int_<number_kind_integer>& /*to_type*/, const mpl::int_<number_kind_complex>& /*from_type*/)
{
   typedef typename component_type<number<From> >::type component_number;
   typedef typename component_number::backend_type component_backend;

   generic_interconvert_complex_to_scalar(to, from, mpl::bool_<boost::is_same<component_backend, To>::value>(), mpl::bool_<boost::is_constructible<To, const component_backend&>::value>());
}

}
}
} // namespaces

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif  // BOOST_MP_GENERIC_INTERCONVERT_HPP


/// @file DeconvolverHogbom.tcc
/// @brief Class for a deconvolver based on the Hogbom CLEAN
/// @details This concrete class defines a deconvolver used to estimate an
/// image from a dirty image, psf optionally using a mask and a weights image.
/// @ingroup Deconvolver
///
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution is free software: you can redistribute it
/// and/or modify it under the terms of the GNU General Public License as
/// published by the Free Software Foundation; either version 2 of the License,
/// or (at your option) any later version.
///
/// This program is distributed in the hope that it will be useful,
/// but WITHOUT ANY WARRANTY; without even the implied warranty of
/// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
/// GNU General Public License for more details.
///
/// You should have received a copy of the GNU General Public License
/// along with this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
///

#include <string>

#include <casacore/casa/aips.h>
#include <boost/shared_ptr.hpp>
#include <casacore/casa/Arrays/Array.h>
#include <askap/AskapLogging.h>
ASKAP_LOGGER(dechogbomlogger, ".deconvolution.hogbom");

#include <deconvolution/DeconvolverHogbom.h>

namespace askap {

    namespace synthesis {

        /// @brief Class for a deconvolver based on the Hogbom Clean
        /// @details This base class defines a deconvolver used to estimate an
        /// image from a dirty image, psf optionally using a mask and a weights image.
        /// The template argument T is the type, and FT is the transform
        /// e.g. DeconvolverHogbom<Double, DComplex>
        /// @ingroup Deconvolver

        template<class T, class FT>
        DeconvolverHogbom<T, FT>::~DeconvolverHogbom()
        {
        };

        template<class T, class FT>
        DeconvolverHogbom<T, FT>::DeconvolverHogbom(Vector<Array<T> >& dirty, Vector<Array<T> >& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
            if (this->itsNumberDirtyTerms > 1) {
                throw(AskapError("Hogbom CLEAN cannot perform multi-term deconvolutions"));
            }
        };

        template<class T, class FT>
        DeconvolverHogbom<T, FT>::DeconvolverHogbom(Array<T>& dirty, Array<T>& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
        };

        template<class T, class FT>
        void DeconvolverHogbom<T, FT>::initialise()
        {
            DeconvolverBase<T, FT>::initialise();
        }

        template<class T, class FT>
        bool DeconvolverHogbom<T, FT>::deconvolve()
        {
            this->initialise();

            ASKAPLOG_INFO_STR(dechogbomlogger, "Performing Hogbom CLEAN for " << this->control()->targetIter() << " iterations");
            do {
                this->oneIteration();
                this->monitor()->monitor(*(this->state()));
                this->state()->incIter();
            } while (!this->control()->terminate(*(this->state())));

            ASKAPLOG_INFO_STR(dechogbomlogger, "Performed Hogbom CLEAN for " << this->state()->currentIter() << " iterations");

            ASKAPLOG_INFO_STR(dechogbomlogger, this->control()->terminationString());

            this->finalise();

            return True;
        }

        template<class T, class FT>
        void DeconvolverHogbom<T, FT>::configure(const LOFAR::ParameterSet& parset)
        {
            DeconvolverBase<T, FT>::configure(parset);
        }

        // This contains the heart of the Hogbom Clean algorithm
        template<class T, class FT>
        bool DeconvolverHogbom<T, FT>::oneIteration()
        {
            const bool isMasked(this->weight(0).shape().conform(this->dirty(0).shape()));

            // Find peak in residual image
            casa::IPosition minPos;
            casa::IPosition maxPos;
            T minVal, maxVal;
            if (isMasked) {
                casa::minMaxMasked(minVal, maxVal, minPos, maxPos, this->dirty(0), this->weight(0));
                minVal = this->dirty(0)(minPos);
                maxVal = this->dirty(0)(maxPos);
            } else {
                casa::minMax(minVal, maxVal, minPos, maxPos, this->dirty(0));
            }
            //
            ASKAPLOG_INFO_STR(dechogbomlogger, "Maximum = " << maxVal << " at location " << maxPos);
            ASKAPLOG_INFO_STR(dechogbomlogger, "Minimum = " << minVal << " at location " << minPos);

            T absPeakVal = 0.0;
            casa::IPosition absPeakPos;
            if (abs(minVal) < abs(maxVal)) {
                absPeakVal = maxVal;
                absPeakPos = maxPos;
            } else {
                absPeakVal = minVal;
                absPeakPos = minPos;
            }

            this->state()->setPeakResidual(absPeakVal);
            this->state()->setObjectiveFunction(absPeakVal);
            this->state()->setTotalFlux(sum(this->model()));

            // Has this terminated for any reason?
            if (this->control()->terminate(*(this->state()))) {
                return True;
            }

            const uInt nx(this->psf(0).shape()(0));
            const uInt ny(this->psf(0).shape()(1));

            IPosition subPsfShape(this->findSubPsfShape());

            // Now we adjust model and residual for this component
            const casa::IPosition residualShape(this->dirty(0).shape().nonDegenerate());
            const IPosition subPsfStart(2, nx / 2 - subPsfShape(0) / 2, ny / 2 - subPsfShape(1) / 2);
            const IPosition subPsfEnd(2, nx / 2 + subPsfShape(0) / 2 - 1, ny / 2 + subPsfShape(1) / 2 - 1);
            const IPosition subPsfStride(2, 1, 1);

            Slicer subPsfSlicer(subPsfStart, subPsfEnd, subPsfStride, Slicer::endIsLast);

            const casa::IPosition psfShape(2, nx, ny);

            casa::IPosition residualStart(2, 0), residualEnd(2, 0), residualStride(2, 1);
            casa::IPosition psfStart(2, 0), psfEnd(2, 0), psfStride(2, 1);

            const casa::IPosition modelShape(this->model(0).shape().nonDegenerate());
            casa::IPosition modelStart(2, 0), modelEnd(2, 0), modelStride(2, 1);

            // Wrangle the start, end, and shape into consistent form.
            for (uInt dim = 0; dim < 2; dim++) {
                residualStart(dim) = max(0, Int(absPeakPos(dim) - psfShape(dim) / 2));
                residualEnd(dim) = min(Int(absPeakPos(dim) + psfShape(dim) / 2 - 1), Int(residualShape(dim) - 1));
                // Now we have to deal with the PSF. Here we want to use enough of the
                // PSF to clean the residual image.
                psfStart(dim) = max(0, Int(this->itsPeakPSFPos(dim) - (absPeakPos(dim) - residualStart(dim))));
                psfEnd(dim) = min(Int(this->itsPeakPSFPos(dim) - (absPeakPos(dim) - residualEnd(dim))),
                                  Int(psfShape(dim) - 1));

                modelStart(dim) = residualStart(dim);
                modelEnd(dim) = residualEnd(dim);
            }

            casa::Slicer psfSlicer(psfStart, psfEnd, psfStride, Slicer::endIsLast);
            casa::Slicer residualSlicer(residualStart, residualEnd, residualStride, Slicer::endIsLast);
            casa::Slicer modelSlicer(modelStart, modelEnd, modelStride, Slicer::endIsLast);

            if (!(residualSlicer.length() == psfSlicer.length()) || !(residualSlicer.stride() == psfSlicer.stride())) {
                ASKAPLOG_INFO_STR(dechogbomlogger, "Peak of PSF  : " << this->itsPeakPSFPos);
                ASKAPLOG_INFO_STR(dechogbomlogger, "Peak of residual: " << absPeakPos);
                ASKAPLOG_INFO_STR(dechogbomlogger, "Residual start  : " << residualStart << " end: " << residualEnd);
                ASKAPLOG_INFO_STR(dechogbomlogger, "PSF   start  : " << psfStart << " end: " << psfEnd);
                ASKAPLOG_INFO_STR(dechogbomlogger, "Residual slicer : " << residualSlicer);
                ASKAPLOG_INFO_STR(dechogbomlogger, "PSF slicer   : " << psfSlicer);
                throw AskapError("Mismatch in slicers for residual and psf images");
            }

            // Add to model
            this->model()(absPeakPos) = this->model()(absPeakPos) + this->control()->gain() * absPeakVal;

            // Subtract entire PSF from residual image

            this->dirty()(residualSlicer) = this->dirty()(residualSlicer)
                                            - this->control()->gain() * absPeakVal * this->psf()(psfSlicer);

            return True;
        }

    } // namespace synthesis

} // namespace askap


#ifndef AIKIDO_COMMON_SPLINE_HPP_
#define AIKIDO_COMMON_SPLINE_HPP_

#include <cstddef>
#include <memory>
#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Sparse>
#include <Eigen/StdVector>

namespace aikido {
namespace common {

/// An arbitrary dimensional polynomial spline. The number of coefficients,
/// outputs, and knot points may be specified either at compile time (via
/// template parameters) or at runtime (if the template parameters are
/// \c Eigen::Dynamic). We suggest setting as many of these parameters at
/// compile time as possible for best performance.
///
/// Note that this spline is not guaranteed to be continuous at knot points;
/// this is the responsbility of the user who constructs the splines
/// coefficients. See \c SplineProblem for a helper class that constructs a
/// continuous spline from a set of constraints.
///
/// \tparam _Scalar floating type used to represent a scalar value
/// \tparam _Index integral type used to represent an index
/// \tparam _NumCoefficients number of polynomial coefficients, or \c Dynamic
/// \tparam _NumOutputs number of outputs, or \c Dynamic
/// \tparam _NumKnots number of knots, or \c Dynamic
template <class _Scalar = double,
          class _Index = int,
          _Index _NumCoefficients = Eigen::Dynamic,
          _Index _NumOutputs = Eigen::Dynamic,
          _Index _NumKnots = Eigen::Dynamic>
class SplineND
{
public:
  using Scalar = _Scalar;
  using Index = _Index;

  static constexpr Index NumCoefficientsAtCompileTime = _NumCoefficients;
  static constexpr Index NumOutputsAtCompileTime = _NumOutputs;
  static constexpr Index NumKnotsAtCompileTime = _NumKnots;
  static constexpr Index NumSegmentsAtCompileTime
      = (_NumKnots != Eigen::Dynamic) ? (NumKnotsAtCompileTime - 1)
                                      : Eigen::Dynamic;
  static constexpr Index DimensionAtCompileTime
      = (NumSegmentsAtCompileTime != Eigen::Dynamic
         && _NumCoefficients != Eigen::Dynamic)
            ? (NumSegmentsAtCompileTime * NumCoefficientsAtCompileTime)
            : Eigen::Dynamic;

  using TimeVector = Eigen::Matrix<Scalar, NumKnotsAtCompileTime, 1>;
  using SolutionMatrix = Eigen::Matrix<Scalar,
                                       NumOutputsAtCompileTime,
                                       NumCoefficientsAtCompileTime>;
  using OutputVector = Eigen::Matrix<Scalar, NumOutputsAtCompileTime, 1>;
  using SolutionMatrices
      = std::vector<SolutionMatrix, Eigen::aligned_allocator<SolutionMatrix> >;

  /// Constructs an empty spline.
  SplineND() = default;

  /// Constructs a spline with the specified coefficients. The \c _solution
  /// is a vector of \c _times.size() - 1 spline coefficients, where the
  /// i-th matrix defines the polynomial coefficients for the segment between
  /// knot points (i) and (i + 1). Each coefficient matrix of size
  /// (num outputs) x (num coefficients), where element (i, j) is the
  /// coefficient on the \c x^j term for output \c i.
  ///
  /// \param _times times of knot points, must be monotone increasing
  /// \param _solution list of polynomial coefficients for each segment
  SplineND(
      const TimeVector& _times,
      const std::vector<SolutionMatrix,
                        Eigen::aligned_allocator<SolutionMatrix> >& _solution);

  // Default copy and move semantics.
  SplineND(SplineND&& _other) = default;
  SplineND(const SplineND& _other) = default;
  SplineND& operator=(SplineND&& _other) = default;
  SplineND& operator=(const SplineND& _other) = default;

  /// Sets the time of the \c _index-th knot point. Times must remain monotone
  /// after performing this operation.
  ///
  /// \param _index index of a knot point
  /// \param _t new time for that knot point
  void setTime(Index _index, Scalar _t);

  /// Sets the times of all knot points.
  ///
  /// \param _t new times, must be monotone increasing
  void setTimes(TimeVector&& _t);

  /// Sets the times of all knot points.
  ///
  /// \param _t new times, must be monotone increasing
  void setTimes(const TimeVector& _t);

  /// Gets times of all knot points.
  ///
  /// \return times of knot points
  const TimeVector& getTimes() const;

  /// Gets polynomial coefficients for all segments.
  ///
  /// \return polynomial coefficients
  const SolutionMatrices& getCoefficients() const;

  /// Gets the number of knot points.
  ///
  /// \return number of knot points
  Index getNumKnots() const;

  /// Gets the number of outputs points.
  ///
  /// \return number of outputs points
  Index getNumOutputs() const;

  /// Gets an upperbound on the number of non-zero derivatives.
  ///
  /// \return upper bound on the number of non-zero derivatives
  Index getNumDerivatives() const;

  /// Gets the number of polynomial coefficients in each segment.
  ///
  /// \return number of polynomial coefficients
  Index getNumCoefficients() const;

  /// Gets the duration of the spline. This is the difference between the time
  /// of the first and last knot points.
  ///
  /// \return duration of the spline
  Scalar getDuration() const;

  /// Gets the index of the segment that contains time \c _t.
  ///
  /// \param _t time parameter
  /// \return index of the segment that contains this time
  Index getSegmentIndex(Scalar _t) const;

  /// Evaluate the \c _derivative-th order of the spline at time \c _t. The
  /// zero-th order derivative is function value. All derivatives greater than
  /// \c getNumDerivatives() are guaranteed to be zero.
  ///
  /// \param _t time parameter
  /// \param _derivative order of derivative to evaluate
  /// \return value of the specified derivative at time \c _t
  OutputVector evaluate(Scalar _t, Index _derivative = 0) const;

private:
  using CoefficientVector
      = Eigen::Matrix<Scalar, NumCoefficientsAtCompileTime, 1>;
  using CoefficientMatrix = Eigen::Matrix<Scalar,
                                          NumCoefficientsAtCompileTime,
                                          NumCoefficientsAtCompileTime>;

  TimeVector mTimes;
  SolutionMatrices mSolution;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(TimeVector::NeedsToAlign)
};

/// Utility for fitting splines given constraints on function value, derivative
/// value, and continuity. This class is intended to be used by calling methods
/// to add constraints to the spline, then calling \c fit() to find spline
/// coefficients that satisfy those constraints.
///
/// The number of coefficients, outputs, and knot points may be specified
/// either at compile time (via template parameters) or at runtime (if the
/// template parameters are \c Eigen::Dynamic). We suggest setting as many of
/// these parameters at compile time as possible for best performance.
///
/// \tparam _Scalar floating type used to represent a scalar value
/// \tparam _Index integral type used to represent an index
/// \tparam _NumCoefficients number of polynomial coefficients, or \c Dynamic
/// \tparam _NumOutputs number of outputs, or \c Dynamic
/// \tparam _NumKnots number of knots, or \c Dynamic
template <class _Scalar = double,
          class _Index = int,
          _Index _NumCoefficients = Eigen::Dynamic,
          _Index _NumOutputs = Eigen::Dynamic,
          _Index _NumKnots = Eigen::Dynamic>
class SplineProblem
{
public:
  using Scalar = _Scalar;
  using Index = _Index;

  static constexpr Index NumCoefficientsAtCompileTime = _NumCoefficients;
  static constexpr Index NumOutputsAtCompileTime = _NumOutputs;
  static constexpr Index NumKnotsAtCompileTime = _NumKnots;
  static constexpr Index NumSegmentsAtCompileTime
      = (_NumKnots != Eigen::Dynamic) ? (NumKnotsAtCompileTime - 1)
                                      : Eigen::Dynamic;
  static constexpr Index DimensionAtCompileTime
      = (NumSegmentsAtCompileTime != Eigen::Dynamic
         && _NumCoefficients != Eigen::Dynamic)
            ? (NumSegmentsAtCompileTime * NumCoefficientsAtCompileTime)
            : Eigen::Dynamic;

  using TimeVector = Eigen::Matrix<Scalar, NumKnotsAtCompileTime, 1>;
  using OutputVector = Eigen::Matrix<Scalar, NumOutputsAtCompileTime, 1>;
  using OutputMatrix = Eigen::Matrix<Scalar,
                                     NumCoefficientsAtCompileTime,
                                     NumOutputsAtCompileTime>;
  using CoefficientVector
      = Eigen::Matrix<Scalar, NumCoefficientsAtCompileTime, 1>;
  using CoefficientMatrix = Eigen::Matrix<Scalar,
                                          NumCoefficientsAtCompileTime,
                                          NumCoefficientsAtCompileTime>;
  using ProblemMatrix = Eigen::SparseMatrix<Scalar, 0, Index>;
  using ProblemVector
      = Eigen::Matrix<Scalar, DimensionAtCompileTime, NumOutputsAtCompileTime>;
  using SolutionMatrix = Eigen::Matrix<Scalar,
                                       NumOutputsAtCompileTime,
                                       NumCoefficientsAtCompileTime>;
  using Spline
      = SplineND<Scalar, Index, _NumCoefficients, _NumOutputs, _NumKnots>;

  /// Constructs a spline fitting problem with the knot points at the specified
  /// times. This overload is only supported if \c _NumCoefficients and
  /// \c _NumOutputs are specified as template parameters. The length of
  /// \c _times must equal \c _NumKnots if that template parameter is specified
  ///
  /// If \c _NumCoefficients or \c _NumOutputs is \c Eigen::Dynamic, you must
  /// use the other constructor overload.
  ///
  /// \param _times list of knot point times, must be monotone increasing
  explicit SplineProblem(const TimeVector& _times);

  /// Constructs a spline fitting problem with knot points at the specified
  /// times and the specified number of coefficients and outputs. These
  /// parameters must match \c _NumKnots and \c _NumCoefficients if they are
  /// specified at compile time. Additionally, the length of \c _times must
  /// match \c _NumKnots if that template parameter is specified.
  ///
  /// \param _times list of knot point times, must be monotone increasing
  /// \param _numCoefficients number of polynomial coefficients
  /// \param _numOutputs number of outputs
  SplineProblem(
      const TimeVector& _times, Index _numCoefficients, Index _numOutputs);

  // Default copy and move semantics.
  SplineProblem(SplineProblem&& _other) = default;
  SplineProblem(const SplineProblem& _other) = default;
  SplineProblem& operator=(SplineProblem&& _other) = default;
  SplineProblem& operator=(const SplineProblem& _other) = default;

  /// Creates a vector of the form [ 1, t, t^2, ... t^_n ]
  static CoefficientVector createTimeVector(Scalar _t, Index _i, Index _n);

  /// Creates the \c _n by \c _n matrix of derivative coefficients for a
  /// polynomial with \c _n coefficients. The i-th row of this matrix
  /// corresponds to coefficients for the i-th derivative. The j-th column of
  /// this matrix corresponds to the coefficient on x^j.
  ///
  /// \param _n number of coefficients
  /// \return polynomial coefficient matrix
  static CoefficientMatrix createCoefficientMatrix(Index _n);

  /// Adds a constraint that the \c _derivative-th order derivative of knot
  /// point \c _knot should equal \c _value. This adds one constraint if the
  /// knot is an end point and two constraints for interior knots. The zero-th
  /// derivative is function value.
  ///
  /// \param _knot knot index to constraint
  /// \param _derivative order of derivative to constraint
  /// \param _value desired value of that knot point's derivative
  void addConstantConstraint(
      Index _knot, Index _derivative, const OutputVector& _value);

  /// Adds a continuity constraint on the \c _derivative-th order derivative at
  /// knot point \c _knot. This operation is only defined for interior knot
  /// points and adds one constraint.
  ///
  /// \param _knot knot index to constraint
  /// \param _derivative order of derivative to constraint
  void addContinuityConstraint(Index _knot, Index _derivative);

  /// Fit a spline given the constraints on added to this object. The behavior
  /// of this function is undefined if the problem is over or
  /// under-constrained. To avoid this, be sure to only add
  /// (num coefficients) * (num knots - 1) constraints to this class.
  ///
  /// \return spline that satisfies the constraints
  Spline fit();

  /// Gets the number of knot points.
  ///
  /// \return number of knot points
  Index getNumKnots() const;

  /// Gets the number of outputs points.
  ///
  /// \return number of outputs points
  Index getNumOutputs() const;

  /// Gets the duration of the spline. This is the difference between the time
  /// of the first and last knot points.
  ///
  /// \return duration of the spline
  Scalar getDuration() const;

private:
  Index mNumKnots;
  Index mNumSegments;
  Index mNumCoefficients;
  Index mNumOutputs;
  Index mDimension;

  CoefficientMatrix mCoefficientMatrix;

  Index mRowIndex;
  TimeVector mTimes;
  ProblemMatrix mA;
  ProblemVector mB;

  std::vector<SolutionMatrix,
              Eigen::aligned_allocator<SolutionMatrix> >
      mSolution; // length _NumSegments

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(
      CoefficientMatrix::NeedsToAlign || TimeVector::NeedsToAlign
      || ProblemMatrix::NeedsToAlign
      || ProblemVector::NeedsToAlign)
};

} // namespace common
} // namespace aikido

#include "detail/Spline-impl.hpp"

#endif // AIKIDO_COMMON_SPLINE_HPP_


#include "eigen-sparse.h"
#include <Eigen/LU>

#if __cplusplus > 199711L
  #define SMART_PTR std::unique_ptr
#else
  #define SMART_PTR std::auto_ptr
#endif

template <class T>
RET sparse_new(int rows, int cols, void** pr) {
    typedef SparseMatrix<T> M;
    *(M**)pr = new M(rows, cols);
    return 0;
}
API(sparse_new, (int code, int rows, int cols, void** pr), (rows,cols,pr));

template <class T>
RET sparse_clone(void* p, void** q) {
    typedef SparseMatrix<T> M;
    *(M**)q = new M(*(M*)p);
    return 0;
}
API(sparse_clone, (int code, void* p, void** q), (p,q));

template <class T>
RET sparse_fromList(int rows, int cols, void* data, int size, void** pr) {
    typedef SparseMatrix<T> M;
    typedef Triplet<T> E;
    SMART_PTR<M> a(new M(rows, cols));
    a->setFromTriplets((E*)data, (E*)data + size);
    *(M**)pr = a.release();
    return 0;
}
API(sparse_fromList, (int code, int rows, int cols, void* data, int size, void** pr), (rows,cols,data,size,pr));

template <class  T>
RET sparse_toList(void* p, void* q, int s) {
    int n = 0;
    typedef SparseMatrix<T> M;
    typedef Triplet<T> E;
    M* m = (M*)p;
    for (int k = 0; k < m->outerSize(); ++k) {
        for (typename M::InnerIterator i(*m, k); i; ++i) {
            if (n >= s)
                return strdup("sparse_toList: buffer overrun detected");
            ((E*)q)[n++] = E(i.row(), i.col(), i.value());
        }
    }
    return n < s ? strdup("sparse_toList: buffer underrun detected") : 0;
}
API(sparse_toList, (int code, void* p, void* q, int s), (p,q,s));

template <class T>
RET sparse_free(void* p) {
    delete (SparseMatrix<T>*)p;
    return 0;
}
API(sparse_free, (int code, void* p), (p));

#define SPARSE_UNOP_INPLACE(name)\
template <class T>\
RET sparse_##name(void* p, void** pr) {\
    typedef SparseMatrix<T> M;\
    std::unique_ptr<M> a(new M(*(M*)p));\
    a->name();\
    *(M**)pr = a.release();\
    return 0;\
}\
API(sparse_##name, (int code, void* p, void** pr), (p, pr));

SPARSE_UNOP_INPLACE(makeCompressed);
SPARSE_UNOP_INPLACE(uncompress);

#define SPARSE_UNOP(name)\
template <class T>\
RET sparse_##name(void* p, void** pr) {\
    typedef SparseMatrix<T> M;\
    *(M**)pr = new M(((M*)p)->name());\
    return 0;\
}\
API(sparse_##name, (int code, void* p, void** pr), (p, pr));

SPARSE_UNOP(adjoint);
SPARSE_UNOP(transpose);

template <class T>
RET sparse_pruned(void* p, void** pr) {
    typedef SparseMatrix<T> M;
    std::unique_ptr<M> a(new M(*(M*)p));
    a->prune(T(1));
    *(M**)pr = a.release();
    return 0;
}
API(sparse_pruned, (int code, void* p, void** pr), (p, pr));

template <class T>
RET sparse_prunedRef(void* p, void* q, void** pr) {
    typedef SparseMatrix<T> M;
    std::unique_ptr<M> a(new M(*(M*)p));
    a->prune(*(T*)q);
    *(M**)pr = a.release();
    return 0;
}
API(sparse_prunedRef, (int code, void* p, void* q, void** pr), (p, q, pr));

template <class T>
RET sparse_scale(void* p, void* q, void** pr) {
    typedef SparseMatrix<T> M;
    *(M**)pr = new M(*(T*)q * *(M*)p);
    return 0;
}
API(sparse_scale, (int code, void* p, void* q, void** pr), (p, q, pr));

template <class T>
RET sparse_coeff(void* p, int row, int col, void* pr) {
    *(T*)pr = ((SparseMatrix<T>*)p)->coeff(row, col);
    return 0;
}
API(sparse_coeff, (int code, void* p, int row, int col, void* pr), (p, row, col, pr));

template <class T>
RET sparse_coeffRef(void* p, int row, int col, void** pr) {
    *(T**)pr = &((SparseMatrix<T>*)p)->coeffRef(row, col);
    return 0;
}
API(sparse_coeffRef, (int code, void* p, int row, int col, void** pr), (p, row, col, pr));

#define SPARSE_PROP(name,type)\
template <class T>\
RET sparse_##name(void* p, void* pr) {\
    *(type*)pr = ((SparseMatrix<T>*)p)->name();\
    return 0;\
}\
API(sparse_##name, (int code, void* p, void* pr), (p, pr));

SPARSE_PROP(cols, int);
SPARSE_PROP(rows, int);
SPARSE_PROP(innerSize, int);
SPARSE_PROP(outerSize, int);
SPARSE_PROP(nonZeros, int);
SPARSE_PROP(isCompressed, int);
SPARSE_PROP(norm, T);
SPARSE_PROP(squaredNorm, T);
SPARSE_PROP(blueNorm, T);

#define SPARSE_BINOP(name,op)\
template <class T>\
RET sparse_##name(void* p, void* q, void** pr) {\
    typedef SparseMatrix<T> M;\
    *(M**)pr = new M(*(M*)p op *(M*)q);\
    return 0;\
}\
API(sparse_##name, (int code, void* p, void* q, void** pr), (p, q, pr));

SPARSE_BINOP(add, +);
SPARSE_BINOP(sub, -);
SPARSE_BINOP(mul, *);

template <class T>
RET sparse_block(void* p, int row, int col, int rows, int cols, void** pr) {
    typedef SparseMatrix<T> M;
    *(M**)pr = new M(((M*)p)->block(row,col,rows,cols));
    return 0;
}
API(sparse_block, (int code, void* p, int row, int col, int rows, int cols, void** pr), (p, row, col, rows, cols, pr));

template <class T> bool isZero(T x) { return x == 0; }
template <class T> bool isZero(std::complex<T> x) { return x.real() == 0 && x.imag() == 0; }

template <class T>
RET sparse_fromMatrix(void* p, int rows, int cols, void** pq) {
    typedef SparseMatrix<T> M;
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix src((T*)p, rows, cols);
    std::unique_ptr<M> dst(new M(rows, cols));
    for (int row = 0; row < rows; ++row) {
        for (int col = 0; col < cols; ++col) {
            T val = src.coeff(row,col);
            if (!isZero(val))
                dst->insert(row, col) = val;
        }
    }
    dst->makeCompressed();
    *(M**)pq = dst.release();
    return 0;
}
API(sparse_fromMatrix, (int code, void* p, int rows, int cols, void** pq), (p,rows,cols,pq));

template <class T>
RET sparse_toMatrix(void* p, void* q, int rows, int cols) {
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix((T*)q, rows, cols) = *(SparseMatrix<T>*)p;
    return 0;
}
API(sparse_toMatrix, (int code, void* p, void* q, int rows, int cols), (p,q,rows,cols));

template <class T>
RET sparse_values(void* p, int* s, void** q) {
    SparseMatrix<T>* m = (SparseMatrix<T>*)p;
    *s = m->outerIndexPtr()[m->outerSize()];
    *q = m->valuePtr();
    return 0;
}
API(sparse_values, (int code, void*p, int* s, void** q), (p,s,q));

template <class T>
RET sparse_outerStarts(void* p, int* s, void** q) {
    SparseMatrix<T>* m = (SparseMatrix<T>*)p;
    *s = m->outerSize() + 1;
    *q = m->outerIndexPtr();
    return 0;
}
API(sparse_outerStarts, (int code, void*p, int* s, void** q), (p,s,q));

template <class T>
RET sparse_innerIndices(void* p, int* s, void** q) {
    SparseMatrix<T>* m = (SparseMatrix<T>*)p;
    *s = m->outerIndexPtr()[m->outerSize()];
    *q = m->innerIndexPtr();
    return 0;
}
API(sparse_innerIndices, (int code, void*p, int* s, void** q), (p,s,q));

template <class T>
RET sparse_innerNNZs(void* p, int* s, void** q) {
    SparseMatrix<T>* m = (SparseMatrix<T>*)p;
    *s = m->outerSize();
    *q = m->innerNonZeroPtr();
    return 0;
}
API(sparse_innerNNZs, (int code, void*p, int* s, void** q), (p,s,q));

#define SPARSE_INPLACE(name,op)\
template <class T>\
RET sparse_##name(void* p) {\
    ((SparseMatrix<T>*)p)->op();\
    return 0;\
}\
API(sparse_##name, (int code, void* p), (p));

SPARSE_INPLACE(setIdentity, setIdentity);
SPARSE_INPLACE(setZero, setZero);
SPARSE_INPLACE(compressInplace, makeCompressed);
SPARSE_INPLACE(uncompressInplace, uncompress);

template <class T>
RET sparse_reserve(void* p, int s) {
    ((SparseMatrix<T>*)p)->reserve(s);
    return 0;
}
API(sparse_reserve, (int code, void* p, int s), (p,s));

template <class T>
RET sparse_resize(void* p, int r, int c) {
    ((SparseMatrix<T>*)p)->resize(r,c);
    return 0;
}
API(sparse_resize, (int code, void* p, int r, int c), (p,r,c));

template <class T>
RET sparse_conservativeResize(void* p, int r, int c) {
    ((SparseMatrix<T>*)p)->conservativeResize(r,c);
    return 0;
}
API(sparse_conservativeResize, (int code, void* p, int r, int c), (p,r,c));


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include <cstdio>
#include <cmath>
#include <thread>
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Core/Core.h>
#include <Core/Utility/Console.h>

#define NUM_THREADS 4
#define NUM_START 1
#define NUM_END 10

void simple_task()
{
    int n_a_rows = 2000;
    int n_a_cols = 2000;
    int n_b_rows = 2000;
    int n_b_cols = 2000;

    Eigen::MatrixXd a(n_a_rows, n_a_cols);
    for (int i = 0; i < n_a_rows; ++i)
        for (int j = 0; j < n_a_cols; ++j)
            a(i, j) = n_a_cols * i + j;

    Eigen::MatrixXd b(n_b_rows, n_b_cols);
    for (int i = 0; i < n_b_rows; ++i)
        for (int j = 0; j < n_b_cols; ++j)
            b(i, j) = n_b_cols * i + j;

    Eigen::MatrixXd d(n_a_rows, n_b_cols);
    d = a * b;
}

void svd_task()
{
    int n_a_rows = 10000;
    int n_a_cols = 200;
    Eigen::MatrixXd a(n_a_rows, n_a_cols);
    for (int i = 0; i < n_a_rows; ++i)
        for (int j = 0; j < n_a_cols; ++j)
            a(i, j) = n_a_cols * i + j;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(a,
            Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd pca = svd.matrixU().block<10000, 10>(0, 0
            ).transpose() * a;
}

void TestMatrixMultiplication(int argc, char** argv)
{
    int i = 0, nRet = 0, nSum = 0, nStart = NUM_START, nEnd = NUM_END;
    int nThreads = 1, nTmp = nStart + nEnd;
    unsigned uTmp = (unsigned(nEnd - nStart + 1) * unsigned(nTmp)) / 2;
    int nSumCalc = uTmp;

    if (nTmp < 0) {
        nSumCalc = -nSumCalc;
    }

#ifdef _OPENMP
    printf("OpenMP is supported.\n");
#else
    printf("OpenMP is not supported.\n");
#endif

#ifdef _OPENMP
    omp_set_num_threads(NUM_THREADS);
#endif

#pragma omp parallel default(none) private(i) shared(nSum, nThreads, nStart, nEnd)
    {
#ifdef _OPENMP
#pragma omp master
        nThreads = omp_get_num_threads();
#endif

#pragma omp for
        for (i = nStart; i <= nEnd; ++i) {
#pragma omp atomic
            nSum += i;
        }
    }

    if (nThreads == NUM_THREADS) {
        printf("%d OpenMP threads were used.\n", NUM_THREADS);
        nRet = 0;
    } else {
        printf("Expected %d OpenMP threads, but %d were used.\n",
                NUM_THREADS, nThreads);
        nRet = 1;
    }

    if (nSum != nSumCalc) {
        printf("The sum of %d through %d should be %d, "
                "but %d was reported!\n",
                NUM_START, NUM_END, nSumCalc, nSum);
        nRet = 1;
    } else {
        printf("The sum of %d through %d is %d\n",
                NUM_START, NUM_END, nSum);
    }

    int test_thread = 256;
    if (argc > 1) {
        test_thread = std::stoi(argv[1]);
    }
    open3d::PrintInfo("Benchmark multithreading up to %d threads.\n",
            test_thread);

    for (int i = 1; i <= test_thread; i *= 2) {
        char buff[1024];
        sprintf(buff, "simple task, %d tasks, %d threads", i, i);
        open3d::ScopeTimer t(buff);
#ifdef _OPENMP
        omp_set_num_threads(i);
#endif
#pragma omp parallel default(none) shared(nThreads)
        {
            simple_task();
        }
    }

    for (int i = 1; i <= test_thread; i *= 2) {
        char buff[1024];
        sprintf(buff, "simple task, %d tasks, %d threads", i, i);
        open3d::ScopeTimer t(buff);
        std::vector<std::thread> threads(i);
        for (int k = 0; k < i; k++) {
            threads[k] = std::thread(simple_task);
        }
        for (int k = 0; k < i; k++) {
            threads[k].join();
        }
    }

    for (int i = 1; i <= test_thread; i *= 2) {
        char buff[1024];
        sprintf(buff, "svd, %d tasks, %d threads", i, i);
        open3d::ScopeTimer t(buff);
#ifdef _OPENMP
        omp_set_num_threads(i);
#endif
#pragma omp parallel default(none) shared(nThreads)
        {
            svd_task();
        }
    }

    for (int i = 1; i <= test_thread; i *= 2) {
        char buff[1024];
        sprintf(buff, "svd task, %d tasks, %d threads", i, i);
        open3d::ScopeTimer t(buff);
        std::vector<std::thread> threads(i);
        for (int k = 0; k < i; k++) {
            threads[k] = std::thread(svd_task);
        }
        for (int k = 0; k < i; k++) {
            threads[k].join();
        }
    }
}

inline void ComputeSomething(int i, Eigen::Vector6d &A_r, double &r,
        std::vector<Eigen::Vector3d> &data)
{
    const Eigen::Vector3d &vs = data[i];
    const Eigen::Vector3d &vt = data[i];
    const Eigen::Vector3d &nt = data[i];
    r = (vs - vt).dot(nt);
    //A_r.setZero();
    A_r.block<3, 1>(0, 0).noalias() = vs.cross(nt);
    A_r.block<3, 1>(3, 0).noalias() = nt;
}

/// Function to simulate building Jacobian matrix
/// uses simple way of using OpenMP and std::bind
void TestBindedFunction()
{
    // data generation
    const int NCORR = 200000000;
    std::vector<Eigen::Vector3d> data;
    {
        open3d::ScopeTimer timer1("Data generation");
        data.resize(NCORR);
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < NCORR; i++) {
            data[i] = Eigen::Vector3d::Random();
        }
    }

    // data we want to build
    Eigen::Matrix6d ATA;
    Eigen::Vector6d ATb;

    // to do using private ATA
    // https://stackoverflow.com/questions/24948395/openmp-calling-global-variables-through-functions
    auto f = std::bind(ComputeSomething, std::placeholders::_1,
        std::placeholders::_2, std::placeholders::_3, data);

    auto f_lambda = [&](int i, Eigen::Vector6d &A_r, double &r) {
        ComputeSomething(i, A_r, r, data);
    };

    ATA.setZero();
    ATb.setZero();
    {
        open3d::ScopeTimer timer("Calling binding function");
#ifdef _OPENMP
#pragma omp parallel
        {
#endif
            Eigen::Matrix6d ATA_private;
            Eigen::Vector6d ATb_private;
            ATA_private.setZero();
            ATb_private.setZero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
            for (int i = 0; i < NCORR; i++) {
                Eigen::Vector6d A_r;
                double r;
                f(i, A_r, r);
                ATA_private.noalias() += A_r * A_r.transpose();
                ATb_private.noalias() += A_r * r;
            }
#ifdef _OPENMP
#pragma omp critical
            {
#endif
                ATA += ATA_private;
                ATb += ATb_private;
#ifdef _OPENMP
            }    // omp critical
        }    // omp parallel
#endif
    }
    std::cout << ATA << std::endl;
    std::cout << ATb << std::endl;

    ATA.setZero();
    ATb.setZero();
    {
        open3d::ScopeTimer timer("Calling lambda function");
#ifdef _OPENMP
#pragma omp parallel
        {
#endif
            Eigen::Matrix6d ATA_private;
            Eigen::Vector6d ATb_private;
            ATA_private.setZero();
            ATb_private.setZero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
            for (int i = 0; i < NCORR; i++) {
                Eigen::Vector6d A_r;
                double r;
                f_lambda(i, A_r, r);
                ATA_private.noalias() += A_r * A_r.transpose();
                ATb_private.noalias() += A_r * r;
            }
#ifdef _OPENMP
#pragma omp critical
            {
#endif
                ATA += ATA_private;
                ATb += ATb_private;
#ifdef _OPENMP
            }    // omp critical
        }    // omp parallel
#endif
    }
    std::cout << ATA << std::endl;
    std::cout << ATb << std::endl;

    ATA.setZero();
    ATb.setZero();
    {
        open3d::ScopeTimer timer("Calling function directly");
#ifdef _OPENMP
#pragma omp parallel
        {
#endif
            Eigen::Matrix6d ATA_private;
            Eigen::Vector6d ATb_private;
            ATA_private.setZero();
            ATb_private.setZero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
            for (int i = 0; i < NCORR; i++) {
                Eigen::Vector6d A_r;
                double r;
                ComputeSomething(i, A_r, r, data);
                ATA_private.noalias() += A_r * A_r.transpose();
                ATb_private.noalias() += A_r * r;
            }
#ifdef _OPENMP
#pragma omp critical
            {
#endif
                ATA += ATA_private;
                ATb += ATb_private;
#ifdef _OPENMP
            }    // omp critical
        }    // omp parallel
#endif
    }
    std::cout << ATA << std::endl;
    std::cout << ATb << std::endl;

    ATA.setZero();
    ATb.setZero();
    {
        open3d::ScopeTimer timer("Direct optration");
#ifdef _OPENMP
#pragma omp parallel
        {
#endif
            Eigen::Matrix6d ATA_private;
            Eigen::Vector6d ATb_private;
            ATA_private.setZero();
            ATb_private.setZero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
            for (int i = 0; i < NCORR; i++) {
                const Eigen::Vector3d &vs = data[i];
                const Eigen::Vector3d &vt = data[i];
                const Eigen::Vector3d &nt = data[i];
                double r = (vs - vt).dot(nt);
                Eigen::Vector6d A_r;
                A_r.block<3, 1>(0, 0).noalias() = vs.cross(nt);
                A_r.block<3, 1>(3, 0).noalias() = nt;
                ATA_private.noalias() += A_r * A_r.transpose();
                ATb_private.noalias() += A_r * r;
            }
#ifdef _OPENMP
#pragma omp critical
            {
#endif
                ATA += ATA_private;
                ATb += ATb_private;
#ifdef _OPENMP
            }    // omp critical
        }    // omp parallel
#endif
    }
    std::cout << ATA << std::endl;
    std::cout << ATb << std::endl;
}

int main(int argc, char **argv)
{
    using namespace open3d;

    if (ProgramOptionExists(argc, argv, "--test_bind")) {
        TestBindedFunction();
    }
    else {
        TestMatrixMultiplication(argc, argv);
    }
}