#ifndef INHOMOGENOUSCOUPLEDPDEODESOLVER_TEMPLATED_HPP_
#define INHOMOGENOUSCOUPLEDPDEODESOLVER_TEMPLATED_HPP_

#include "ChemChasteFeAssemblerCommon.hpp"
#include "ChemChasteVolumeAssembler.hpp"
#include "ChemChasteSurfaceAssembler.hpp"
#include "AbstractAssemblerSolverHybrid.hpp"
#include "AbstractDynamicLinearPdeSolver.hpp"
#include "AbstractLinearParabolicPdeSystemForCoupledOdeSystem.hpp"
#include "TetrahedralMesh.hpp"
#include "BoundaryConditionsContainer.hpp"
#include "AbstractInhomogenousOdeSystemForCoupledPdeSystem.hpp"
#include "InhomogenousParabolicPdeForCoupledOdeSystem_templated.hpp"
#include "AbstractOdeSystemForCoupledPdeSystem.hpp"
#include "CvodeAdaptor.hpp"
#include "BackwardEulerIvpOdeSolver.hpp"
#include "Warnings.hpp"
#include "VtkMeshWriter.hpp"
#include "StateVariableRegister.hpp"

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <boost/shared_ptr.hpp>

// Same as other class but the numberOfStateVariables to the ode terms are not all equal.
// The ode's change on a nodal basis

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM=ELEMENT_DIM, unsigned PROBLEM_DIM=1>
class InhomogenousCoupledPdeOdeSolverTemplated
    : public AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>,
      public AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>
{
private:

    /** Pointer to the mesh. */
    AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* mpMesh;

    /** The PDE system to be solved. */
    InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* mpPdeSystem;

    /** Vector of pointers to ODE systems, defined at nodes. */
    std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> mOdeSystemsAtNodes;

    /** The values of the ODE system state variables, interpolated at a quadrature point. */
    std::vector<double> mInterpolatedOdeStateVariables;

    /** The ODE solvers. */
    std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> mpOdeSolvers;

    /**
     * A sampling timestep for writing results to file. Set to
     * PdeSimulationTime::GetPdeTimeStep() in the constructor;
     * may be overwritten using the SetSamplingTimeStep() method.
     */
    double mSamplingTimeStep;

    /** Whether ODE systems are present (if not, then the system comprises coupled PDEs only). */
    bool mOdeSystemsPresent;

    /** Meta results file for VTK. */
    out_stream mpVtkMetaFile;

    /**
     * Whether the output directory should be cleared before solve or not. False by default.
     * Can be changed when setting the output directory
     */
    bool mClearOutputDirectory;

    // for calculating the total mass, interpolate the nodal solution and add to sum
    bool mCalculateTotalMass = true;

    std::vector<double> mTotalMass = {0.0};

    std::vector<double> mOldTotalMass = {0.0};

    bool mInterpolateStateVariable = true;

 //   std::vector<double> mValueAtX(PROBLEM_DIM,0.0);

    // for calculating the trace over a line segment
    bool mCalculateSliceMass = true;

    unsigned mInterpolationNodeCount =0;

    unsigned mNodesInElement =3;

    bool mInterpolatePosition=true;

  //  ChastePoint<SPACE_DIM> mX(0,0,0);

    std::vector<std::vector<double>> mSliceMass;

    std::vector<std::vector<double>> mSlicePositions;

    /**
     * Write the current results to mpVtkMetaFile.
     */
    void WriteVtkResultsToFile();

    /**
     * @return the term to be added to the element stiffness matrix.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeMatrixTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * @return the term to be added to the element stiffness vector.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeVectorTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * Reset the member variable mInterpolatedOdeStateVariables.
     */
    void ResetInterpolatedQuantities();

    /**
     * Update the member variable mInterpolatedOdeStateVariables by computing the
     * interpolated value of each ODE state variable at each Gauss point.
     *
     * @param phiI
     * @param pNode pointer to a Node
     */
    void IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode);

    /**
     * Initialise method: sets up the linear system (using the mesh to
     * determine the number of unknowns per row to preallocate) if it is not
     * already set up. Can use an initial solution as PETSc template,
     * or base it on the mesh size.
     *
     * @param initialSolution Initial solution (defaults to NULL) for PETSc to use as a template.
     */
    void InitialiseForSolve(Vec initialSolution=NULL);

    /**
     * Completely set up the linear system that has to be solved each timestep.
     *
     * @param currentSolution The current solution which can be used in setting up
     *  the linear system if needed (NULL if there isn't a current solution)
     * @param computeMatrix Whether to compute the LHS matrix of the linear system
     *   (mainly for dynamic solves).
     */
    void SetupLinearSystem(Vec currentSolution, bool computeMatrix);

public:

    /**
     * Constructor.
     *
     * @param pMesh pointer to the mesh
     * @param pPdeSystem pointer to the PDE system
     * @param pBoundaryConditions pointer to the boundary conditions.
     * @param odeSystemsAtNodes optional vector of pointers to ODE systems, defined at nodes
     * @param pOdeSolver optional pointer to an ODE solver (defaults to NULL)
     */
    InhomogenousCoupledPdeOdeSolverTemplated(TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
                                    InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
                                    BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
                                    std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes=std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*>(),
                                    std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> pOdeSolvers=std::vector<boost::shared_ptr<AbstractIvpOdeSolver>>());

    /**
     * Destructor.
     * If an ODE system is present, the pointers to the ODE system objects are deleted here.
     */
    ~InhomogenousCoupledPdeOdeSolverTemplated();

    /**
     * Overridden PrepareForSetupLinearSystem() method.
     * Pass the current solution to the PDE system to the ODE system and solve it over the next timestep.
     *
     * @param currentPdeSolution the solution to the PDE system at the current time
     */
    void PrepareForSetupLinearSystem(Vec currentPdeSolution);

    /**
     * Set mOutputDirectory.
     *
     * @param outputDirectory the output directory to use
     * @param clearDirectory whether to clear outputDirectory or not. Note that the actual clearing happens when you call SolveAndWriteResultsToFile().
     *                       False by default.
     */
    void SetOutputDirectory(std::string outputDirectory, bool clearDirectory=false);

    /**
     * Set mSamplingTimeStep.
     *
     * @param samplingTimeStep the sampling timestep to use
     */
    void SetSamplingTimeStep(double samplingTimeStep);

    /**
     * Solve the coupled PDE/ODE system over the pre-specified time interval,
     * and record results using mSamplingTimeStep.
     */
    void SolveAndWriteResultsToFile();

    /**
     * Write the solution to VTK. Called by SolveAndWriteResultsToFile().
     *
     * @param solution the solution of the coupled PDE/ODE system
     * @param numTimeStepsElapsed the number of timesteps that have elapsed
     */
    void WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed);

    /**
     * Get a pointer to the ODE system defined at a given node.
     *
     * @param index the global index of a node in the mpMesh
     * @return mOdeSystemsAtNodes[index]
     */
    AbstractOdeSystemForCoupledPdeSystem* GetOdeSystemAtNode(unsigned index);

    void ResetInterpolatedQuantitiesOnNewTimeStep();

    void OutputInterpolatedQuantitiesOnTimestep();
};

///////////////////////////////////////////////////////////////////////////////////
// Implementation
///////////////////////////////////////////////////////////////////////////////////

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm( - start"<<std::endl;
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> matrix_term = zero_matrix<double>(PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate matrix_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);

        // in general this should be looking to the domain field to determine diffusion properties, interpolation required?

        c_matrix<double, SPACE_DIM, SPACE_DIM> this_pde_diffusion_term = mpPdeSystem->ComputeDiffusionTerm(rX, pde_index, pElement);

        c_matrix<double, 1*(ELEMENT_DIM+1), 1*(ELEMENT_DIM+1)> this_stiffness_matrix =
            prod(trans(rGradPhi), c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>(prod(this_pde_diffusion_term, rGradPhi)) )
                + timestep_inverse * this_dudt_coefficient * outer_prod(rPhi, rPhi);

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            for (unsigned j=0; j<ELEMENT_DIM+1; j++)
            {
                matrix_term(i*PROBLEM_DIM + pde_index, j*PROBLEM_DIM + pde_index) = this_stiffness_matrix(i,j);
            }
        }
    }
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm( - end"<<std::endl;
    return matrix_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm( - start"<<std::endl;
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> vector_term;
    vector_term = zero_vector<double>(PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate vector_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        // property of the pde not the ode systems
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);

        // property of the ode systems; returns indexed state of the interpolated state variables, odes already solved
        double this_source_term = mpPdeSystem->ComputeSourceTerm(rX, rU, mInterpolatedOdeStateVariables, pde_index);

        c_vector<double, ELEMENT_DIM+1> this_vector_term;
        this_vector_term = (this_source_term + timestep_inverse*this_dudt_coefficient*rU(pde_index))* rPhi;

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            vector_term(i*PROBLEM_DIM + pde_index) = this_vector_term(i);
        }
    }
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm( - end"<<std::endl;
    return vector_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ResetInterpolatedQuantities()
{
    mInterpolatedOdeStateVariables.clear();

    if (mOdeSystemsPresent)
    {
        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();
        mInterpolatedOdeStateVariables.resize(num_state_variables, 0.0);
    }

    // reset for interpolation of slice mass
  //  ChastePoint<SPACE_DIM> tempX(0,0,0)
  //  mX = tempX;
    mInterpolationNodeCount = 0;

  //  std::vector<double> temp(PROBLEM_DIM,0.0);

   // mValueAtX = temp;

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated::IncrementInterpolatedQuantities"<<std::endl;
    if (mOdeSystemsPresent)
    {
        unsigned matchedIndex;
        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();
        std::vector<std::string> pde_variable_register = mpPdeSystem->GetStateVariableRegister() -> GetStateVariableRegisterVector();

        for (unsigned i=0; i<num_state_variables; i++)
        {
            // each node may not have the total number of states in the ode system
            // select using the statevariable registers at the nodes, if state variable isn't present the value is 0

            if(mOdeSystemsAtNodes[pNode->GetIndex()] -> GetStateVariableRegister() -> IsStateVariablePresent(pde_variable_register[i]))
            {
                matchedIndex = mOdeSystemsAtNodes[pNode->GetIndex()] -> GetStateVariableRegister() -> RetrieveStateVariableIndex(pde_variable_register[i]);

                //rGetStateVariables() returns the current values of the state variables, index
                mInterpolatedOdeStateVariables[i] += phiI * mOdeSystemsAtNodes[pNode->GetIndex()]->rGetStateVariables()[matchedIndex];
                // rGetStateVariables if the value of the ode states variables at the nodes, size < num_state_vars in the pde system
            }

        }

    }

    if(mCalculateTotalMass)
    {
        // interpolate the previous timestep solution at nodes
        // sum the interpolated value

        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();

        ReplicatableVector current_nodal_values(this->mInitialCondition);

        for (unsigned i=0; i<num_state_variables; i++)
        {
            mOldTotalMass[i] += phiI * current_nodal_values[PROBLEM_DIM*pNode->GetIndex()+i];
        }
    }

    if(mInterpolatePosition)
    {
     //   mX.rGetLocation() += phiI*pNode->rGetLocation();
    }

    if(mInterpolateStateVariable)
    {
    //    ReplicatableVector current_nodal_values(this->mInitialCondition);

    //    for (unsigned i=0; i<num_state_variables; i++)
     //   {
      //      mValueAtX[i] += phiI * current_nodal_values[PROBLEM_DIM*pNode->GetIndex()+i];
       // }

    }

    mInterpolationNodeCount++;

    if(mInterpolationNodeCount == mNodesInElement)
    {
        // position and state variable vector are fully known
        ChastePoint<SPACE_DIM> X = AbstractFeVolumeIntegralAssembler<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, true, true, NORMAL>::GetPosition();

        c_vector<double,PROBLEM_DIM> U = AbstractFeVolumeIntegralAssembler<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, true, true, NORMAL>::GetStateVariable();

        if(mCalculateTotalMass)
        {
            for (unsigned i=0; i<PROBLEM_DIM; i++)
            {
                mTotalMass[i] += phiI * U[i];
            }
        }

        if(mCalculateSliceMass)
        {
            //std::cout<<"y: "<<X[1]<<std::endl;
            if(2.85>X[1] && X[1]>2.8)
            {
                // store the x position and the state variable U
                std::vector<double> thisSlicePosition;
                std::vector<double> thisSliceMass;

                for(unsigned i=0; i<SPACE_DIM; i++)
                {
                    thisSlicePosition.push_back(X[i]);
                    //std::cout<<"x: "<<X[i]<<std::endl;
                }

                for(unsigned j=0; j<PROBLEM_DIM; j++)
                {
                    thisSliceMass.push_back(U[j]);
                }

                mSlicePositions.push_back(thisSlicePosition);
                mSliceMass.push_back(thisSliceMass);

            }
            // else ignore point
        }

    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::InitialiseForSolve(Vec initialSolution)
{
    if (this->mpLinearSystem == NULL)
    {
        unsigned preallocation = mpMesh->CalculateMaximumContainingElementsPerProcess() + ELEMENT_DIM;
        if (ELEMENT_DIM > 1)
        {
            // Highest connectivity is closed
            preallocation--;
        }
        preallocation *= PROBLEM_DIM;

        /*
         * Use the current solution (ie the initial solution) as the
         * template in the alternative constructor of LinearSystem.
         * This is to avoid problems with VecScatter.
         */
        this->mpLinearSystem = new LinearSystem(initialSolution, preallocation);
    }

    assert(this->mpLinearSystem);
    this->mpLinearSystem->SetMatrixIsSymmetric(true);
    this->mpLinearSystem->SetKspType("cg");
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetupLinearSystem(Vec currentSolution, bool computeMatrix)
{
    this->SetupGivenLinearSystem(currentSolution, computeMatrix, this->mpLinearSystem);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::InhomogenousCoupledPdeOdeSolverTemplated(
        TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
        InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
        BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
        std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes,
        std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> pOdeSolvers)
    : AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>(pMesh, pBoundaryConditions),
      AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>(pMesh),
      mpMesh(pMesh),
      mpPdeSystem(pPdeSystem),
      mOdeSystemsAtNodes(odeSystemsAtNodes),
      mpOdeSolvers(pOdeSolvers),
      mSamplingTimeStep(DOUBLE_UNSET),
      mOdeSystemsPresent(false),
      mClearOutputDirectory(false)
{
    this->mpBoundaryConditions = pBoundaryConditions;
    /*
     * If any ODE systems are passed in to the constructor, then we aren't just
     * solving a coupled PDE system, in which case the number of ODE system objects
     * must match the number of nodes in the finite element mesh.
     */
    if (!mOdeSystemsAtNodes.empty())
    {
        mOdeSystemsPresent = true;
        assert(mOdeSystemsAtNodes.size() == mpMesh->GetNumNodes());

        /*
         * In this case, if an ODE solver is not explicitly passed into the
         * constructor, then we create a default solver.
         */
        if (!mpOdeSolvers[0])
        {
#ifdef CHASTE_CVODE
            for(unsigned i=0; i<mpOdeSolvers.size(); i++)
            {
                mpOdeSolvers[i].reset(new CvodeAdaptor);
            }

#else
            for(unsigned i=0; i<mOdeSystemsAtNodes.size(); i++)
            {
                mpOdeSolvers.push_back(new BackwardEulerIvpOdeSolver(mOdeSystemsAtNodes[i]->GetNumberOfStateVariables()));
            }

#endif //CHASTE_CVODE
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::~InhomogenousCoupledPdeOdeSolverTemplated()
{
    if (mOdeSystemsPresent)
    {
        for (unsigned i=0; i<mOdeSystemsAtNodes.size(); i++)
        {
            delete mOdeSystemsAtNodes[i];
        }
    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem(Vec currentPdeSolution)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem( - start"<<std::endl;
    if (mOdeSystemsPresent)
    {
        double time = PdeSimulationTime::GetTime();
        double next_time = PdeSimulationTime::GetNextTime();
        double dt = PdeSimulationTime::GetPdeTimeStep();

        ReplicatableVector soln_repl(currentPdeSolution);
        std::vector<double> current_soln_this_node(PROBLEM_DIM);

        // Loop over nodes
        for (unsigned node_index=0; node_index<mpMesh->GetNumNodes(); node_index++)
        {
            // Store the current solution to the PDE system at this node
            for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
            {
                double current_soln_this_pde_this_node = soln_repl[PROBLEM_DIM*node_index + pde_index];

                current_soln_this_node[pde_index] = current_soln_this_pde_this_node;
            }

            // Pass it into the ODE system at this node, of full state space dimensions
            mOdeSystemsAtNodes[node_index]->SetPdeSolution(current_soln_this_node);

            // Solve ODE system at this node
            mpOdeSolvers[node_index]->SolveAndUpdateStateVariable(mOdeSystemsAtNodes[node_index], time, next_time, dt);

        }
    }
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem( - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetOutputDirectory(std::string outputDirectory, bool clearDirectory)
{
    mClearOutputDirectory = clearDirectory;
    this->mOutputDirectory = outputDirectory;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetSamplingTimeStep(double samplingTimeStep)
{
    assert(samplingTimeStep >= this->mIdealTimeStep);
    mSamplingTimeStep = samplingTimeStep;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile()
{
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile( - start"<<std::endl;
    // A number of methods must have been called prior to this method
    if (this->mOutputDirectory == "")
    {
        EXCEPTION("SetOutputDirectory() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mTimesSet == false)
    {
        EXCEPTION("SetTimes() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mIdealTimeStep <= 0.0)
    {
        EXCEPTION("SetTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (mSamplingTimeStep == DOUBLE_UNSET)
    {
        EXCEPTION("SetSamplingTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (!this->mInitialCondition)
    {
        EXCEPTION("SetInitialCondition() must be called prior to SolveAndWriteResultsToFile()");
    }

//#ifdef CHASTE_VTK
    ResetInterpolatedQuantitiesOnNewTimeStep();
    // Create a .pvd output file
    //std::cout<<"outputfilehandeler: "<<this->mOutputDirectory<<std::endl;

    OutputFileHandler output_file_handler(this->mOutputDirectory, mClearOutputDirectory);

    mpVtkMetaFile = output_file_handler.OpenOutputFile("results.pvd");
    *mpVtkMetaFile << "<?xml version=\"1.0\"?>\n";
    *mpVtkMetaFile << "<VTKFile type=\"Collection\" version=\"0.1\" byte_order=\"LittleEndian\" compressor=\"vtkZLibDataCompressor\">\n";
    *mpVtkMetaFile << "    <Collection>\n";

    std::cout<<"Get output loc: "<<output_file_handler.GetChasteTestOutputDirectory()<<std::endl;
    std::cout<<"Get output dir full path: "<<output_file_handler.GetOutputDirectoryFullPath()<<std::endl;
    std::cout<<"Get relative path: "<<output_file_handler.GetRelativePath()<<std::endl;

    // Write initial condition to VTK
    Vec initial_condition = this->mInitialCondition;

    ReplicatableVector result_repl(this->mInitialCondition);

    WriteVtkResultsToFile(initial_condition, 0);

    // The helper class TimeStepper deals with issues such as small final timesteps so we don't have to
    TimeStepper stepper(this->mTstart, this->mTend, mSamplingTimeStep);

    std::vector<std::string> p_pde_stateVariableNames = mpPdeSystem -> GetStateVariableRegister() ->GetStateVariableRegisterVector();

    // make a vector of file streams to store slice values per pde dim
    std::vector<std::shared_ptr<ofstream> > sliceMassFiles;
    for(unsigned i=0; i<PROBLEM_DIM; i++)
    {
    //    std::ofstream sliceMassFile;
     //   std::string sliceMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sliceMass_"+p_pde_stateVariableNames[i]+".csv";
     //   std::cout<<"sliceMassFilename: "<<sliceMassFilename<<std::endl;
     //   sliceMassFile.open(sliceMassFilename);
     //   sliceMassFiles.push_back(sliceMassFile);
        std::shared_ptr<std::ofstream> sliceMassFile(new std::ofstream);
        std::string sliceMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sliceMass_"+p_pde_stateVariableNames[i]+".csv";
        std::cout<<"sliceMassFilename: "<<sliceMassFilename<<std::endl;
        sliceMassFile -> open(sliceMassFilename);
        sliceMassFiles.push_back(sliceMassFile);
    }

    std::ofstream slicePositionFile;
    std::string slicePositionFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"slicePosition.csv";
    std::cout<<"slicePositionFilename: "<<slicePositionFilename<<std::endl;
    slicePositionFile.open (slicePositionFilename);

    std::ofstream sumCoeffFile;
    std::string sumCoeffFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sumCoeff.csv";
    std::cout<<"sumCoeffFilename: "<<sumCoeffFilename<<std::endl;
    sumCoeffFile.open (sumCoeffFilename);

    for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double sumCoeff=0;
        for(unsigned node_index=0;node_index<mpMesh->GetNumNodes();node_index++)
        {
            sumCoeff += result_repl[PROBLEM_DIM*node_index + pde_index];
        }
        sumCoeffFile << sumCoeff<<",";
    }
    sumCoeffFile <<"\n";

    std::ofstream totalMassFile;
    std::string totalMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"totalMass.csv";
    std::cout<<"totalMassFilename: "<<totalMassFilename<<std::endl;
    totalMassFile.open (totalMassFilename);

    std::ofstream totalMassOldFile;
    std::string totalMassOldFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"totalMassOld.csv";
    std::cout<<"totalMassFilename: "<<totalMassOldFilename<<std::endl;
    totalMassOldFile.open (totalMassOldFilename);

    std::ofstream timeFile;
    std::string timeFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"time.csv";
    std::cout<<"timeFilename: "<<timeFilename<<std::endl;
    timeFile.open (timeFilename);

    if(mCalculateTotalMass)
    {
        for(unsigned pdeNum=1; pdeNum<PROBLEM_DIM;pdeNum++)
        {
            mTotalMass.push_back(0.0);
            mOldTotalMass.push_back(0.0);
        }
    }

    // Main time loop, interate the stepper
    while (!stepper.IsTimeAtEnd())
    {
        // Reset start and end times
        this->SetTimes(stepper.GetTime(), stepper.GetNextTime());

        timeFile << stepper.GetTime()<<"\n";

        ResetInterpolatedQuantitiesOnNewTimeStep();

        // Solve the system up to the new end time
        Vec soln = this->Solve();
        ReplicatableVector result_repl(soln);

        // Reset the initial condition for the next timestep
        if (this->mInitialCondition != initial_condition)
        {
            PetscTools::Destroy(this->mInitialCondition);
        }
        this->mInitialCondition = soln;

        // Move forward in time
        stepper.AdvanceOneTimeStep();

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
        {
            double sumCoeff=0;
            for(unsigned node_index=0;node_index<mpMesh->GetNumNodes();node_index++)
            {
                sumCoeff += result_repl[PROBLEM_DIM*node_index + pde_index];
            }
            sumCoeffFile << sumCoeff<<",";
        }
        sumCoeffFile << "\n";

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM;pde_index++)
        {
           totalMassFile << mTotalMass[pde_index]<<",";
        }
        totalMassFile <<"\n";

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM;pde_index++)
        {
           totalMassOldFile << mOldTotalMass[pde_index]<<",";
        }
        totalMassOldFile <<"\n";

        // for a given timestep output the slice data
        for(unsigned pos=0; pos<mSlicePositions.size(); pos++)
        {
            // take the x value of the position
            slicePositionFile << mSlicePositions[pos][0]<<",";
            for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
            {
                *sliceMassFiles[pde_index] << mSliceMass[pos][pde_index]<<",";
            }
        }
        // set the new line for the next time trace
        slicePositionFile << "\n";
        for(unsigned i=0; i<PROBLEM_DIM; i++)
        {
            *sliceMassFiles[i] << "\n";
        }

        // Write solution to VTK
        WriteVtkResultsToFile(soln, stepper.GetTotalTimeStepsTaken());
    }

    // close all files
    timeFile.close();
    sumCoeffFile.close();
    totalMassFile.close();
    totalMassOldFile.close();
    slicePositionFile.close();
    for(unsigned i=0; i<PROBLEM_DIM; i++)
    {
        sliceMassFiles[i]->close();
    }

    // Restore saved initial condition to avoid user confusion!
    if (this->mInitialCondition != initial_condition)
    {
        PetscTools::Destroy(this->mInitialCondition);
    }
    this->mInitialCondition = initial_condition;

    // Close .pvd output file
    *mpVtkMetaFile << "    </Collection>\n";
    *mpVtkMetaFile << "</VTKFile>\n";
    mpVtkMetaFile->close();
//#else //CHASTE_VTK
// LCOV_EXCL_START // We only test this in weekly builds
//    WARNING("VTK is not installed and is required for this functionality");
// LCOV_EXCL_STOP
//#endif //CHASTE_VTK
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile( - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed)
{

#ifdef CHASTE_VTK

    // Create a new VTK file for this time step
    std::stringstream time;
    time << numTimeStepsElapsed;
    //std::cout<<this->mOutputDirectory<<std::endl;
    VtkMeshWriter<ELEMENT_DIM, SPACE_DIM> mesh_writer(this->mOutputDirectory, "results_"+time.str(), false);
    // need to ensure StateVariableRegister is defined

    std::vector<std::string> p_pde_stateVariableNames = mpPdeSystem -> GetStateVariableRegister() ->GetStateVariableRegisterVector();

    /*
     * We first loop over PDEs. For each PDE we store the solution
     * at each node in a vector, then pass this vector to the mesh
     * writer.
     */
    ReplicatableVector solution_repl(solution);

    unsigned num_nodes = mpMesh->GetNumNodes();
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        // Store the solution of this PDE at each node
        std::vector<double> pde_index_data;
        pde_index_data.resize(num_nodes, 0.0);
        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            pde_index_data[node_index] = solution_repl[PROBLEM_DIM*node_index + pde_index];
        }

        // Add this data to the mesh writer
        std::stringstream data_name;
        data_name << "PDE variable " << p_pde_stateVariableNames[pde_index];
        mesh_writer.AddPointData(data_name.str(), pde_index_data);
    }

    if (mOdeSystemsPresent)
    {
        /*
         * We cannot loop over ODEs like PDEs, since the solutions are not
         * stored in one place. Therefore we build up a large 'vector of
         * vectors', then pass each component of this vector to the mesh
         * writer.
         */

        std::vector<std::vector<double> > ode_data;
        unsigned num_state_vars = mpPdeSystem -> GetStateVariableRegister() ->GetNumberOfStateVariables();
        ode_data.resize(num_state_vars);
        for (unsigned state_var_index=0; state_var_index<num_state_vars; state_var_index++)
        {
            ode_data[state_var_index].resize(num_nodes, 0.0);
        }

        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            std::vector<double> all_odes_this_node = mOdeSystemsAtNodes[node_index]->rGetStateVariables();
            // this could be of variable size, is of only the states that the ode modifies

            std::vector<std::string> ode_var_names = mOdeSystemsAtNodes[node_index]->GetStateVariableRegister() -> GetStateVariableRegisterVector();
            for (unsigned ode_index=0; ode_index<ode_var_names.size(); ode_index++)
            {
                // for each state variable in the ode system, find and update the corresponding variable in the pde system
                ode_data[mpPdeSystem -> GetStateVariableRegister() -> RetrieveStateVariableIndex(ode_var_names[ode_index])][node_index] = all_odes_this_node[ode_index];

            }

        }

        for (unsigned ode_index=0; ode_index<num_state_vars; ode_index++)
        {
            // ode_index is the state variable
            std::vector<double> ode_index_data = ode_data[ode_index];

            // Add this data to the mesh writer
            std::stringstream data_name;
            data_name << "ODE variable " << p_pde_stateVariableNames[ode_index];
            mesh_writer.AddPointData(data_name.str(), ode_index_data);
        }
    }

    mesh_writer.WriteFilesUsingMesh(*mpMesh);
    *mpVtkMetaFile << "        <DataSet timestep=\"";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << "\" group=\"\" part=\"0\" file=\"results_";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << ".vtu\"/>\n";
#endif // CHASTE_VTK

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
AbstractOdeSystemForCoupledPdeSystem* InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::GetOdeSystemAtNode(unsigned index)
{
    return mOdeSystemsAtNodes[index];
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ResetInterpolatedQuantitiesOnNewTimeStep()
{

    if(mCalculateTotalMass)
    {
        // reset previous timstep value of total mass
        std::fill(mTotalMass.begin(), mTotalMass.end(), 0.0);

        std::fill(mOldTotalMass.begin(), mOldTotalMass.end(), 0.0);
    }

    if(mCalculateSliceMass)
    {
        mSliceMass = std::vector<std::vector<double>> ();

        mSlicePositions = std::vector<std::vector<double>> ();

    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::OutputInterpolatedQuantitiesOnTimestep()
{

}

#endif


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

/// @file
/// Implements full BTE calculations defined in beyondRTA.hpp.

#include <iostream>
#include <map>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <utilities.hpp>
#include <beyondRTA.hpp>

namespace alma {
namespace beyondRTA {
Eigen::MatrixXd calc_kappa(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    const std::vector<alma::Threeph_process>& threeph_procs,
    const std::vector<alma::Twoph_process>& twoph_procs,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    double T,
    bool iterative,
    boost::mpi::communicator& comm) {
    typedef std::array<int, 3> idx_triplet;
    typedef std::array<int, 2> idx_pair;

    // GATHER INFORMATION FOR FULL BTE SYSTEM AND DECLARE SOME VARIABLES

    int Ngridpoints = grid.nqpoints;

    // determine number of irreducible q-points
    int Nclasses = grid.get_nequivalences();

    int Nbranches = grid.get_spectrum_at_q(0).omega.size();
    int Ntot = Nclasses * Nbranches;

    // Unknowns in the linear system
    // (factor 3: each H unknown is a vector with x,y,z components)
    Eigen::VectorXd H(3 * Ntot);

    // stores heat capacities of the irreducible points
    Eigen::VectorXd C(Ntot);
    C.setConstant(0.0);

    // stores relaxation times of the irreducible points
    Eigen::VectorXd tau(Ntot);
    tau.setConstant(0.0);

    // stores phonon frequencies of the irreducible points
    Eigen::VectorXd omega(Ntot);
    omega.setConstant(0.0);

    // stores group velocity vectors of the irreducible points
    Eigen::MatrixXd vg(Ntot, 3);
    vg.fill(0.0);

    // table that maps a single-index value to the corresponding unknown
    std::map<int, int> idx_to_unknown;

    // table that maps an unknown to the single-index
    std::vector<int> unknown_to_idx;
    unknown_to_idx.resize(Ntot);

    // Gamma coefficients for 3-phonon processes
    std::map<idx_triplet, double> Gamma_plus;
    std::map<idx_triplet, double> Gamma_minus;

    // Gamma coefficients for 2-phonon processes
    std::map<idx_pair, double> Gamma2;

    // GATHER PHONON PROPERTIES FOR IRREDUCIBLE Q-POINTS

    int unknownCounter = 0;
    double C_factor = 1e27 * alma::constants::kB / Ngridpoints / poscar.V;

    // scan over all equivalence classes in the grid

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        int nqbase = grid.get_representative(nclass);
        auto spectrum = grid.get_spectrum_at_q(nqbase);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            // register this phonon mode in the mapping tables
            int single_idx = nbranch * Ngridpoints + nqbase;
            idx_to_unknown[single_idx] = unknownCounter;
            unknown_to_idx[unknownCounter] = single_idx;

            // store heat capacity
            C(unknownCounter) = C_factor * alma::bose_einstein_kernel(
                                               spectrum.omega[nbranch], T);

            // store relaxation time
            double mytau =
                (w0(nbranch, nqbase) == 0.) ? 0. : (1. / w0(nbranch, nqbase));
            tau(unknownCounter) = mytau;

            // store phonon frequency
            omega(unknownCounter) = spectrum.omega(nbranch);

            // store group velocity vector
            Eigen::Vector3d myvg = spectrum.vg.col(nbranch);
            vg(unknownCounter, 0) = myvg(0);
            vg(unknownCounter, 1) = myvg(1);
            vg(unknownCounter, 2) = myvg(2);

            // update unkownCounter
            unknownCounter++;
        }
    }

    // CALCULATE NON-RTA CONDUCTIVITY BY SOLVING LINEAR SYSTEM

    // build the list of Gamma values for 2-phonon processes

    for (std::size_t nproc = 0; nproc < twoph_procs.size(); nproc++) {
        // retrieve Gamma value
        alma::Twoph_process myprocess = twoph_procs.at(nproc);
        double Gamma = myprocess.compute_gamma(poscar, grid);

        // obtain single indices
        int idx1 = myprocess.alpha[0] * Ngridpoints + myprocess.q[0];
        int idx2 = myprocess.alpha[1] * Ngridpoints + myprocess.q[1];

        // store Gamma value in the map
        idx_pair key({{idx1, idx2}});
        Gamma2[key] = Gamma;
    } // done scanning over all 2-phonon processes

    // build the list of Gamma values for 3-phonon processes

    for (std::size_t nproc = 0; nproc < threeph_procs.size(); nproc++) {
        alma::Threeph_process myprocess = threeph_procs.at(nproc);
        double Gamma = myprocess.compute_gamma(grid, T);
        bool gammaplus = (myprocess.type == alma::threeph_type::absorption);

        // store Gamma value

        int idx1 = myprocess.alpha[0] * Ngridpoints + myprocess.q[0];
        int idx2 = myprocess.alpha[1] * Ngridpoints + myprocess.q[1];
        int idx3 = myprocess.alpha[2] * Ngridpoints + myprocess.q[2];

        idx_triplet key({{idx1, idx2, idx3}});

        if (gammaplus) {
            Gamma_plus[key] = Gamma;
        }
        else {
            Gamma_minus[key] = Gamma;
        }
    } // done scanning over all 3-phonon processes

    // build system of equations "A*H = B"

    Eigen::MatrixXd A(3 * Ntot, 3 * Ntot);
    A.fill(0.0);

    Eigen::VectorXd B(3 * Ntot);

    // x-components of omega*MFP_RTA
    B.segment(0, Ntot) = omega.array() * tau.array() * vg.col(0).array();
    // y-components of omega*MFP_RTA
    B.segment(Ntot, Ntot) = omega.array() * tau.array() * vg.col(1).array();
    // z-components of omega*MFP_RTA
    B.segment(2 * Ntot, Ntot) = omega.array() * tau.array() * vg.col(2).array();

    // contributions from 3-phonon absorption processes
    for (auto& it3 : Gamma_plus) {
        idx_triplet key = it3.first;
        double Gamma = it3.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // PROCESS CHILD POINT 1

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0; //

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) +=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }

        // PROCESS CHILD POINT 2

        // obtain unknownIndex for the parent of the non-irreducible point
        child_nq = key[2] % Ngridpoints;
        mybranch = key[2] / Ngridpoints;
        parent_nq = grid.getParentIdx(child_nq);
        colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        symmID = grid.getSymIdxToParent(child_nq);
        symmop_idx = symmID / 2;
        // account for time reversal if needed
        sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // contributions from 3-phonon emission processes

    for (auto& it3 : Gamma_minus) {
        idx_triplet key = it3.first;
        double Gamma = it3.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // PROCESS CHILD POINT 1

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    0.5 * R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }

        // PROCESS CHILD POINT 2

        // obtain unknownIndex for the parent of the non-irreducible point
        child_nq = key[2] % Ngridpoints;
        mybranch = key[2] / Ngridpoints;
        parent_nq = grid.getParentIdx(child_nq);
        colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        symmID = grid.getSymIdxToParent(child_nq);
        symmop_idx = symmID / 2;
        // account for time reversal if needed
        sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    0.5 * R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // Share the contributions from 3-phonon processes among cores and
    // add them together.
    if (comm.size() > 1) {
        Eigen::MatrixXd my_A(A);
        A.fill(0.);
        boost::mpi::all_reduce(
            comm, my_A.data(), my_A.size(), A.data(), std::plus<double>());
    }

    // contributions from 2-phonon processes
    for (auto& it2 : Gamma2) {
        idx_pair key = it2.first;
        double Gamma = it2.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer (being its transpose)
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // diagonal elements in the system
    for (int diag_idx = 0; diag_idx < 3 * Ntot; diag_idx++) {
        A(diag_idx, diag_idx) += 1.0;
    }

    // Normalise each row for better numerical stability

    for (int nrow = 0; nrow < 3 * Ntot; nrow++) {
        double scale = A.row(nrow).array().abs().maxCoeff();
        A.row(nrow) /= scale;
        B(nrow) /= scale;
    }

    // SOLVE SYSTEM

    if (iterative) {
        Eigen::BiCGSTAB<Eigen::MatrixXd> solver;
        solver.compute(A);

        // RTA solution to be used as initial guess
        Eigen::VectorXd H_guess(3 * Ntot);

        H_guess.segment(0, Ntot) =
            omega.array() * tau.array() * vg.col(0).array();
        H_guess.segment(Ntot, Ntot) =
            omega.array() * tau.array() * vg.col(1).array();
        H_guess.segment(2 * Ntot, Ntot) =
            omega.array() * tau.array() * vg.col(2).array();

        H = solver.solveWithGuess(B, H_guess);
    }

    else {
        H = A.partialPivLu().solve(B);
    }

    double rel_err = (A * H - B).norm() / B.norm();

    if (rel_err > 1e-3) {
        std::cout << "alma::beyondRTA::calc_kappa > WARNING:" << std::endl;
        std::cout << "solution of linear system might be unstable."
                  << std::endl;
        std::cout << "Relative error metric = " << rel_err << std::endl;
    }

    // PROCESS THE LINEAR SYSTEM SOLUTION

    // construct "generalised MFPs"
    Eigen::MatrixXd MFP_nonRTA(Ntot, 3);
    MFP_nonRTA.col(0) = H.segment(0, Ntot).array() / omega.array();
    MFP_nonRTA.col(1) = H.segment(Ntot, Ntot).array() / omega.array();
    MFP_nonRTA.col(2) = H.segment(2 * Ntot, Ntot).array() / omega.array();

    // fix potential NaN/Inf problems
    for (int n = 0; n < Ntot; n++) {
        if (omega(n) <= 0.0) {
            MFP_nonRTA(n, 0) = 0.0;
            MFP_nonRTA(n, 1) = 0.0;
            MFP_nonRTA(n, 2) = 0.0;
        }
    }

    // symmetrise obtained MFP_nonRTA

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        // obtain the q-index of the class representative
        std::size_t nq_parent = grid.get_representative(nclass);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int single_idx = nbranch * Ngridpoints + nq_parent;
            int unknownIndex = idx_to_unknown[single_idx];

            Eigen::Vector3d MFP_parent =
                MFP_nonRTA.row(unknownIndex).transpose();
            Eigen::Vector3d MFP_buffer =
                grid.copy_symmetry(nq_parent, syms, MFP_parent);

            MFP_nonRTA.row(unknownIndex) = MFP_buffer.transpose();
        }
    }

    // fix potential NaN/Inf problems
    for (int n = 0; n < Ntot; n++) {
        if (omega(n) <= 0.0) {
            MFP_nonRTA(n, 0) = 0.0;
            MFP_nonRTA(n, 1) = 0.0;
            MFP_nonRTA(n, 2) = 0.0;
        }
    }

    // CALCULATE NON-RTA CONDUCTIVITY

    Eigen::Matrix3d kappatensor;
    kappatensor.fill(0.0);

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        // obtain the q-index of the class representative
        std::size_t nq_parent = grid.get_representative(nclass);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int single_idx = nbranch * Ngridpoints + nq_parent;
            int unknownIndex = idx_to_unknown[single_idx];

            Eigen::Vector3d MFP_parent =
                MFP_nonRTA.row(unknownIndex).transpose();

            for (auto nq_member : grid.get_equivalence(nclass)) {
                // obtain the group velocity vector of the class member
                Eigen::Vector3d vg_member =
                    (grid.get_spectrum_at_q(nq_member)).vg.col(nbranch);

                // obtain the rotation matrix that maps the class member
                // to its irreducible parent
                std::size_t symmID = grid.getSymIdxToParent(nq_member);
                std::size_t symmop_idx = symmID / 2;
                double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

                Eigen::Matrix3d ROT_buffer;
                ROT_buffer.col(0) = syms.rotate_v(
                    Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
                ROT_buffer.col(1) = syms.rotate_v(
                    Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
                ROT_buffer.col(2) = syms.rotate_v(
                    Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

                // obtain the rotation matrix R for which v_child = R*v_parent.
                // This is the inverse of ROT_buffer, being its transpose,
                // corrected for sign.
                Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

                // calculate the non-RTA MFP vector of this mode
                Eigen::Vector3d MFP_member = R * MFP_parent;

                // obtain conductivity contribution
                Eigen::Matrix3d outer =
                    (1e-9 * MFP_member) * (1e3 * vg_member.transpose());
                kappatensor += C(unknownIndex) * outer;
            }
        }
    }

    // symmetrise kappa tensor

    Eigen::Matrix3d kappa_accumulated;
    kappa_accumulated.fill(0.0);

    for (std::size_t nsymm = 0; nsymm < syms.get_nsym(); nsymm++) {
        kappa_accumulated += syms.rotate_m<double>(kappatensor, nsymm, true);
    }

    kappatensor = kappa_accumulated / static_cast<double>(syms.get_nsym());

    // RETURN RESULT

    return kappatensor;
} // end of calc_kappa
} // end of namespace beyondRTA
} // end of namespace alma


#ifndef H_trex_europa_interv_patch
# define H_trex_europa_interv_patch

#include <boost/numeric/interval.hpp>

namespace TREX {
  namespace europa {

    inline bool
    intersect(EUROPA::Domain& dom, EUROPA::edouble::basis_type lb, EUROPA::edouble::basis_type ub,
	      EUROPA::edouble::basis_type decimal_places)
    {
      EUROPA::edouble const p_inf = std::numeric_limits<EUROPA::edouble>::infinity();
      EUROPA::edouble const n_inf = std::numeric_limits<EUROPA::edouble>::minus_infinity();
      EUROPA::edouble const prec = decimal_places;

      if( lb>ub || lb>=p_inf || ub<=n_inf ) {
	dom.empty();
	return true;
      } else {
	EUROPA::edouble lo = n_inf, hi = p_inf;

	// std::cerr<<dom.toString()<<" * ["<<lb<<", "<<ub<<"]"<<std::flush;

	if( lb>lo ) {
	  lo = lb;
	  if( lo > dom.getUpperBound() )
	    lo -= prec;
	}
	if( ub<hi ) {
	  hi = ub;
	  if( hi < dom.getLowerBound() )
	    hi += prec;
	}
	bool ret = dom.intersect(lo, hi);

	// if( dom.isEmpty() )
	// 	std::cerr<<" EMPTY"<<std::endl;
	// else
	// 	std::cerr<<" = "<<dom.toString()<<std::endl;

	return ret;
      }
    }

    inline bool intersect(EUROPA::Domain &dom, EUROPA::edouble lb, EUROPA::edouble ub,
		   EUROPA::edouble::basis_type decimal_places) {

      EUROPA::edouble const prec = decimal_places;

      if( lb>dom.getUpperBound() &&
	  lb < std::numeric_limits<EUROPA::edouble>::infinity() )
	lb -= prec;
      if( ub<dom.getLowerBound() &&
	  ub > std::numeric_limits<EUROPA::edouble>::minus_infinity() )
	ub += prec;

      // std::cerr<<dom.toString()<<" * ["<<lb<<", "<<ub<<"]"<<std::endl;
      return dom.intersect(lb, ub);
    }

    /*
     * A replacement to boost::rounded_arith_opp as it has a bug under clang :
     *   - the to_int call made in the boost version needed to be replaced by
     *     Rounding::to_int to allow the compiler know that this static method
     *     is coming from the base class
     */
    template<class T, class Rounding>
    struct my_rounded_arith_opp: Rounding {
      void init() { this->upward(); }
# define BOOST_DN(EXPR)				\
      this->downward();				\
      T r = this->force_rounding(EXPR);		\
      this->upward();				\
      return r
# define BOOST_NR(EXPR)				\
      this->to_nearest();			\
      T r = this->force_rounding(EXPR);		\
      this->upward();				\
      return r
# define BOOST_UP(EXPR) return this->force_rounding(EXPR)
# define BOOST_UP_NEG(EXPR) return -this->force_rounding(EXPR)
      template<class U> T conv_down(U const &v) { BOOST_UP_NEG(-v); }
      template<class U> T conv_up  (U const &v) { BOOST_UP(v); }
      T add_down(const T& x, const T& y) { BOOST_UP_NEG((-x) - y); }
      T sub_down(const T& x, const T& y) { BOOST_UP_NEG(y - x); }
      T mul_down(const T& x, const T& y) { BOOST_UP_NEG(x * (-y)); }
      T div_down(const T& x, const T& y) { BOOST_UP_NEG(x / (-y)); }
      T add_up  (const T& x, const T& y) { BOOST_UP(x + y); }
      T sub_up  (const T& x, const T& y) { BOOST_UP(x - y); }
      T mul_up  (const T& x, const T& y) { BOOST_UP(x * y); }
      T div_up  (const T& x, const T& y) { BOOST_UP(x / y); }
      T median  (const T& x, const T& y) { BOOST_NR((x + y) / 2); }
      T sqrt_down(const T& x)
      { BOOST_NUMERIC_INTERVAL_using_math(sqrt); BOOST_DN(sqrt(x)); }
      T sqrt_up  (const T& x)
      { BOOST_NUMERIC_INTERVAL_using_math(sqrt); BOOST_UP(sqrt(x)); }
      T int_down(const T& x) { return -Rounding::to_int(-x); }
      T int_up  (const T& x) { return Rounding::to_int(x); }
# undef BOOST_DN
# undef BOOST_NR
# undef BOOST_UP
# undef BOOST_UP_NEG
    };

  }
}

#endif


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library 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.1 of the License, or (at your option) any later version.
*
*  This library 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 library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "../data/Matrix.hpp"
#include "../core/Assert.hpp"
#include "../core/Logger.hpp"

#undef BOOST_UBLAS_TYPE_CHECK
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/algorithms/within.hpp>

typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint> BoostPolygon;
typedef boost::geometry::model::ring<BoostPoint> BoostRing;
typedef boost::geometry::model::multi_polygon<BoostPolygon> BoostMultiPolygon;

#include <polypartition/polypartition.h>

#include <list>

// remove_spikes
// adapted from https://github.com/boostorg/geometry/commits/develop/include/boost/geometry/algorithms/remove_spikes.hpp eb3260708eb241d8da337f4be73b41d69d33cd09

/*
Remove spikes from a ring/polygon.
Ring (having 8 vertices, including closing vertex)
+------+
| |
| +--+
| | ^this "spike" is removed, can be located outside/inside the ring
+------+
(the actual determination if it is removed is done by a strategy)

*/

namespace openstudio {

  template <typename Point1, typename Point2, typename Point3>
  static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b)
  {
    // adapted from boost\geometry\algorithms\detail\point_is_spike_or_equal.hpp to include tolerance checking

    // segment_a is at the beginning
    // segment_b is in the middle
    // last_point is at the end

    // segment_b is being considered for deletion

    double normTol = 0.001; // 1 mm
    double tol = 0.001; // relative to 1

    double diff1_x = last_point.x()-segment_b.x();
    double diff1_y = last_point.y()-segment_b.y();
    double norm1 = sqrt(pow(diff1_x, 2) + pow(diff1_y, 2));
    if (norm1 > normTol){
      diff1_x = diff1_x/norm1;
      diff1_y = diff1_y/norm1;
    }else{
      // last point is too close to segment b
      return true;
    }

    double diff2_x = segment_b.x()-segment_a.x();
    double diff2_y = segment_b.y()-segment_a.y();
    double norm2 = sqrt(pow(diff2_x, 2) + pow(diff2_y, 2));
    if (norm2 > normTol){
      diff2_x = diff2_x/norm2;
      diff2_y = diff2_y/norm2;
    }else{
      // segment b is too close to segment a
      return true;
    }

    double crossProduct = diff1_x*diff2_y-diff1_y*diff2_x;
    if (abs(crossProduct) < tol){
      double dotProduct = diff1_x*diff2_x+diff1_y*diff2_y;
      if (dotProduct <= -1.0 + tol){
        // reversal
        return true;
      }
    }

    return false;
  }

}

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace remove_spikes
{

template <typename Range>
struct range_remove_spikes
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Range>::type
    >::type side_strategy;

    typedef typename coordinate_type<Range>::type coordinate_type;
    typedef typename point_type<Range>::type point_type;

    static inline void apply(Range& range)
    {
        std::size_t n = boost::size(range);
        std::size_t const min_num_points = core_detail::closure::minimum_ring_size
            <
                geometry::closure<Range>::value
            >::value;
        if (n < min_num_points)
        {
            return;
        }

        std::deque<point_type> cleaned;
        for (typename boost::range_iterator<Range const>::type it = boost::begin(range);
            it != boost::end(range); ++it)
        {
            // Add point
            cleaned.push_back(*it);

            while(cleaned.size() >= 3
                    && openstudio::point_is_spike_or_equal(cleaned.back(), *(cleaned.end() - 3), *(cleaned.end() - 2)))
            {
                // Remove pen-ultimate point causing the spike (or which was equal)
                cleaned.erase(cleaned.end() - 2);
            }
        }

        // For a closed-polygon, remove closing point, this makes checking first point(s) easier and consistent
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.pop_back();
        }

        bool found = false;
        do
        {
            found = false;
            // Check for spike in first point
            int const penultimate = 2;
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(cleaned.front(), *(cleaned.end() - penultimate), cleaned.back()))
            {
                cleaned.pop_back();
                found = true;
            }
            // Check for spike in second point
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(*(cleaned.begin() + 1), cleaned.back(), cleaned.front()))
            {
                cleaned.pop_front();
                found = true;
            }
        }
        while (found);

        // Close if necessary
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.push_back(cleaned.front());
        }

        // Copy output
        geometry::clear(range);
        std::copy(cleaned.begin(), cleaned.end(), std::back_inserter(range));
    }
};

template <typename Polygon>
struct polygon_remove_spikes
{
    static inline void apply(Polygon& polygon)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;

        typedef range_remove_spikes<ring_type> per_range;
        per_range::apply(exterior_ring(polygon));

        typename interior_return_type<Polygon>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_range::apply(*it);
        }
    }
};

}} // namespace detail::remove_spikes
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct remove_spikes
{
    static inline void apply(Geometry&)
    {}
};

template <typename Ring>
struct remove_spikes<Ring, ring_tag>
    : detail::remove_spikes::range_remove_spikes<Ring>
{};

template <typename Polygon>
struct remove_spikes<Polygon, polygon_tag>
    : detail::remove_spikes::polygon_remove_spikes<Polygon>
{};

} // namespace dispatch
#endif

namespace resolve_variant {

template <typename Geometry>
struct remove_spikes
{
    static void apply(Geometry& geometry)
    {
        concept::check<Geometry>();
        dispatch::remove_spikes<Geometry>::apply(geometry);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct remove_spikes<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    struct visitor: boost::static_visitor<void>
    {
        template <typename Geometry>
        void operator()(Geometry& geometry) const
        {
            remove_spikes<Geometry>::apply(geometry);
        }
    };

    static inline void apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>& geometry)
    {
        boost::apply_visitor(visitor(), geometry);
    }
};

} // namespace resolve_variant

/*!
\ingroup remove_spikes
\tparam Geometry geometry type
\param geometry the geometry to make remove_spikes
*/
template <typename Geometry>
inline void remove_spikes(Geometry& geometry)
{
    resolve_variant::remove_spikes<Geometry>::apply(geometry);
}

}} // namespace boost::geometry
// remove_spikes

namespace openstudio{

  // Private implementation functions

  BoostPolygon removeSpikes(const BoostPolygon& polygon)
  {
    BoostPolygon temp(polygon);
    boost::geometry::remove_spikes(temp);
    return temp;
  }

  std::vector<BoostPolygon> removeSpikes(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon& polygon : polygons){
      result.push_back(removeSpikes(polygon));
    }
    return result;
  }

  std::vector<BoostPolygon> removeHoles(const BoostPolygon& boostPolygon)
  {
    std::vector<BoostPolygon> result;

    // convert to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    BoostRing outer = boostPolygon.outer();
    TPPLPoly outerPoly; // must be counter-clockwise
    outerPoly.Init(outer.size() - 1);
    outerPoly.SetHole(false);
    //std::cout << "outer :";
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      outerPoly[i].x = outer[i].x();
      outerPoly[i].y = outer[i].y();
      //std::cout << "(" << outer[i].x() << ", " << outer[i].y() << ") ";
    }
    //std::cout << std::endl;
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    std::vector<BoostRing> inners = boostPolygon.inners();
    for (const BoostRing& inner : inners){
      TPPLPoly innerPoly; // must be clockwise
      innerPoly.Init(inner.size() - 1);
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < inner.size() - 1; ++i){
        innerPoly[i].x = inner[i].x();
        innerPoly[i].y = inner[i].y();
        //std::cout << "(" << inner[i].x() << ", " << inner[i].y() << ") ";
      }
      //std::cout << std::endl;
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.ConvexPartition_HM(&polys,&resultPolys);
    if (test == 0){
      LOG_FREE(Error, "utilities.geometry.removeHoles", "Failed to partition polygon");
      return result;
    }

    // convert back to BoostPolygon
    std::list<TPPLPoly>::iterator it, itend;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
      BoostPolygon newBoostPolygon;
      //std::cout << "result :";
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
        //std::cout << "(" << point.x << ", " << point.y << ") ";
      }
      TPPLPoint point = it->GetPoint(0);
      boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
      //std::cout << "(" << point.x << ", " << point.y << ") ";
      //std::cout << std::endl;

      boost::geometry::correct(newBoostPolygon);
      result.push_back(newBoostPolygon);
    }

    return result;
  }

  std::vector<BoostPolygon> removeHoles(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon polygon : polygons){
      if (polygon.inners().empty()){
        // DLM: might also want to partition if this polygon is self intersecting?
        result.push_back(polygon);
      }else{
        std::vector<BoostPolygon> temp = removeHoles(polygon);
        result.insert(result.end(), temp.begin(), temp.end());
      }
    }
    return result;
  }

  // convert a Point3d to a BoostPoint
  boost::tuple<double, double> boostPointFromPoint3d(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    OS_ASSERT(abs(point3d.z()) <= tol);

    // simple method
    //return boost::make_tuple(point3d.x(), point3d.y());

    // detailed method, try to combine points within tolerance
    Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

    return boost::make_tuple(resultPoint.x(), resultPoint.y());
  }

  // convert vertices to a boost polygon, all vertices must lie on z = 0 plane
  boost::optional<BoostPolygon> boostPolygonFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostPolygon polygon;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostPolygonFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(polygon, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(polygon, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(polygon);

    boost::optional<double> testArea = boost::geometry::area(polygon);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return polygon;
  }

  boost::optional<BoostPolygon> nonIntersectingBoostPolygonFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostPolygon> result = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostPolygonFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert vertices to a boost ring, all vertices must lie on z = 0 plane
  boost::optional<BoostRing> boostRingFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostRing ring;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostRingFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(ring, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(ring, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(ring);

    boost::optional<double> testArea = boost::geometry::area(ring);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return ring;
  }

  boost::optional<BoostRing> nonIntersectingBoostRingFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostRing> result = boostRingFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostRingFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert a boost polygon to vertices
  std::vector<Point3d> verticesFromBoostPolygon(const BoostPolygon& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    BoostRing outer = polygon.outer();
    if (outer.empty()){
      return result;
    }

    // add point for each vertex except final vertex
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      Point3d point3d(outer[i].x(), outer[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    OS_ASSERT(polygon.inners().empty());

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // convert a boost ring to vertices
  std::vector<Point3d> verticesFromBoostRing(const BoostRing& ring, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    // add point for each vertex except final vertex
    for (unsigned i = 0; i < ring.size() - 1; ++i){
      Point3d point3d(ring[i].x(), ring[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // struct used to sort polygons in descending order by area
  struct BoostPolygonAreaGreater{
    bool operator()(const BoostPolygon& left, const BoostPolygon& right){
      boost::optional<double> leftA = boost::geometry::area(left);
      if (!leftA){
        leftA = 0;
      }
      boost::optional<double> rightA = boost::geometry::area(right);
      if (!rightA){
        rightA = 0;
      }
      return (*leftA > *rightA);
    }
  };

  // Public functions

  IntersectionResult::IntersectionResult(const std::vector<Point3d>& polygon1,
                                         const std::vector<Point3d>& polygon2,
                                         const std::vector< std::vector<Point3d> >& newPolygons1,
                                         const std::vector< std::vector<Point3d> >& newPolygons2)
    : m_polygon1(polygon1), m_polygon2(polygon2), m_newPolygons1(newPolygons1), m_newPolygons2(newPolygons2)
  {
  }

  std::vector<Point3d> IntersectionResult::polygon1() const
  {
    return m_polygon1;
  }

  std::vector<Point3d> IntersectionResult::polygon2() const
  {
    return m_polygon2;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons1() const
  {
    return m_newPolygons1;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons2() const
  {
    return m_newPolygons2;
  }

  std::vector<Point3d> removeSpikes(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> boostPolygon = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return std::vector<Point3d>();
    }

    BoostPolygon boostResult = removeSpikes(*boostPolygon);

    std::vector<Point3d> result = verticesFromBoostPolygon(boostResult, allPoints, tol);

    return result;
  }

  bool pointInPolygon(const Point3d& point, const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon = nonIntersectingBoostRingFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return false;
    }

    if (abs(point.z()) > tol){
      return false;
    }

    boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
    BoostPoint boostPoint(p.get<0>(), p.get<1>());

    //boost::geometry::strategy::within::winding<BoostPoint> strategy;
    //boost::geometry::strategy::within::franklin<BoostPoint> strategy;
    //boost::geometry::strategy::within::crossings_multiply<BoostPoint> strategy;
    //bool result = boost::geometry::within(boostPoint, *boostPolygon, strategy);

    //bool result = boost::geometry::intersects(boostPoint, *boostPolygon);

    //bool result = boost::geometry::overlaps(boostPoint, *boostPolygon);

    double distance = boost::geometry::distance(boostPoint, *boostPolygon);
    bool result = (distance <= 0.0001);

    return result;

  }

  boost::optional<std::vector<Point3d> > join(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // union the points in face coordinates,
    std::vector<BoostPolygon> unionResult;
    try{
      boost::geometry::union_(*boostPolygon1, *boostPolygon2, unionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "overlay_invalid_input_exception");
      return boost::none;
    }

    unionResult = removeSpikes(unionResult);

    // should not be any holes, check for that below

    // check that union is ok
    if (unionResult.empty()){
      return boost::none;
    }else if (unionResult.size() > 1){
      return boost::none;
    }

    std::vector<Point3d> unionVertices = verticesFromBoostPolygon(unionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(unionResult[0]);
    if (!testArea || unionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.join", "Cannot compute area of union");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.join", "Union has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(unionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "Union is self intersecting");
      return boost::none;
    }

    // check for holes
    if (!unionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.join", "Union has inner loops");
      return boost::none;
    };

    unionVertices = reorderULC(unionVertices);
    unionVertices = removeCollinear(unionVertices);

    return unionVertices;
  }

  std::vector<std::vector<Point3d> > joinAll(const std::vector<std::vector<Point3d> >& polygons, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    unsigned N = polygons.size();
    if (N <= 1){
      return polygons;
    }

    // compute adjacency matrix
    Matrix A(N,N,0.0);
    for (unsigned i = 0; i < polygons.size(); ++i){
      A(i,i) = 1.0;
      for (unsigned j = i+1; j < polygons.size(); ++j){
        if (join(polygons[i], polygons[j], tol)){
          A(i,j) = 1.0;
          A(j,i) = 1.0;
        }
      }
    }

    std::vector<std::vector<unsigned> > connectedComponents = findConnectedComponents(A);
    for (const std::vector<unsigned>& component : connectedComponents){
      std::vector<Point3d> points;
      for (unsigned i : component){
        if (points.empty()){
          points = polygons[i];
        }else{
          boost::optional<std::vector<Point3d> > joined = join(points, polygons[i], tol);
          if (!joined){
            LOG_FREE(Error, "utilities.geometry.joinAll", "Expected polygons to join together");
          }else{
            points = *joined;
          }
        }
      }
      result.push_back(points);
    }

    return result;
  }

  boost::optional<IntersectionResult> intersect(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> resultPolygon1;
    std::vector<Point3d> resultPolygon2;
    std::vector< std::vector<Point3d> > newPolygons1;
    std::vector< std::vector<Point3d> > newPolygons2;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // intersect the points in face coordinates,
    std::vector<BoostPolygon> intersectionResult;
    try{
      boost::geometry::intersection(*boostPolygon1, *boostPolygon2, intersectionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "overlay_invalid_input_exception");
      return boost::none;
    }

    // check if intersection is empty
    if (intersectionResult.empty()){
      //LOG_FREE(Info, "utilities.geometry.intersect", "Intersection is empty");
      return boost::none;
    }

    intersectionResult = removeSpikes(intersectionResult);
    intersectionResult = removeHoles(intersectionResult);

    // check for multiple intersections
    if (intersectionResult.size() > 1){
      LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has " << intersectionResult.size() << " elements");
      std::sort(intersectionResult.begin(), intersectionResult.end(), BoostPolygonAreaGreater());
    }

    // check that largest intersection is ok
    std::vector<Point3d> intersectionVertices = verticesFromBoostPolygon(intersectionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(intersectionResult[0]);
    if (!testArea || intersectionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of largest intersection");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.intersect", "Largest intersection has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(intersectionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection is self intersecting");
      return boost::none;
    }
    if (!intersectionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection has inner loops");
      return boost::none;
    };

    // intersections are the same
    resultPolygon1 = intersectionVertices;
    resultPolygon2 = intersectionVertices;

    // create new polygon for each remaining intersection
    for (unsigned i = 1; i < intersectionResult.size(); ++i){

      std::vector<Point3d> newPolygon = verticesFromBoostPolygon(intersectionResult[i], allPoints, tol);

      testArea = boost::geometry::area(intersectionResult[i]);
      if (!testArea || newPolygon.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of intersection, result will not include this polygon, " << newPolygon);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(intersectionResult[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection is self intersecting, result will not include this polygon, " << newPolygon);
        continue;
      }
      if (!intersectionResult[i].inners().empty()){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection has inner loops, result will not include this polygon, " << newPolygon);
        continue;
      };

      newPolygons1.push_back(newPolygon);
      newPolygons2.push_back(newPolygon);
    }

    // polygon1 minus polygon2
    std::vector<BoostPolygon> differenceResult1;
    boost::geometry::difference(*boostPolygon1, *boostPolygon2, differenceResult1);
    differenceResult1 = removeSpikes(differenceResult1);
    differenceResult1 = removeHoles(differenceResult1);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult1.size(); ++i){

      std::vector<Point3d> newPolygon1 = verticesFromBoostPolygon(differenceResult1[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult1[i]);
      if (!testArea || newPolygon1.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon1);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon1);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult1[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon1);
        continue;
      }

      newPolygons1.push_back(newPolygon1);
    }

    // polygon2 minus polygon1
    std::vector<BoostPolygon> differenceResult2;
    boost::geometry::difference(*boostPolygon2, *boostPolygon1, differenceResult2);
    differenceResult2 = removeSpikes(differenceResult2);
    differenceResult2 = removeHoles(differenceResult2);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult2.size(); ++i){

      std::vector<Point3d> newPolygon2 = verticesFromBoostPolygon(differenceResult2[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult2[i]);
      if (!testArea || newPolygon2.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon2);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon2);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult2[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon2);
        continue;
      }

      newPolygons2.push_back(newPolygon2);
    }

    return IntersectionResult(resultPolygon1, resultPolygon2, newPolygons1, newPolygons2);
  }

  std::vector<std::vector<Point3d> > subtract(const std::vector<Point3d>& polygon, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> initialBoostPolygon = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!initialBoostPolygon){
      return result;
    }

    std::vector<BoostPolygon> boostPolygons;
    boostPolygons.push_back(*initialBoostPolygon);

    std::vector<BoostPolygon> newBoostPolygons;
    for (const std::vector<Point3d>& hole : holes){
      boost::optional<BoostPolygon> boostHole = nonIntersectingBoostPolygonFromVertices(hole, allPoints, tol);
      if (!boostHole){
        return result;
      }

      for (const BoostPolygon& boostPolygon : boostPolygons){
        std::vector<BoostPolygon> diffResult;
        boost::geometry::difference(boostPolygon, *boostHole, diffResult);
        diffResult = removeSpikes(diffResult);
        diffResult = removeHoles(diffResult);
        newBoostPolygons.insert(newBoostPolygons.end(), diffResult.begin(), diffResult.end());
      }
      boostPolygons.swap(newBoostPolygons);
    }

    for (const BoostPolygon& boostPolygon : boostPolygons){
      result.push_back(verticesFromBoostPolygon(boostPolygon, allPoints, tol));
    }

    return result;
  }

  bool selfIntersects(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!bp){
      return false;
    }
    return true;
  }

  bool intersects(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(polygon1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

    if (bp1 && bp2){
      return boost::geometry::intersects(*bp1, *bp2);
    }

    return false;
  }

  bool within(const Point3d& point1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> geometry1;
    geometry1.push_back(point1);
    return within(geometry1, polygon2, tol);
  }

  bool within(const std::vector<Point3d>& geometry1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    if (geometry1.size() == 1){
      if (geometry1[0].z() > tol){
        return false;
      }

      boost::tuple<double, double> p = boostPointFromPoint3d(geometry1[0], allPoints, tol);
      BoostPoint boostPoint(p.get<0>(), p.get<1>());

      boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

      if (bp2){
        return boost::geometry::within(boostPoint, *bp2);
      }

      return false;
    }

    /*
    // DLM: this is the better implementation, requires boost 1.57
    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(geometry1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp1 && bp2){
      return boost::geometry::within(*bp1, *bp2);
    }
    */

    // DLM: temp code
    if (geometry1.size() < 3){
      return false;
    }
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp2){
      for (const Point3d& point : geometry1)
      {
        boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
        BoostPoint boostPoint(p.get<0>(), p.get<1>());

        if (!boost::geometry::within(boostPoint, *bp2)){
          return false;
        }
      }
    } else{
      return false;
    }

    return true;
  }

} // openstudio